LocationRecovery - GTSAM Docs

Created by Codex.

Build generic location-recovery graphs from pairwise direction measurements.

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

See LICENSE for the license information

Mathematical idea¶

Given a measured direction $u_{ab}$ , location recovery can use either the normalized chordal residual

r_{ab}^{\mathrm{chordal}}=\frac{T_b-T_a}{\lVert T_b-T_a\rVert}-u_{ab},

(1)

or the Bilinear Angle-based Translation Averaging (BATA) residual $r_{ab}^{\mathrm{BATA}}=|s_{ab}|(T_b-T_a)-u_{ab}$ . The latter adds one scale variable per edge. Both models require gauge constraints because directions alone determine neither global translation nor metric scale.

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

Role¶

LocationRecovery is the unopinionated base for direction-based position estimation. Edges may connect any Point3 variables. bilinear=False creates chordal TranslationFactors; bilinear=True creates BATA factors plus one scale variable per edge.

The class builds and initializes graphs but deliberately does not choose a complete gauge beyond the anchor helper.

direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.02)
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),
]

recovery = gtsam.LocationRecovery()
graph = recovery.buildGraph(edges, bilinear=True)
recovery.addAnchorPrior(X(0), graph)
initial = recovery.initializeRandomly({X(0), X(1), X(2)}, len(edges), True)

print("graph factors:", graph.size())
print("initial keys:", initial.keys())

graph factors: 3
initial keys: [5980780305148018688, 5980780305148018689, 8646911284551352320, 8646911284551352321, 8646911284551352322]

Gauge warning¶

An anchor removes global translation but direction-only geometry also has a scale gauge. Add a physically meaningful distance, position, or scale constraint before optimizing. TranslationRecovery supplies a two-key gauge policy; GlobalPositioner supplies a camera/landmark-specific workflow.

Source¶

LocationRecovery.h