TranslationRecovery - GTSAM Docs

Created by Codex.

Recover camera translations from world-frame pairwise directions while fixing translation and scale gauge.

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 gtsam
import numpy as np

from gtsam import symbol_shorthand

C = symbol_shorthand.C
K = symbol_shorthand.K
P = symbol_shorthand.P
S = symbol_shorthand.S
X = symbol_shorthand.X

Mathematical idea¶

Translation recovery minimizes direction residuals over positions $T_i$ . With the Bilinear Angle-based Translation Averaging (BATA) option, each edge contributes

r_{ij}=|s_{ij}|(T_j-T_i)-u_{ij};

(1)

the chordal option instead normalizes $T_j-T_i$ . Fixing one position removes the translation gauge, while a known baseline or metric measurement fixes global scale.

Workflow¶

TranslationRecovery builds direction factors, pins the first edge’s first endpoint to the origin, and fixes scale either from the requested first-edge scale or from supplied metric BinaryMeasurementPoint3 constraints. Zero-direction edges merge same-location nodes before optimization.

direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.01)
edges = [
    gtsam.BinaryMeasurementUnit3(X(0), X(1), gtsam.Unit3(np.array([1.0, 0.0, 0.0])), direction_noise),
    gtsam.BinaryMeasurementUnit3(X(1), X(2), gtsam.Unit3(np.array([0.0, 1.0, 0.0])), direction_noise),
    gtsam.BinaryMeasurementUnit3(X(0), X(2), gtsam.Unit3(np.array([1.0, 1.0, 0.0])), direction_noise),
]

recovery = gtsam.TranslationRecovery()
result = recovery.run(edges, scale=1.0)

for key in (X(0), X(1), X(2)):
    print(key, result.atPoint3(key))

8646911284551352320 [ 2.58209087e-22 -1.71203466e-22 -1.33103579e-16]
8646911284551352321 [ 1.00000000e+00 -9.23930048e-23  1.33103898e-16]
8646911284551352322 [1.00000000e+00 1.00000000e+00 1.35764717e-15]

Choosing the residual¶

The default constructor uses chordal TranslationFactors. Construct with TranslationRecovery(lm_params, True) to use bilinear BATA factors. Supply metric between translations when available; they give scale a direct physical meaning rather than defining it from the first direction edge.

Source¶

TranslationRecovery.h