Certifiable localization in SE(3)

This notebook develops a complete certifiable localization example from a small g2o dataset. We solve the same deterministic 20-pose problem three ways: using a local Gauss–Newton solver and using two convex relaxations in the form of semidefinite programs (SDPs), one monolithic and one chordally decomposed. The local solution gives a familiar nonlinear least-squares baseline, while each SDP supplies a global lower bound. When a relaxation has a rank-one solution, its dominant eigenvector recovers the global minimizer directly; more generally, agreement between the lower bound and any feasible solution certifies that solution as globally optimal.

The example is deliberately organized around frame-aware names. We use the convention that $T^a_b$ transforms coordinates from frame $b$ to frame $a$. Thus $T^w_k$ is the conventional GTSAM pose of sensor frame $k$ in the world, while $T^k_w=(T^w_k)^{-1}$ maps world coordinates into sensor frame $k$. A world landmark is $L^w$. Its Cartesian measurement in frame $k$ is $\widetilde{P}^k$, while the pose predicts $T^k_w L^w$.

The g2o file stores conventional ground-truth poses $T^w_k$, world landmarks $L^w$, bearing-range observations, and relative-pose measurements $T^i_j$. The certifiable formulation intentionally optimizes the inverse states $T^k_w$.

Two known-landmark factors are available in Python. The wrapper exposes KnownLandmarkFactorPose3, which follows the conventional GTSAM $T^w_k$ state direction, and KnownLandmarkFactor2Pose3, which follows the inverse $T^k_w$ direction. This notebook deliberately uses the second version, KnownLandmarkFactor2Pose3, because it is the convention used in the implemented QCQP conversion. The first version is usually the more natural factor for ordinary nonlinear localization, but it is not compatible with the implemented QCQP conversion.

Along the way we will:

1. convert bearing-range measurements and covariances into Cartesian frame-$k$ measurements;

2. build a graph from KnownLandmarkFactor2Pose3 and FrobeniusLeftBetweenFactorPose3;

3. solve that graph locally over $T^k_w$ poses;

4. convert the same graph exactly into a QCQP and solve two SDP relaxations; and

5. recover conventional $T^w_k$ trajectories for comparison in the world frame.

For a tutorial centered on the first, conventional wrapper factor, see Known landmark localization in SE(3). API-level details are in the KnownLandmarkFactor notebook and FrobeniusFactor notebook.

From g2o measurements to certifiable factors¶

The dataset is stored using standard 3D GTSAM factors and values. Pose vertices are conventional $T^w_k$ poses, landmark vertices are $L^w$ points, each landmark observation is a BearingRangeFactor3D, and each odometry edge is a BetweenFactorPose3. The target graph has only pose variables: every loaded $L^w$ becomes fixed data inside a known-landmark factor.

Bearing-range to Cartesian coordinates¶

A bearing-range factor stores a measured unit bearing $u$, a scalar range $\rho$, and a Gaussian model in local bearing-range coordinates. The corresponding Cartesian point measured in frame $k$ is

Let $B\in\mathbb{R}^{3\times2}$ be the tangent basis returned by the measured Unit3. For a small bearing perturbation $\delta b\in\mathbb{R}^2$, the induced Cartesian perturbation is $\delta P^k\approx\rho B\,\delta b$. Thus the Jacobian with respect to bearing is $\rho B$, while the Jacobian with respect to range is $u$. The Jacobian from local bearing-range coordinates to Cartesian coordinates is therefore

We push the bearing-range covariance through that Jacobian:

This is the covariance seen by KnownLandmarkFactor2Pose3. The transformation is exact at the measurement linearization point. It does not make a nonlinear Gaussian in bearing-range coordinates globally identical to a Cartesian Gaussian; away from the measured ray, the two likelihood surfaces generally differ. Here $\kappa(\Sigma_{P^k})$ is the spectral condition number, so $\sqrt{\kappa(\Sigma_{P^k})}$ is the ratio between the longest and shortest one-standard-deviation axes. The code verifies that this ratio is 10.

Why the inverse-state factors are quadratic¶

This is where the distinction between the two wrapped landmark factors matters. KnownLandmarkFactorPose3 would evaluate a conventional $T^w_k$ with transformTo; here we instead instantiate KnownLandmarkFactor2Pose3 and supply a $T^k_w$. The 2 variant preserves the same physical prediction—a known $L^w$ expressed as a measured $\widetilde{P}^k$—but reverses the optimized pose direction to expose the matrix structure used below.

For a fixed world landmark $L^w$, KnownLandmarkFactor2Pose3 uses residual terms of the form

Here the multiplication is the usual homogeneous point transformation. Because $L^w$ is fixed, this residual is affine in the matrix entries of $T^k_w$. A full Gaussian noise model supplies the Cartesian information matrix, and the weighted squared residual is quadratic even when the noise is anisotropic and non-diagonal.

For consecutive poses, the loaded relative measurement $T^i_j$ and inverse states obey

That is precisely the left-composition convention used by FrobeniusLeftBetweenFactorPose3. Consequently, both factor families admit an exact D=1 QCQP representation of their nonlinear objectives. The following cell performs both conversions, rejects unexpected factor types, and confirms that the resulting graph contains $20\times4=80$ landmark factors plus 19 relative-pose factors.

Local optimization over $T^k_w$¶

Gauss–Newton needs values expressed in the same convention as the factors. The values loaded from g2o are $T^w_k$, so we explicitly invert each one before inserting it into ground_truth_kTws. This conversion is easy to overlook: inserting the loaded poses unchanged would give every factor the opposite transformation from the one its residual expects.

The initial estimate is constructed by retracting each ground-truth $T^k_w$ with a fixed tangent perturbation scaled by the pose index. The growing scale creates accumulated drift along the trajectory rather than giving every pose the same offset. This is useful pedagogically because the initial graph error is visibly poor, while the problem remains within the convergence basin of a local optimizer.

Gauss–Newton minimizes the nonlinear graph directly. It provides a feasible trajectory at every iteration, but it is a local method: convergence alone does not prove that another basin cannot contain a better solution. We therefore use it as both a practical baseline and a candidate solution to compare with the SDP lower bounds.

The measurements in the committed dataset are perturbed while the vertices store the exact generating trajectory. For that reason, the maximum-likelihood estimate is expected to have a slightly lower objective than the ground truth. This is not overfitting or an inconsistency; it is the normal consequence of optimizing against one particular realization of measurement noise. The assertions check that the objective decreases from both the initial estimate and the generating trajectory, and that the recovered poses nevertheless remain close to ground truth. Pose error is computed with Pose3.localCoordinates, so it is the norm of a six-dimensional rotational/translational tangent vector.

Monolithic and chordal semidefinite relaxations¶

The constructor QcqpProblem(graph, 1) creates a QCQP reformulation in which each of the above factors becomes a quadratic cost term in a flattened, homogenized pose vector (thus D=1), and quadratic constraints make each variable a valid vectorized pose. At a feasible collection of $T^k_w$ poses, the QCQP objective is the same weighted least-squares objective evaluated by the nonlinear graph. This is neither a linearization nor a relaxation of the factor graph: the QCQP is an exact reformulation of its nonlinear objective and pose constraints.

A QCQP is still nonconvex because the pose constraints are nonconvex. To form the semidefinite relaxation, we lift the homogeneous vector $x$ to a matrix $X=xx^{\mathsf T}$. The quadratic objective and constraints become linear in $X$; dropping the nonconvex condition $\operatorname{rank}(X)=1$ produces a convex semidefinite program whose optimum is a lower bound on the original minimization problem. If the optimal lifted blocks are rank one, the relaxation is tight and their dominant eigenvectors recover a globally optimal feasible QCQP solution. Rank one is sufficient but not necessary for tightness: even when an SDP solution has higher rank, a feasible local solution is certified globally optimal whenever its objective matches the SDP lower bound, up to numerical tolerance.

This notebook solves two representations of the same relaxation:

• Monolithic SDP: one global positive-semidefinite matrix contains all lifted variables. This is the most direct formulation, but it does not exploit much of the graph’s sparsity.

• Chordal SDP: a METIS ordering exposes sparse cliques. Smaller positive-semidefinite matrices are assigned to those cliques, with overlap constraints enforcing consistency. This usually reduces memory and can improve scalability while preserving the relaxation.

The MOSEK backend is optional, so the cell first checks whether its wrappers are available. After each solve, qcqpValues() recovers one homogeneous QCQP block per pose and extractQcqpValuesPose3 converts those blocks into feasible $T^k_w$ poses. We then evaluate the recovered poses in the original nonlinear graph, not merely in the relaxation.

The eigenvalue ratio (EVR) $\lambda_1/\lambda_2$ measures how close each lifted block is to rank one. A very large minimum EVR means the leading eigenvector dominates every block. In that rank-one case, the recovered feasible objective must agree with the SDP lower bound, and the recovered poses are globally optimal. Independently, agreement between the Gauss–Newton objective and the SDP lower bound certifies the local solution even if the relaxation were tight with a higher-rank optimizer. Here we require all EVRs to exceed 10⁵, all recovered poses to remain within the same small error threshold as the local solution, and all three objectives to agree closely.

Compare all results in the world frame¶

Every optimized state in the certifiable graph is $T^k_w$, but trajectories are normally visualized as sensor positions in the world. The translation stored directly in $T^k_w$ is the world origin expressed in frame $k$; it is not the position of frame $k$ in the world. We therefore invert every optimized pose before reading its translation:

The loaded ground truth is already stored as $T^w_k$, so its translations can be plotted directly. The landmarks are loaded as $L^w$ and likewise require no conversion. The helper world_translations makes the inversion explicit for the initial, Gauss–Newton, and recovered SDP values.

The plot is intended as a convention check as much as a performance comparison. If a $T^w_k/T^k_w$ conversion were accidentally omitted, the trajectory would not merely be shifted—its position and orientation would generally be interpreted in the wrong frame. With the conversions in place, the local and certifiable trajectories should nearly overlap the generating trajectory, while the dashed initialization visibly shows the injected drift.

Reading the result¶

The numerical results connect the local and certifiable viewpoints:

• The large initial error confirms that the deterministic perturbation created a meaningful optimization problem.

• Gauss–Newton lowers the objective below both the initialization and the ground truth while keeping every pose close to the generating trajectory.

• Each SDP objective is a lower bound on the QCQP optimum.

• Each recovered feasible graph error is an upper bound.

• The large EVRs show that the lifted SDP blocks are numerically rank one, so their dominant eigenvectors recover globally optimal feasible poses.

• Independently, the nearly matching SDP lower bound and Gauss–Newton error certify that the local optimizer found the globally optimal solution for this instance, within solver tolerance.

The central modeling choice is the state direction. KnownLandmarkFactor2Pose3 and FrobeniusLeftBetweenFactorPose3 optimize $T^k_w$ because this exposes the exact quadratic structure needed by the QCQP. That direction is intentional and local to the certifiable formulation; ordinary GTSAM localization normally represents poses as $T^w_k$. Whenever results leave the optimization graph—for plotting, interpretation, or comparison with the loaded values—we invert them back to $T^w_k$.

Finally, the certificate applies to the optimization model constructed here: the Cartesian known-landmark likelihood, the left-Frobenius relative-pose likelihood, and the supplied Gaussian weights. The earlier bearing-range-to-Cartesian covariance conversion is locally exact at each measurement, but the resulting nonlinear Cartesian model is not globally identical to the original bearing-range Gaussian model. Keeping that distinction explicit is part of interpreting the certificate correctly.