ReferenceFrameFactor

import gtsam
print([k for k in gtsam.__dict__.keys() if "ReferenceFrame" in k])

['ReferenceFrameFactorPoint3Pose3']

ReferenceFrameFactor<POINT, TRANSFORM> is a ternary factor used to relate landmark observations made in two different reference frames (e.g., from two different robots or two distinct SLAM sessions). It connects:

A landmark (POINT) expressed in a ‘global’ or common reference frame (globalKey).
A transform (TRANSFORM) representing the pose of a ‘local’ frame relative to the ‘global’ frame (transKey). Typically TRANSFORM = global_T_local.
The same landmark (POINT) expressed in the ‘local’ reference frame (localKey).

The factor enforces the constraint that the globally expressed landmark, when transformed by the global_T_local transform, should match the locally expressed landmark. The transformation logic depends on the specific POINT and TRANSFORM types and is handled by the transform_point helper function (which usually calls Transform::transformFrom).

Error: $\text{Log}(\text{local}^{-1} \cdot \text{transform\_point}(\text{trans}, \text{global}))$

This factor is crucial for multi-robot map merging or combining results from different SLAM sessions where common landmarks have been observed.

import gtsam
import numpy as np
from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph
# The Python wrapper creates specific instantiations
from gtsam import ReferenceFrameFactorPoint3Pose3
from gtsam.symbol_shorthand import L, T, O

Creating a ReferenceFrameFactor¶

Instantiate by providing the keys for the global landmark, the transform, the local landmark, and a noise model. The noise model dimensionality should match the dimension of the POINT type.

global_landmark_key = L(0)
transform_key = T(0)
local_landmark_key = O(0)

# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)
noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev

# Factor type includes Point and Transform types
factor = ReferenceFrameFactorPoint3Pose3(global_landmark_key,
                                         transform_key,
                                         local_landmark_key,
                                         noise)
factor.print("ReferenceFrameFactor: ")

ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)
isotropic dim=3 sigma=0.1

Evaluating the Error¶

The error represents how much the transformed global landmark deviates from the local landmark estimate.

values = Values()

# Example values
global_landmark = Point3(5.0, 2.0, 1.0)
global_T_local = Pose3(Rot3.Yaw(np.pi/2), Point3(1, 0, 0))
expected_local_landmark = global_T_local.transformTo(global_landmark)
print(f"Expected local landmark: {expected_local_landmark}")

values.insert(global_landmark_key, global_landmark)
values.insert(transform_key, global_T_local)
values.insert(local_landmark_key, expected_local_landmark)

error_gt = factor.error(values)
print(f"\nError at ground truth: {error_gt} (Should be zero)")

# Introduce error in the local landmark estimate
noisy_local_landmark = expected_local_landmark + Point3(0.1, -0.1, 0.05)
values.update(local_landmark_key, noisy_local_landmark)
error_noisy = factor.error(values)
print(f"Error with noisy local landmark: {error_noisy}")

Expected local landmark: [ 2. -4.  1.]

Error at ground truth: 4500.0 (Should be zero)
Error with noisy local landmark: 4621.125