Certifiable Range-Aided SLAM on SE(3)

This notebook solves an $SE(3)$ range-aided SLAM problem with GTSAM’s Burer-Monteiro Riemannian Staircase and checks an SDP certificate that, when it passes, establishes global optimality. It extends the landmark example with range measurements to fixed beacons.

A raw range residual $(\lVert \ell_k - t_i \rVert - d_{ik})^2$ is not polynomial, so it cannot enter a QCQP. Each range instead gets an auxiliary unit direction $u_m$ and becomes

\nu_{ik}\lVert \ell_k - t_i - d_{ik}\, u_m \rVert^2 \quad\text{subject to}\quad \lVert u_m \rVert = 1,

(1)

whose minimum over the unit sphere is the raw residual, so the reformulation is exact. QuadraticRangeFactor3 contributes that term. The auxiliary is a Unit3, which lifts to a 1-by- $p$ block.

A cube world with range beacons¶

size, spacing = 2, 2.0

# Boustrophedon ("lawnmower") visit order through the cube.
order, cell_index = [], {}
for z in range(size):
    for y in (range(size) if z % 2 == 0 else reversed(range(size))):
        forward = (y % 2 == 0) == (z % 2 == 0)
        for x in (range(size) if forward else reversed(range(size))):
            cell_index[(x, y, z)] = len(order)
            order.append((x, y, z))

positions = np.array(order, dtype=float) * spacing

# Yaw follows the direction of travel; roll and pitch vary so the orientations
# do not stay inside a single one-parameter subgroup.
truth = []
for k in range(len(order)):
    step = positions[min(k + 1, len(order) - 1)] - positions[max(k - 1, 0)]
    yaw = np.arctan2(step[1], step[0]) if np.linalg.norm(step[:2]) > 1e-9 else 0.0
    truth.append(
        gtsam.Pose3(
            gtsam.Rot3.RzRyRx(0.15 * np.sin(k), -0.12 * np.cos(k), yaw), positions[k]
        )
    )

beacons = np.array([[-2.0, 1.0, 2.0], [6.0, 5.0, 1.0], [2.0, -3.0, 3.0]])

beacon_range = 7.0

odometry = [(k, k + 1) for k in range(len(order) - 1)]
ranges = [
    (k, b)
    for k in range(len(order))
    for b in range(len(beacons))
    if np.linalg.norm(beacons[b] - positions[k]) < beacon_range
]
print(f"{len(order)} poses, {len(beacons)} beacons")
print(f"{len(odometry)} odometry edges, {len(ranges)} range measurements")

8 poses, 3 beacons
7 odometry edges, 22 range measurements

Generative model¶

kappa, tau, nu = 100.0, 25.0, 40.0  # rotation, odometry, range precisions

# Isotropic Langevin on rotation has sigma = 1 / sqrt(2 kappa) in the
# small-dispersion limit; the others are plain Gaussians.
sigma_R = 1.0 / np.sqrt(2.0 * kappa)
sigma_t = 1.0 / np.sqrt(tau)
sigma_d = 1.0 / np.sqrt(nu)

graph = gtsam.NonlinearFactorGraph()
rot_noise = gtsam.noiseModel.Isotropic.Variance(3, 1.0 / kappa)

for i, j in odometry:
    relative = truth[i].between(truth[j])
    measured = gtsam.Pose3(
        relative.rotation().compose(gtsam.Rot3.Expmap(rng.normal(scale=sigma_R, size=3))),
        relative.translation() + rng.normal(scale=sigma_t, size=3),
    )

    graph.add(
        gtsam.FrobeniusBetweenFactorRot3(R(i), R(j), measured.rotation(), rot_noise)
    )
    graph.add(
        gtsam.RelativeTranslationFactor3(R(i), T(i), T(j), measured.translation(), tau)
    )

# One auxiliary direction per range measurement.
for m, (k, b) in enumerate(ranges):
    measured = np.linalg.norm(beacons[b] - positions[k]) + rng.normal(scale=sigma_d)
    graph.add(gtsam.QuadraticRangeFactor3(T(k), L(b), U(m), measured, nu))

print(f"rotation: kappa = {kappa:g}, sigma = {np.rad2deg(sigma_R):.2f} deg")
print(f"odometry: tau   = {tau:g}, sigma = {sigma_t:.2f} m")
print(f"range:    nu    = {nu:g}, sigma = {sigma_d:.2f} m")
print(f"{graph.size()} factors")

rotation: kappa = 100, sigma = 4.05 deg
odometry: tau   = 25, sigma = 0.20 m
range:    nu    = 40, sigma = 0.16 m
36 factors

Lifted initial values¶

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

initial = gtsam.Values()
for index in range(len(truth)):
    perturbed = truth[index].rotation().compose(
        gtsam.Rot3.Expmap(rng.normal(scale=0.3, size=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

# Beacons are unconstrained rows, the same shape as a translation.
for b in range(len(beacons)):
    initial.insert(L(b), rng.normal(scale=0.5, size=(1, rank)))

# Each auxiliary is a lifted Unit3: a 1-by-p block of unit norm.
for m in range(len(ranges)):
    direction = rng.normal(size=(1, rank))
    initial.insert(U(m), direction / np.linalg.norm(direction))

print("rotation slice:  ", initial.atMatrix(R(0)).shape)
print("beacon slice:    ", initial.atMatrix(L(0)).shape)
print("auxiliary slice: ", initial.atMatrix(U(0)).shape)

rotation slice:   (3, 3)
beacon slice:     (1, 3)
auxiliary slice:  (1, 3)

Run and certify¶

alm_params = gtsam.AugmentedLagrangianParams()
alm_params.maxIterations = 200

params = gtsam.RiemannianStaircaseParams()
params.pMin = rank
params.pMax = 12
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"Solver time:  {result.totalTime:.4f} s")

Certified:    True
Final rank:   4
Ranks tried:  [3 4]
Solver time:  0.0185 s

Round back to $SE(3)$ ¶

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))}
marks = {b: rounded.atMatrix(L(b)).copy() for b in range(len(beacons))}

# 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 list(rows.values()) + list(marks.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 Rot3 from a proper 3-by-3 rotation matrix."""
    return gtsam.Rot3(matrix)

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.Point3(rows[index][0]))
for b in range(len(beacons)):
    estimate.insert(L(b), gtsam.Point3(marks[b][0]))

# The optimal auxiliary is the unit vector between the two positions it links,
# so recover it from the recovered geometry rather than from its own block.
for m, (k, b) in enumerate(ranges):
    offset = np.asarray(estimate.atPoint3(L(b))) - np.asarray(estimate.atPoint3(T(k)))
    estimate.insert(U(m), gtsam.Unit3(offset))

relaxation_bound = float(np.asarray(result.getCostPerLevel())[-1])
rounded_objective = graph.error(estimate)

print(f"Relaxation lower bound: {relaxation_bound:.6f}")
print(f"Rounded objective:      {rounded_objective:.6f}")

Relaxation lower bound: 4.220448
Rounded objective:      7.886567

The two numbers are far apart, and that is expected here. Certification says the relaxation was solved to global optimality, not that the relaxation is tight. Dropping $\lVert u_m \rVert = 1$ to $u_m$ free lets every range term shrink toward zero, so the bound can sit well below anything achievable on the sphere. Judge the estimate by its geometry below rather than by the gap; the pose-graph and landmark notebooks, whose relaxations are tight, are where the bound is worth reading as a certificate of the answer itself.

# Each odometry edge contributes 6 residuals. A range gives only its radial
# component, and the auxiliary direction that absorbs the other 2 is fixed by
# the geometry at the optimum, so it adds no free parameter. The gauge removes
# one pose.
residuals = len(odometry) * 6 + len(ranges)
parameters = len(truth) * 6 + len(beacons) * 3 - 6
dof = residuals - parameters
print(f"residuals {residuals} - parameters {parameters} = {dof} dof")
print(f"Expected optimal cost (0.5 * chi^2): {0.5 * dof:.1f}")
print(f"Observed optimal cost:               {rounded_objective:.1f}")
print(f"Ratio:                               {rounded_objective / (0.5 * dof):.2f}")

residuals 64 - parameters 51 = 13 dof
Expected optimal cost (0.5 * chi^2): 6.5
Observed optimal cost:               7.9
Ratio:                               1.21

The estimated map¶

def procrustes(source, target):
    """Best rigid alignment of source onto target."""
    source_centre, target_centre = source.mean(0), target.mean(0)
    A, B = source - source_centre, target - target_centre
    left, _, right_transpose = np.linalg.svd(A.T @ B)
    return (A @ (left @ right_transpose)) + target_centre

positions_est = np.array([estimate.atPoint3(T(i)) for i in range(len(truth))])
beacons_est = np.array([estimate.atPoint3(L(b)) for b in range(len(beacons))])

# The relaxation fixes no gauge, so align poses and beacons together.
aligned = procrustes(np.vstack([positions_est, beacons_est]),
                     np.vstack([positions, beacons]))
aligned_poses = aligned[: len(positions)]
aligned_beacons = aligned[len(positions) :]

figure = go.Figure()
figure.add_trace(go.Scatter3d(x=positions[:, 0], y=positions[:, 1], z=positions[:, 2],
                              mode="lines+markers", name="ground truth",
                              line=dict(color="#2a78d6", width=4), marker=dict(size=3)))
figure.add_trace(go.Scatter3d(x=aligned_poses[:, 0], y=aligned_poses[:, 1],
                              z=aligned_poses[:, 2], mode="lines+markers",
                              name="estimate", line=dict(color="#eb6834", width=4),
                              marker=dict(size=3)))
figure.add_trace(go.Scatter3d(x=beacons[:, 0], y=beacons[:, 1], z=beacons[:, 2],
                              mode="markers", name="beacons (truth)",
                              marker=dict(color="#2a78d6", size=7, symbol="square-open")))
figure.add_trace(go.Scatter3d(x=aligned_beacons[:, 0], y=aligned_beacons[:, 1],
                              z=aligned_beacons[:, 2], mode="markers",
                              name="beacons (estimate)",
                              marker=dict(color="#eb6834", size=7, symbol="x")))
figure.update_layout(width=720, height=600,
                     scene=dict(xaxis_title="x [m]", yaxis_title="y [m]",
                                zaxis_title="z [m]", aspectmode="data"))
figure.show()

error = np.linalg.norm(aligned_poses - positions, axis=1)
print(f"RMS position error after alignment: {np.sqrt((error ** 2).mean()):.4f} m")

RMS position error after alignment: 0.3596 m

Where the time goes¶

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.00018 s
 local solve: 0.01767 s
      verify: 0.00060 s