GlobalPositioner - GTSAM Docs

Created by Codex.

Jointly estimate camera and landmark positions with the Bilinear Angle-based Translation Averaging (BATA) model used by GLOMAP (Global Structure-from-Motion Revisited).

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

See LICENSE for the license information

Mathematical idea¶

For camera position $C_i$ , landmark position $P_j$ , and world-frame unit bearing $u_{ij}$ , each observation contributes the BATA residual

r_{ij}=|s_{ij}|(P_j-C_i)-u_{ij}.

(1)

One scale $s_{ij}$ is optimized per observation. BATA means Bilinear Angle-based Translation Averaging; see Zhuang, Cheong, and Lee, Baseline Desensitizing in Translation Averaging, IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2018. Direction-only data leaves global translation and scale gauges, so anchoring and metric information remain essential.

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

Problem structure¶

GlobalPositioner specializes LocationRecovery for a bipartite camera-landmark graph. It always uses bilinear BATA factors, validates that the anchor is a camera, initializes one scale per observation, and runs Levenberg-Marquardt.

The input directions must point from the camera key (key1) to the landmark key (key2) in a common world frame.

direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.01)
camera_keys = {C(0), C(1)}
landmark_keys = {P(0), P(1)}
directions = [
    gtsam.BinaryMeasurementUnit3(C(0), P(0), gtsam.Unit3(np.array([0.0, 0.0, 1.0])), direction_noise),
    gtsam.BinaryMeasurementUnit3(C(0), P(1), gtsam.Unit3(np.array([1.0, 0.0, 1.0])), direction_noise),
    gtsam.BinaryMeasurementUnit3(C(1), P(0), gtsam.Unit3(np.array([-1.0, 0.0, 1.0])), direction_noise),
    gtsam.BinaryMeasurementUnit3(C(1), P(1), gtsam.Unit3(np.array([0.0, 0.0, 1.0])), direction_noise),
]

positioner = gtsam.GlobalPositioner()
initial = positioner.initializeRandomly(camera_keys, landmark_keys, directions)
graph = positioner.buildGraph(directions, bilinear=True)
positioner.addAnchorPrior(C(0), graph)

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

measurement factors plus anchor: 5
initial keys: [5980780305148018688, 5980780305148018689, 5980780305148018690, 5980780305148018691, 7133701809754865664, 7133701809754865665, 8070450532247928832, 8070450532247928833]

Running a real problem¶

Call positioner.run(directions, camera_keys, landmark_keys, anchor_camera, initial) once the graph is well connected and the initialization contains any available metric information. A direction-only problem retains a global scale ambiguity; in production, seed or constrain scale through the surrounding reconstruction pipeline and check rank if LM reports an indeterminate system.

Source¶

GlobalPositioner.h