WahbaFactor

WahbaFactor estimates a Rot3 from a direction correspondence using the classical three-dimensional chordal Wahba residual. The factor stores both directions as Unit3, supports full three-dimensional Gaussian weighting, and provides an exact D=1 QCQP conversion.

import numpy as np
import gtsam

Chordal direction alignment¶

We use the convention that $R^a_b$ rotates coordinates from frame $b$ into frame $a$ . Given a known bDirection and its measured_aDirection, the unwhitened residual is

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

(1)

The residual is a three-vector formed from the two unit vectors in $\mathbb{R}^3$ . It is not Unit3::errorVector. This distinction determines both the likelihood and the algebraic structure: the chordal residual is affine in the nine entries of the rotation matrix.

key = 0
aRb = gtsam.Rot3.RzRyRx(0.2, -0.1, 0.3)
bDirection = gtsam.Unit3(np.array([1.2, -0.7, 0.5]))
exact_aDirection = aRb.rotate(bDirection)
noise_aRa = gtsam.Rot3.Expmap(np.array([0.01, -0.02, 0.015]))
measured_aDirection = noise_aRa.rotate(exact_aDirection)
information = np.array(
    [[4.0, 1.0, 0.5], [1.0, 3.0, 0.25], [0.5, 0.25, 2.0]]
)
model = gtsam.noiseModel.Gaussian.Information(information)
factor = gtsam.WahbaFactor(
    key, bDirection, measured_aDirection, model
)
residual = factor.evaluateError(aRb)
values = gtsam.Values()
values.insert(key, aRb)
print("three-dimensional chordal residual:", residual)
print(f"weighted factor cost: {factor.error(values):.9f}")
assert residual.shape == (3,)
assert np.linalg.norm(residual) > 0.0

three-dimensional chordal residual: [ 0.00244182 -0.01033505 -0.01540794]
weighted factor cost: 0.000405312

Relation to `RotateDirectionsFactor`¶

RotateDirectionsFactor represents the same geometric correspondence with a two-dimensional tangent-space residual on Unit3. It therefore takes a two-dimensional noise model and defines a different nonlinear likelihood. WahbaFactor takes a three-dimensional noise model and minimizes Euclidean chord length. The two residuals both vanish at an exact correspondence, but they are not interchangeable away from it.

tangent_factor = gtsam.RotateDirectionsFactor(
    key, measured_aDirection, bDirection,
    gtsam.noiseModel.Unit.Create(2),
)
tangent_residual = tangent_factor.evaluateError(aRb)
print("WahbaFactor residual dimension:          ", residual.size)
print("RotateDirectionsFactor residual dimension:", tangent_residual.size)
assert tangent_residual.shape == (2,)

WahbaFactor residual dimension:           3
RotateDirectionsFactor residual dimension: 2

Exact D=1 QCQP conversion¶

With $x=[1;\operatorname{vec}(R^a_b)]$ , every whitened residual can be written as $Bx$ , so its squared norm is the exact quadratic form $\tfrac12x^{\mathsf T}B^{\mathsf T}Bx$ . QcqpProblem(graph, 1) combines that cost with the quadratic constraints defining Rot3. No measurement linearization is introduced.

The factor rejects matrix-valued QCQP column dimensions, null cost graphs, robust noise, and constrained noise because those cases do not represent the supported finite quadratic objective.

graph = gtsam.NonlinearFactorGraph()
graph.add(factor)
qcqp = gtsam.QcqpProblem(graph, 1)
qcqp_values = gtsam.Values()
gtsam.insertQcqpValueRot3(key, aRb, qcqp_values)
qcqp_cost, equality_violation, inequality_violation = qcqp.evaluate(
    qcqp_values
)
nonlinear_cost = graph.error(values)
print(f"nonlinear cost:          {nonlinear_cost:.9f}")
print(f"QCQP cost:               {qcqp_cost:.9f}")
print(f"equality violation:      {equality_violation:.3e}")
print(f"inequality violation:    {inequality_violation:.3e}")
assert np.isclose(qcqp_cost, nonlinear_cost, atol=1e-9)
assert equality_violation < 1e-10
assert inequality_violation < 1e-10

nonlinear cost:          0.000405312
QCQP cost:               0.000405312
equality violation:      2.282e-17
inequality violation:    0.000e+00

Known landmark factors apply the same frame-explicit correspondence idea to finite world landmarks and Pose2/Pose3 states.
Rotate factors documents RotateDirectionsFactor, the two-dimensional tangent-space alternative.
Frobenius factors provide related matrix-entry objectives for priors and relative transformations.
CertifiableWahbaProblem is the complete tutorial comparing local optimization with a monolithic SDP certificate.

Chordal direction alignment¶

Relation to RotateDirectionsFactor¶

Exact D=1 QCQP conversion¶

Related documentation¶

Relation to `RotateDirectionsFactor`¶