Certifiable Pose Graph Optimization on SE(2)

This notebook solves an $SE(2)$ synchronization problem with GTSAM’s Burer-Monteiro Riemannian Staircase and checks an SDP certificate that, when it passes, establishes global optimality. It extends the rotation averaging example with translations, so the estimate is a full trajectory rather than a set of orientations.

Each relative pose measurement is split across two factors that share a rotation key:

FrobeniusBetweenFactorRot2 contributes $\kappa_{ij}\lVert R_j - R_i R_{ij}\rVert_F^2$ ,
RelativeTranslationFactor2 contributes $\tau_{ij}\lVert t_j - t_i - R_i \tilde{t}_{ij}\rVert^2$ .

Both are quadratic, so the staircase can lift the whole problem to a low-rank SDP. See also the $SE(3)$ version.

import numpy as np
import plotly.graph_objects as go

import gtsam
from gtsam.symbol_shorthand import R, T

np.set_printoptions(precision=4, suppress=True)
rng = np.random.default_rng(6)

A grid world¶

A ring has a single cycle. A grid has many: the robot drives a boustrophedon (lawnmower) path through a 5-by-5 grid, and every pair of grid neighbours the path did not visit consecutively becomes a loop closure. The resulting short cycles all have to be reconciled against each other, which is a more representative test than a ring.

rows, cols, spacing = 5, 5, 2.0

# Boustrophedon ("lawnmower") visit order through the grid.
order, cell_index = [], {}
for row in range(rows):
    columns = range(cols) if row % 2 == 0 else reversed(range(cols))
    for col in columns:
        cell_index[(row, col)] = len(order)
        order.append((row, col))

positions = np.array([[col * spacing, row * spacing] for row, col in order])

# Each pose faces along its direction of travel.
truth = []
for k in range(len(order)):
    step = positions[min(k + 1, len(order) - 1)] - positions[max(k - 1, 0)]
    truth.append(
        gtsam.Pose2(positions[k][0], positions[k][1], np.arctan2(step[1], step[0]))
    )

# Odometry follows the path; loop closures join grid neighbours the path skipped.
odometry = [(k, k + 1) for k in range(len(order) - 1)]
neighbours = [(1, 0), (0, 1)]
loops = sorted({
    (min(k, other), max(k, other))
    for (row, col), k in cell_index.items()
    for step in neighbours
    for other in [cell_index.get((row + step[0], col + step[1]))]
    if other is not None and (k, other) not in odometry and (other, k) not in odometry
})
edges = odometry + loops

print(f"{len(truth)} poses, {len(edges)} edges "
      f"({len(odometry)} odometry, {len(loops)} loop closures)")

25 poses, 40 edges (24 odometry, 16 loop closures)

Generative model¶

Measurements follow the noise model that the estimator assumes, so the objective below is the maximum-likelihood one for this model. For an edge $(i,j)$ with true relative pose $R_{ij} = R_i^\top R_j$ and $t_{ij} = R_i^\top(t_j - t_i)$ :

\tilde{R}_{ij} = R_{ij}\,\mathrm{Exp}(\omega),\quad \omega \sim \mathcal{N}(0, \sigma_R^2 I), \qquad \tilde{t}_{ij} = t_{ij} + \varepsilon,\quad \varepsilon \sim \mathcal{N}(0, \tau^{-1} I_2).

(1)

The rotational law is the small-dispersion limit of the isotropic Langevin distribution with concentration $\kappa$ , whose density is proportional to $\exp(\kappa\,\mathrm{tr}\,R)$ . Since $\mathrm{tr}\,\mathrm{Exp}(\omega) \approx d - \lVert\omega\rVert^2$ , that density behaves like a Gaussian of variance $1/(2\kappa)$ , so

\sigma_R = \frac{1}{\sqrt{2\kappa}},\qquad \sigma_t = \frac{1}{\sqrt{\tau}}.

(2)

The factor of two matters. Sampling with $\sigma_R = 1/\sqrt{\kappa}$ instead injects noise that does not match the weight $\kappa$ the factor applies, and the optimal cost then sits well above its $\chi^2$ expectation. The check after rounding will show it; changing the line below is enough to reproduce.

kappa, tau = 100.0, 25.0  # rotational and translational precisions
sigma_rotation = 1.0 / np.sqrt(2.0 * kappa)
sigma_translation = 1.0 / np.sqrt(tau)

graph = gtsam.NonlinearFactorGraph()
rotation_noise = gtsam.noiseModel.Isotropic.Variance(1, 1.0 / kappa)
for source, target in edges:
    relative = truth[source].between(truth[target])
    noisy = gtsam.Pose2(
        gtsam.Rot2.fromAngle(relative.theta() + rng.normal(scale=sigma_rotation)),
        relative.translation() + rng.normal(scale=sigma_translation, size=2),
    )
    graph.add(
        gtsam.FrobeniusBetweenFactorRot2(
            R(source), R(target), noisy.rotation(), rotation_noise
        )
    )
    graph.add(
        gtsam.RelativeTranslationFactor2(
            R(source), T(source), T(target), noisy.translation(), tau
        )
    )

print(f"rotation:    kappa = {kappa:g}, sigma = {np.degrees(sigma_rotation):.2f} deg")
print(f"translation: tau   = {tau:g}, sigma = {sigma_translation:.2f} m")
print(f"{graph.size()} factors over {len(truth)} rotation and {len(truth)} translation keys")

rotation:    kappa = 100, sigma = 4.05 deg
translation: tau   = 25, sigma = 0.20 m
80 factors over 25 rotation and 25 translation keys

Lifted initial values¶

The staircase optimizes over a stacked matrix $Y \in \mathbb{R}^{n \times p}$ , and each variable occupies a horizontal slice of it. The two variable kinds have different row counts, which is the one thing worth getting right:

variable	slice shape	contents
rotation `R(i)`	$2 \times p$	the lifted $R_i^\top$ ; at $p = d$ this is literally `rotation.matrix().T`
translation `T(i)`	$1 \times p$	the lifted $t_i^\top$ , a single row

We start at $p = d = 2$ , from perturbed ground-truth rotations and random translations. The certificate is checked wherever the solver lands, so a poor initialization costs staircase levels rather than correctness.

rank = 2  # p = d for the first staircase level

initial = gtsam.Values()
for index in range(len(truth)):
    perturbed = gtsam.Rot2.fromAngle(
        truth[index].theta() + rng.normal(scale=0.3)
    )
    initial.insert(R(index), perturbed.matrix().T)                   # d x p
    initial.insert(T(index), rng.normal(scale=0.5, size=(1, rank)))  # 1 x p

print("rotation slice:", initial.atMatrix(R(0)).shape)
print("translation slice:", initial.atMatrix(T(0)).shape)

rotation slice: (2, 2)
translation slice: (1, 2)

Run and certify¶

The inner augmented-Lagrangian solver enforces the orthogonality constraints that the rotation factors emit. The outer staircase then forms the dual matrix $S = Q + \mathcal{A}^*(\lambda)$ and checks $S \succeq 0$ . If the check passes, the factorization $Z = YY^\top$ is a global optimum of the SDP relaxation; if it fails, the staircase lifts to rank $p+1$ along the negative-eigenvalue direction and tries again.

alm_params = gtsam.AugmentedLagrangianParams()
alm_params.maxIterations = 50
alm_params.absoluteViolationTolerance = 1e-8

params = gtsam.RiemannianStaircaseParams()
params.pMin = rank
params.pMax = 6
params.eta = 1e-3
params.setAlmParams(alm_params)

result = gtsam.RiemannianStaircaseOptimizer(graph, initial, params).optimize()

print(f"Certified:    {result.certified}")
print(f"Final rank:   {result.finalRank}")
print(f"Ranks tried:  {np.asarray(result.getRanksVisited()).astype(int)}")
print(f"lambda_min:   {result.minEigenvalue:.3e}")
print(f"Solver time:  {result.totalTime:.4f} s")

Certified:    True
Final rank:   2
Ranks tried:  [2]
lambda_min:   -1.000e-03
Solver time:  0.0105 s

Round back to $SE(2)$ ¶

The staircase optimizes over $O(d)$ , which has a reflected component, so the rounded blocks can come out with $\det = -1$ . We flip the last column of every block when the reflected blocks are in the majority, then project each rotation slice to the closest proper rotation and read each translation off its single row.

The reported objective is graph.error, which is gauge invariant, so no alignment to ground truth is needed to judge the solution.

if not result.hasRoundedSolution():
    raise RuntimeError("The staircase did not return a rounded solution.")

rounded = result.roundedValues()
blocks = {i: rounded.atMatrix(R(i)).copy() for i in range(len(truth))}
rows = {i: rounded.atMatrix(T(i)).copy() for i in range(len(truth))}

# Certifiable methods optimize over O(d); undo a global reflection if present.
if sum(np.linalg.det(block) < 0.0 for block in blocks.values()) > len(truth) // 2:
    for block in blocks.values():
        block[:, -1] *= -1.0
    for row in rows.values():
        row[:, -1] *= -1.0

def project_to_so(matrix):
    """Return the closest proper rotation to a square matrix."""
    left, _, right_transpose = np.linalg.svd(matrix)
    correction = np.eye(matrix.shape[0])
    correction[-1, -1] = np.linalg.det(left @ right_transpose)
    return left @ correction @ right_transpose

def rotation_from_matrix(matrix):
    """Rebuild a Rot2 from a proper 2-by-2 rotation matrix."""
    return gtsam.Rot2.fromAngle(np.arctan2(matrix[1, 0], matrix[0, 0]))

estimate = gtsam.Values()
for index in range(len(truth)):
    estimate.insert(R(index), rotation_from_matrix(project_to_so(blocks[index].T)))
    estimate.insert(T(index), gtsam.Point2(rows[index][0]))

truth_values = gtsam.Values()
for index in range(len(truth)):
    truth_values.insert(R(index), truth[index].rotation())
    truth_values.insert(T(index), gtsam.Point2(truth[index].translation()))

relaxation_bound = float(np.asarray(result.getCostPerLevel())[-1])
rounded_objective = graph.error(estimate)
gap = (rounded_objective - relaxation_bound) / max(relaxation_bound, 1e-12)

print(f"Relaxation lower bound: {relaxation_bound:.6f}")
print(f"Rounded objective:      {rounded_objective:.6f}")
print(f"Relative gap:           {gap:.3e}")
print(f"Objective at truth:     {graph.error(truth_values):.6f}")

Relaxation lower bound: 24.328973
Rounded objective:      24.328975
Relative gap:           1.121e-07
Objective at truth:     61.512158

A relative gap at the level of solver tolerance means the rounded $SE(2)$ trajectory matches the SDP lower bound to that tolerance, which is what certifies it as a global minimizer. The objective at ground truth is higher, as it should be: with noisy measurements the maximum-likelihood estimate fits the data better than the poses that generated it.

Is the noise model right?¶

Because graph.error is one half of a sum of squared whitened residuals, a correctly specified model puts the optimum near $\tfrac12\chi^2$ with degrees of freedom equal to the number of independent residuals minus the number of free parameters. Each edge contributes 1 rotational and 2 translational residual dimensions, and each pose carries the same count of parameters. This is a single draw, so expect the ratio to scatter by roughly $\pm\sqrt{2/\mathrm{dof}}$ .

rotation_dof = 1
residuals = len(edges) * (rotation_dof + 2)
parameters = len(truth) * (rotation_dof + 2)
expected = 0.5 * (residuals - parameters)

print(f"residuals {residuals} - parameters {parameters} = {residuals - parameters} dof")
print(f"Expected optimal cost (0.5 * chi^2): {expected:.1f}")
print(f"Observed optimal cost:               {rounded_objective:.1f}")
print(f"Ratio:                               {rounded_objective / expected:.2f}")

residuals 120 - parameters 75 = 45 dof
Expected optimal cost (0.5 * chi^2): 22.5
Observed optimal cost:               24.3
Ratio:                               1.08

The estimated grid¶

Absolute poses are only determined up to a global $SE(2)$ transform, so a Procrustes fit is applied here purely so the two graphs can be drawn in one frame. It plays no role in the certificate or the objective above. Every edge is drawn, so the loop closures that tie the grid together are visible.

def procrustes(source, target):
    """Rigid transform aligning source points to target points, for display only."""
    source_mean, target_mean = source.mean(axis=0), target.mean(axis=0)
    covariance = (target - target_mean).T @ (source - source_mean)
    left, _, right_transpose = np.linalg.svd(covariance)
    correction = np.eye(covariance.shape[0])
    correction[-1, -1] = np.linalg.det(left @ right_transpose)
    rotation = left @ correction @ right_transpose
    return rotation, target_mean - rotation @ source_mean

truth_points = np.array([truth[i].translation() for i in range(len(truth))])
estimate_points = np.array([estimate.atPoint2(T(i)) for i in range(len(truth))])
alignment, offset = procrustes(estimate_points, truth_points)
aligned = estimate_points @ alignment.T + offset

def segments(points, pairs):
    """Flatten graph edges into one polyline trace separated by None gaps."""
    return [
        np.array([coordinate
                  for i, j in pairs
                  for coordinate in (points[i][axis], points[j][axis], None)],
                 dtype=object)
        for axis in range(points.shape[1])
    ]

figure = go.Figure()
for points, pairs, name, colour, width in (
    (truth_points, edges, "ground truth", "#888888", 2),
    (aligned, edges, "certified estimate", "#1f77b4", 3),
):
    xs, ys = segments(points, pairs)
    figure.add_trace(go.Scatter(x=xs, y=ys, mode="lines", name=name,
                        line=dict(color=colour, width=width)))
    figure.add_trace(go.Scatter(x=points[:, 0], y=points[:, 1], mode="markers", showlegend=False,
                        marker=dict(size=3, color=colour)))
figure.update_layout(
    title="Certified SE(2) grid (Procrustes-aligned for display)",
    xaxis_title="x [m]", yaxis_title="y [m]",
    yaxis=dict(scaleanchor="x", scaleratio=1),
    height=560, margin=dict(l=0, r=0, t=40, b=0))
figure.show()

print(f"RMS position error after alignment: "
      f"{np.sqrt(((aligned - truth_points) ** 2).sum(axis=1).mean()):.4f} m")

RMS position error after alignment: 0.2149 m

Where the time goes¶

The staircase reports a per-level breakdown: building the rank- $p$ QCQP, running the local solver, and verifying the certificate.

ranks = np.asarray(result.getRanksVisited()).astype(int)
phases = {
    "QCQP build": np.asarray(result.getQcqpBuildTimePerLevel()),
    "local solve": np.asarray(result.getNlpTimePerLevel()),
    "verify": np.asarray(result.getVerifyTimePerLevel()),
}

timing = go.Figure()
for name, seconds in phases.items():
    timing.add_trace(go.Bar(x=[f"p = {level}" for level in ranks], y=seconds, name=name))
timing.update_layout(
    barmode="stack", title="Time per staircase level",
    xaxis_title="staircase rank", yaxis_title="seconds",
    height=380, margin=dict(l=0, r=0, t=40, b=0))
timing.show()

for name, seconds in phases.items():
    print(f"{name:>12}: {seconds.sum():.5f} s")

  QCQP build: 0.00015 s
 local solve: 0.01020 s
      verify: 0.00010 s