KnownLandmarkFactor

This guide explains how to use KnownLandmarkFactor and KnownLandmarkFactor2 for localization against fixed landmarks. Prefer KnownLandmarkFactor for normal GTSAM use. It follows the conventional pose direction used throughout GTSAM. KnownLandmarkFactor2 accepts the same physical measurement in the opposite state direction and is intended specifically for exact QCQP and certifiable optimization.

Choose the pose convention first¶

We use the convention that $T^a_b$ transforms coordinates from frame $b$ into frame $a$. A conventional GTSAM pose $T^w_k$ maps frame $k$ into the world. Its inverse $T^k_w=(T^w_k)^{-1}$ maps world coordinates into frame $k$. A fixed landmark is $L^w$, and its Cartesian measurement in frame $k$ is $\widetilde{P}^k$.

KnownLandmarkFactor should be the default choice. It uses the standard $T^w_k$ convention and therefore combines naturally with conventional PriorFactor<Pose3>, BetweenFactor<Pose3>, initial values, plotting utilities, and other GTSAM APIs.

Use KnownLandmarkFactor2 only when building an exact QCQP or certifiable optimization in which the rest of the graph also optimizes $T^k_w$. The second variant is not a more accurate measurement model; its only advantage is the algebraic form required by those solvers. It also requires explicit inversion whenever data enter from, or results return to, conventional GTSAM code. Do not insert a $T^w_k$ value under a key expected to hold $T^k_w$, or vice versa.

Both C++ templates support Pose2 and Pose3. Python exposes KnownLandmarkFactorPose2, KnownLandmarkFactorPose3, KnownLandmarkFactor2Pose2, and KnownLandmarkFactor2Pose3.

Prepare one Cartesian landmark measurement¶

Each factor needs four inputs:

1. the pose key k;

2. a fixed world landmark wL;

3. the observed Cartesian point measured_kP; and

4. a noise model for errors expressed in frame $k$.

The residual is a Cartesian vector, so a Pose3 factor requires a three-dimensional noise model and a Pose2 factor requires a two-dimensional model. A full Gaussian model is appropriate when the observation has anisotropic or correlated uncertainty. In mathematical notation, the covariance below is $\Sigma_{P^k}$.

Conventional localization with KnownLandmarkFactor¶

For a conventional pose $T^w_k$, the predicted frame-$k$ point is

implemented by wTk.transformTo(wL). The factor residual is $\widehat{P}^k-\widetilde{P}^k$. Add the factor to an ordinary nonlinear graph and insert a conventional wTk initial value under the same key.

Build a well-constrained graph¶

One point observation does not determine all six Pose3 degrees of freedom. In a real localization graph, add factors for several geometrically distinct known landmarks and, where appropriate, pose priors or odometry factors. Each observation should carry its own measured_kP and noise model. Reusing the same pose key connects all observations made from that pose.

A known landmark is fixed factor data, not a variable in Values. If the landmark itself must be estimated, use a binary measurement factor such as BearingRangeFactor<Pose3, Point3> instead.

Inverse-state localization with KnownLandmarkFactor2¶

The inverse-state factor predicts the same measured point from $T^k_w$:

In code this is kTw.transformFrom(wL). Writing $T^k_w=[R^k_w,t^k_w]$, the prediction is

which is affine in the entries of the homogeneous matrix $T^k_w$. After whitening, its squared residual is therefore an exact quadratic cost, so KnownLandmarkFactor2 can contribute directly to a D=1 QcqpProblem.

The preferred conventional prediction uses the inverse action of $T^w_k=[R^w_k,t^w_k]$:

When parameterized by the entries of $T^w_k$, the term $(R^w_k)^{\mathsf T}t^w_k$ is bilinear. The residual is therefore not affine in those matrix entries, and its squared norm does not give the exact quadratic cost required by the current QCQP conversion. This algebraic limitation—not a difference in the physical observation—is the reason the second factor exists.

Use this variant only when every factor sharing the pose key expects the inverse convention. For relative measurements, FrobeniusLeftBetweenFactor uses the compatible relation $T^i_w=T^i_j T^j_w$. For ordinary nonlinear optimization, return to the preferred KnownLandmarkFactor.

Practical checklist¶

• Prefer KnownLandmarkFactorPose2/Pose3 with conventional wTk states.

• Use KnownLandmarkFactor2Pose2/Pose3 only for QCQP or certifiable optimization over inverse kTw states.

• Express wL in the world frame and measured_kP in the observing frame $k$.

• Supply a Cartesian noise model with dimension two for Pose2 or three for Pose3.

• Add enough non-degenerate observations or other factors to constrain the pose.

• Use a binary landmark factor when the landmark is unknown rather than fixed.

• Convert optimized kTw results back to wTk before combining them with conventional GTSAM pose values.

Related documentation¶

• KnownLandmarkLocalizationExample is the complete conventional $T^w_k$ localization tutorial.

• CertifiableLocalizationExample combines KnownLandmarkFactor2 with FrobeniusLeftBetweenFactor and SDP solvers.

• Frobenius factors document the compatible inverse-state relative-pose factor.

• WahbaFactor is the rotation-only direction-correspondence analogue.

• Dataset utilities document g2o/TORO loading and writing, including 3D landmark observations.