RelativeTranslationFactor

RelativeTranslationFactor<d> is the translational half of a relative pose measurement, written so that it stays quadratic in every variable it touches. It is the piece that lets certifiable $SE(d)$ synchronization reuse the same Burer-Monteiro machinery as rotation averaging.

A standard BetweenFactor<Pose3> couples rotation and translation through a nonlinear residual. Splitting the pose into a rotation key and a translation key, and keeping the translation residual in the ambient linear form, gives a cost that the QCQP lowering can express exactly.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import numpy as np

import gtsam
from gtsam.symbol_shorthand import R, T

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

Residual¶

For an edge $i \to j$ with measured body-frame offset $\tilde{t}_{ij}$ and translational precision $\tau_{ij}$ , the factor contributes

\tau_{ij}\,\bigl\| t_j - t_i - R_i\,\tilde{t}_{ij} \bigr\|^2 .

(1)

evaluateError returns the weighted residual $\sqrt{\tau_{ij}}\,(t_j - t_i - R_i \tilde{t}_{ij})$ under a unit noise model, so the squared residual reproduces the term above. The factor is quadratic in $R_i$ , $t_i$ , and $t_j$ jointly: $R_i$ enters linearly through $R_i \tilde{t}_{ij}$ , and the translations enter linearly. That is exactly the structure a QCQP needs.

Together with FrobeniusBetweenFactor<Rot3> on the same edge, the pair reproduces the usual $SE(d)$ synchronization objective

\sum_{(i,j)} \kappa_{ij}\,\|R_j - R_i R_{ij}\|_F^2 \;+\; \tau_{ij}\,\|t_j - t_i - R_i \tilde{t}_{ij}\|^2 .

(2)

measured = np.array([0.4, -0.2, 0.7])
weight = 2.5
factor = gtsam.RelativeTranslationFactor3(R(0), T(0), T(1), measured, weight)

rotation = gtsam.Rot3.RzRyRx(0.2, -0.1, 0.3)
ti = np.array([1.0, 2.0, 3.0])
tj = np.array([1.5, 1.0, 3.25])

values = gtsam.Values()
values.insert(R(0), rotation)
values.insert(T(0), gtsam.Point3(ti))
values.insert(T(1), gtsam.Point3(tj))

print("residual:", factor.unwhitenedError(values))
print("by hand: ", np.sqrt(weight) * (tj - ti - rotation.matrix() @ measured))

residual: [ 0.1303 -1.2308 -0.6847]
by hand:  [ 0.1303 -1.2308 -0.6847]

A measurement generated from two consistent poses gives an exactly zero residual, which is the sanity check worth running whenever the body-frame convention is in doubt: $\tilde{t}_{ij} = R_i^\top (t_j - t_i)$ .

pose_i = gtsam.Pose3(gtsam.Rot3.RzRyRx(0.1, 0.2, -0.3), np.array([0.5, -1.0, 2.0]))
pose_j = gtsam.Pose3(gtsam.Rot3.RzRyRx(-0.2, 0.4, 0.1), np.array([1.5, 0.5, 1.0]))
consistent = pose_i.rotation().unrotate(pose_j.translation() - pose_i.translation())

exact = gtsam.RelativeTranslationFactor3(R(0), T(0), T(1), consistent, 1.0)
consistent_values = gtsam.Values()
consistent_values.insert(R(0), pose_i.rotation())
consistent_values.insert(T(0), gtsam.Point3(pose_i.translation()))
consistent_values.insert(T(1), gtsam.Point3(pose_j.translation()))

print("residual at the generating poses:", exact.unwhitenedError(consistent_values))

residual at the generating poses: [0. 0. 0.]

QCQP lowering¶

qcqpFactors rewrites the term as a single quadratic cost $\langle Q, XX^\top\rangle$ over the stacked row-space variable. With $\sqrt{\tau}$ folded in, the row vector

B = \bigl[\,-\sqrt{\tau}\,\tilde{t}_{ij}^\top \;\big|\; -\sqrt{\tau} \;\big|\; \sqrt{\tau}\,\bigr]

(3)

spans the rotation block, pose $i$ ’s translation, and pose $j$ ’s translation, and the cost matrix is $Q = B^\top B$ with block dimensions $\{d, 1, 1\}$ . The rotation contributes $d$ rows to the lifted variable and each translation contributes a single row.

The factor emits no constraints: translations are unconstrained in $SE(d)$ synchronization, and the orthogonality constraints on $R_i$ come from the rotation factors on the same edge.

graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.RelativeTranslationFactor3(R(0), T(0), T(1), measured, weight))

# Lower to a rank-3 QCQP: one quadratic cost, no equality or inequality constraints.
problem = gtsam.QcqpProblem(graph, 3)
print("costs, equalities, inequalities:", problem.dim())

# Lifted variables: the rotation block is d-by-p, each translation is 1-by-p.
lifted = gtsam.Values()
lifted.insert(R(0), rotation.matrix().T)
lifted.insert(T(0), ti.reshape(1, 3))
lifted.insert(T(1), tj.reshape(1, 3))
print("cost, equality violation, inequality violation:", problem.evaluate(lifted))

residual = factor.unwhitenedError(values)
print("0.5 * squared residual:", 0.5 * residual @ residual)

costs, equalities, inequalities: (1, 0, 0)
cost, equality violation, inequality violation: (1.0003141852358166, 0.0, 0.0)
0.5 * squared residual: 1.0003141852358168

API¶

C++	Python	Rotation type
`RelativeTranslationFactor<2>`	`RelativeTranslationFactor2`	`Rot2`
`RelativeTranslationFactor<3>`	`RelativeTranslationFactor3`	`Rot3`

The constructor takes (rotationKey, translationKey1, translationKey2, measured, weight), where weight is the translational precision $\tau$ and must be positive.

Practical notes¶

The factor is meant to be paired with a rotation factor on the same edge, keyed so that the rotation key is shared. When converting an anisotropic BetweenFactor<Pose3> covariance, reduce it to two isotropic precisions: $\kappa$ from the rotational block and $\tau$ from the translational block. qcqpFactors requires the staircase column dimension $p$ to be at least $d$ .

For the full certifiable pipeline built on this factor, see CertifiablePoseGraphOptimizationPose2 and CertifiablePoseGraphOptimizationPose3.