A certifiable Wahba problem on SO(3)

This notebook estimates one three-dimensional rotation from noisy direction correspondences. We solve the same chordal Wahba objective two ways: using a local Levenberg–Marquardt solver and using a convex relaxation in the form of a semidefinite program (SDP). The local solution is fast and familiar. When the returned SDP matrix is rank one, the relaxation is tight and its dominant eigenvector recovers the globally optimal solution. Regardless of rank, the SDP also provides a lower bound on the cost.

We follow GTSAM’s frame convention throughout: $R^a_b$ rotates coordinates from frame $b$ into frame $a$ . A known direction expressed in frame $b$ is bDirection ( $d^b$ below), and its noisy measurement in frame $a$ is measured_aDirection ( $\widetilde{d}^a$ below). The new WahbaFactor evaluates

e(R^a_b)=R^a_b d^b-\widetilde{d}^a.

(1)

Both directions are stored as Unit3, but, to create a polynomial cost, the residual is computed as the difference of their three-dimensional unit vectors. This is the classical chordal Wahba residual. It is affine in the nine matrix entries of $R^a_b$ , so its weighted squared norm has an exact quadratic representation. The only nonconvex part is requiring those nine entries to form a proper rotation.

WahbaFactor is intentionally different from RotateDirectionsFactor. The latter returns the two-dimensional Unit3 tangent-space error and represents a spherical likelihood. This notebook uses the three-dimensional chordal likelihood because that is the objective with the exact QCQP conversion.

See the WahbaFactor API notebook for the focused residual, noise-model, and QCQP comparison.

import numpy as np
import plotly.graph_objects as go

import gtsam

np.set_printoptions(precision=4, suppress=True)

Build a noisy direction-alignment problem¶

A Wahba problem needs at least two non-collinear direction correspondences to determine a rotation. We use twelve directions distributed randomly over the sphere. The ground-truth ground_truth_aRb maps each bDirection into frame $a$ , after which a small random rotation perturbs the measured direction. The seed is fixed so the checked-in output is reproducible.

Every factor receives an isotropic three-dimensional Gaussian model. That model weights the chordal vector residual, not a two-dimensional angular residual. Although each Unit3 measurement has only two intrinsic degrees of freedom, the radial component of a chordal residual is useful algebraically: it keeps the prediction affine in the complete rotation matrix. For an angular error $\theta$ , the magnitude of the radial discrepancy is $1-\cos\theta=\tfrac12\theta^2+O(\theta^4)$ , so it is second order for small angular noise.

The graph contains a single unknown, aRb, observed by all twelve factors. There is no gauge freedom because the directions are expressed in fixed frames $a$ and $b$ .

rng = np.random.default_rng(42)
rotation_key = 0
num_directions = 12
direction_sigma = 0.02

ground_truth_aRb = gtsam.Rot3.RzRyRx(0.45, -0.35, 0.70)
raw_bDirections = rng.normal(size=(num_directions, 3))
raw_bDirections /= np.linalg.norm(raw_bDirections, axis=1, keepdims=True)

bDirections = []
measured_aDirections = []
graph = gtsam.NonlinearFactorGraph()
model = gtsam.noiseModel.Isotropic.Sigma(3, direction_sigma)
for raw_bDirection in raw_bDirections:
    bDirection = gtsam.Unit3(raw_bDirection)
    exact_aDirection = ground_truth_aRb.rotate(bDirection)
    noise_aRa = gtsam.Rot3.Expmap(
        rng.normal(scale=direction_sigma, size=3)
    )
    measured_aDirection = noise_aRa.rotate(exact_aDirection)
    graph.add(
        gtsam.WahbaFactor(
            rotation_key, bDirection, measured_aDirection, model
        )
    )
    bDirections.append(bDirection)
    measured_aDirections.append(measured_aDirection)

print(f"direction correspondences: {graph.size()}")
print(f"unknown rotations:         1")
assert graph.size() == num_directions

direction correspondences: 12
unknown rotations:         1

1. Local nonlinear optimization¶

We first optimize the factor graph directly on $SO(3)$ . The initial estimate is deliberately displaced from the generating rotation by a large three-axis perturbation. Levenberg–Marquardt computes increments in the three-dimensional tangent space and retracts them back to Rot3; the factors themselves still evaluate the three-dimensional chordal residual.

This solve produces a valid rotation at every iteration and usually converges quickly for Wahba problems. It does not, by itself, prove global optimality. The optimizer only establishes that it found a stationary point. Later, the SDP objective will serve as a global lower bound against which this feasible locally optimal objective can be compared.

Because the measurements are noisy, the maximum-likelihood estimate need not equal the generating rotation exactly. It can also have a slightly lower graph error than ground truth because it adapts to this particular realization of the noise. We report geodesic rotation error in degrees using Rot3.localCoordinates.

def rotation_error_degrees(expected_aRb, actual_aRb):
    return np.rad2deg(
        np.linalg.norm(expected_aRb.localCoordinates(actual_aRb))
    )

initial_aRb = ground_truth_aRb.compose(
    gtsam.Rot3.Expmap(np.array([0.65, -0.45, 0.55]))
)
initial_values = gtsam.Values()
initial_values.insert(rotation_key, initial_aRb)

parameters = gtsam.LevenbergMarquardtParams()
parameters.setMaxIterations(100)
parameters.setRelativeErrorTol(1e-12)
local_values = gtsam.LevenbergMarquardtOptimizer(
    graph, initial_values, parameters
).optimize()
local_aRb = local_values.atRot3(rotation_key)

ground_truth_values = gtsam.Values()
ground_truth_values.insert(rotation_key, ground_truth_aRb)
initial_error = graph.error(initial_values)
ground_truth_error = graph.error(ground_truth_values)
local_error = graph.error(local_values)
initial_rotation_error = rotation_error_degrees(
    ground_truth_aRb, initial_aRb
)
local_rotation_error = rotation_error_degrees(
    ground_truth_aRb, local_aRb
)

print(f"ground-truth objective: {ground_truth_error:.6f}")
print(f"initial objective:      {initial_error:.6f}")
print(f"locally optimal objective: {local_error:.6f}")
print(f"initial rotation error: {initial_rotation_error:.3f} deg")
print(f"locally optimal rotation error: {local_rotation_error:.3f} deg")

assert local_error < initial_error
assert local_rotation_error < 2.0

ground-truth objective: 7.414537
initial objective:      8638.479601
locally optimal objective: 7.205960
initial rotation error: 55.180 deg
locally optimal rotation error: 0.249 deg

Exact QCQP and semidefinite relaxation¶

For each correspondence, write the residual as follows:

e=R^a_b d^b-\widetilde{d}^a=Bx,

(2)

where $x=[1;\operatorname{vec}(R^a_b)]$ is a homogeneous vector. With this reformulation, we can write the factor cost as $\tfrac12x^{\mathsf T}Qx$ with $Q=B^{\mathsf T}B$ . When we construct QcqpProblem(graph, 1), we accumulate those exact quadratic costs and also add the quadratic orthonormality and handedness constraints required for $R^a_b\in SO(3)$ . This is not a linearization or relaxation of the factor graph: the QCQP is an exact reformulation of the nonlinear factor graph.

The QCQP remains nonconvex because its rotation constraints are nonconvex. An SDP introduces $X=xx^{\mathsf T}$ , rewrites every quadratic expression as a linear function of $X$ , and drops the condition $\operatorname{rank}(X)=1$ . The relaxed optimum is therefore a lower bound on the original Wahba objective. If the returned block is numerically rank one, the relaxation is tight, and the leading eigenvector of $X$ recovers the globally optimal solution.

Before solving the relaxation, the following cell checks the exact objective conversion at the local result. The equality violation should be at numerical precision because local_aRb is a valid Rot3.

qcqp = gtsam.QcqpProblem(graph, 1)
local_qcqp_values = gtsam.Values()
gtsam.insertQcqpValueRot3(rotation_key, local_aRb, local_qcqp_values)
qcqp_cost, equality_violation, inequality_violation = qcqp.evaluate(
    local_qcqp_values
)

print(f"nonlinear objective:       {local_error:.9f}")
print(f"QCQP objective:            {qcqp_cost:.9f}")
print(f"QCQP equality violation:   {equality_violation:.3e}")
print(f"QCQP inequality violation: {inequality_violation:.3e}")

assert np.isclose(qcqp_cost, local_error, atol=1e-9)
assert equality_violation < 1e-10
assert inequality_violation < 1e-10

nonlinear objective:       7.205960070
QCQP objective:            7.205960070
QCQP equality violation:   8.136e-16
QCQP inequality violation: 0.000e+00

2. Global SDP optimization¶

MosekMonolithicSDP is the solver we use to optimize a single positive-semidefinite variable that combines the lifted, vectorized expressions of all variables in the factor graph. For this single-rotation problem, it is the natural direct expression of the relaxation.

For this Wahba problem the lifted homogeneous vector has only ten entries—one constant plus nine rotation-matrix entries—so the monolithic cone is tiny.

Remark. We call this solver MonolithicSDP to distinguish it from ChordalSDP. For factor graphs with many variables, ChordalSDP is usually preferable: it decomposes the problem into maximal cliques and solves an SDP over many smaller cones. See the CertifiableLocalizationExample notebook for more information.

The optional MOSEK backend returns three useful pieces of evidence: objectiveValue() is the relaxed lower bound; the recovered Rot3, evaluated in the original graph, is a feasible upper bound; and variableEVRs() reports $\lambda_1/\lambda_2$ for the lifted block. A large eigenvalue ratio indicates numerical rank one. The notebook skips this section cleanly when GTSAM was built without MOSEK.

sdp_results = {}
mosek_available = hasattr(gtsam, "MosekMonolithicSDP")

if not mosek_available:
    print("This GTSAM build does not include the optional MOSEK backend.")
else:
    monolithic_solver = gtsam.MosekMonolithicSDP(qcqp)
    if not monolithic_solver.solve(
        {"intpntCoTolRelGap": 1e-10, "optimizerMaxTime": 600.0}
    ):
        raise RuntimeError(
            "monolithic SDP did not return a readable solution"
        )

    monolithic_values = gtsam.extractQcqpValuesRot3(
        monolithic_solver.qcqpValues()
    )
    monolithic_aRb = monolithic_values.atRot3(rotation_key)
    monolithic_feasible_error = graph.error(monolithic_values)
    monolithic_rotation_error = rotation_error_degrees(
        ground_truth_aRb, monolithic_aRb
    )
    monolithic_evrs = np.asarray(monolithic_solver.variableEVRs())
    sdp_results["monolithic SDP"] = {
        "aRb": monolithic_aRb,
        "objective": monolithic_solver.objectiveValue(),
        "feasible_error": monolithic_feasible_error,
        "rotation_error": monolithic_rotation_error,
        "solve_time": monolithic_solver.solveTimeSeconds(),
        "min_evr": monolithic_evrs.min(),
    }

    print(f"status:             {monolithic_solver.problemStatus()}")
    print(f"SDP lower bound:    {monolithic_solver.objectiveValue():.9f}")
    print(f"feasible objective: {monolithic_feasible_error:.9f}")
    print(f"solve time:         {monolithic_solver.solveTimeSeconds():.3f} s")
    print(f"minimum EVR:        {monolithic_evrs.min():.3e}")
    print(f"rotation error:     {monolithic_rotation_error:.3f} deg")

    assert monolithic_rotation_error < 2.0
    assert np.all(monolithic_evrs >= 1e5)

status:             ProblemStatus::PrimalAndDualFeasible
SDP lower bound:    7.205960070
feasible objective: 7.205960070
solve time:         0.018 s
minimum EVR:        5.342e+14
rotation error:     0.249 deg

Compare objectives, errors, and aligned directions¶

A certificate is easiest to read as a lower-bound/upper-bound comparison. Each SDP objective is a lower bound because it minimizes over a relaxed feasible set. Evaluating a recovered Rot3 in the original graph gives an upper bound because that rotation is feasible for the nonconvex problem. When the lifted block is rank one, the relaxation is tight, the lower and upper bounds agree up to numerical tolerance, and we can recover the globally optimal solution from the SDP relaxation. If the local solver returns an objective that agrees with the SDP objective, the local optimizer has reached the certified global solution for this model.

The 3D Plotly figure provides a geometric view. Gray markers are the noisy directions measured in frame $a$ . Each line starts at the origin and ends at the prediction $R^a_b d^b$ . The initial predictions are visibly displaced; the local and monolithic SDP predictions should overlap closely near the measurements and the generating directions.

Remember what is certified: the supplied three-dimensional chordal residuals and Gaussian weights. A factor built with Unit3::errorVector, or another spherical noise model, would define a different nonlinear objective and would not inherit this certificate.

summaries = {
    "initial": {
        "objective": initial_error,
        "feasible_error": initial_error,
        "rotation_error": initial_rotation_error,
    },
    "local LM": {
        "objective": local_error,
        "feasible_error": local_error,
        "rotation_error": local_rotation_error,
    },
    **sdp_results,
}
for name, summary in summaries.items():
    print(
        f"{name:15s}: objective={summary['objective']:.9f}, "
        f"feasible={summary['feasible_error']:.9f}, "
        f"rotation error={summary['rotation_error']:.3f} deg"
    )

if mosek_available:
    for name, summary in sdp_results.items():
        assert abs(summary["feasible_error"] - local_error) < 1e-5
        assert abs(summary["objective"] - local_error) < 1e-5

def predicted_aDirections(aRb):
    return np.vstack([
        aRb.rotate(bDirection).unitVector()
        for bDirection in bDirections
    ])

def direction_segments(directions):
    coordinates = [[], [], []]
    for direction in directions:
        for axis in range(3):
            coordinates[axis].extend([0.0, direction[axis], None])
    return coordinates

fig = go.Figure()
measured = np.vstack([
    direction.unitVector() for direction in measured_aDirections
])
fig.add_scatter3d(
    x=measured[:, 0], y=measured[:, 1], z=measured[:, 2],
    mode="markers", marker={"color": "gray", "size": 5},
    name="measured aDirection",
)
estimates = {
    "ground truth aRb": ground_truth_aRb,
    "initial aRb": initial_aRb,
    "local LM": local_aRb,
    **{name: result["aRb"] for name, result in sdp_results.items()},
}
styles = {
    "ground truth aRb": {"color": "black", "dash": "dot"},
    "initial aRb": {"color": "orange", "dash": "dash"},
    "local LM": {"color": "royalblue"},
    "monolithic SDP": {"color": "purple"},
}
for name, estimate_aRb in estimates.items():
    x, y, z = direction_segments(predicted_aDirections(estimate_aRb))
    fig.add_scatter3d(
        x=x, y=y, z=z, mode="lines",
        line={**styles[name], "width": 4}, name=name,
    )
fig.update_layout(
    title="Wahba direction alignment in frame a",
    template="plotly_white", width=850, height=650,
    scene={
        "xaxis_title": "a-x",
        "yaxis_title": "a-y",
        "zaxis_title": "a-z",
        "aspectmode": "cube",
    },
)
fig.show()

initial        : objective=8638.479600792, feasible=8638.479600792, rotation error=55.180 deg
local LM       : objective=7.205960070, feasible=7.205960070, rotation error=0.249 deg
monolithic SDP : objective=7.205960070, feasible=7.205960070, rotation error=0.249 deg

Reading the result¶

The two solution paths answer different questions about the same model:

Local LM finds a feasible Rot3 efficiently, but offers no global guarantee on its own.
Global SDP optimization solves the relaxation over one positive-semidefinite cone and supplies the lower bound and rank diagnostic.

A large EVR says the lifted matrix is numerically rank one. Then the relaxation is tight, and we can recover the globally optimal solution directly from it. Agreement between the SDP lower bound and the local objective certifies that LM reached the global minimizer of this chordal Wahba instance, up to numerical tolerance. The remaining error from the generating ground_truth_aRb reflects measurement noise, not optimization failure.

This clean one-rotation example isolates the role of WahbaFactor: its three-dimensional chordal residual is what makes the exact quadratic cost possible, while the monolithic SDP supplies the certificate.