TranslationFactor - GTSAM Docs

Created by Codex.

Constrain two 3D positions with a measured translation direction using chordal error.

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

See LICENSE for the license information

Mathematical idea¶

For positions $T_a,T_b\in\mathbb R^3$ and a measured unit direction $u_{ab}$ , the chordal residual is

r_{ab}=\frac{T_b-T_a}{\lVert T_b-T_a\rVert}-u_{ab}.

(1)

This compares directions in the ambient three-dimensional space. It is invariant to global translation and positive global scale, so those gauge freedoms must be fixed elsewhere.

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

Model and availability¶

The prediction is normalize(T_b - T_a) and the three-dimensional residual subtracts the measured Unit3 direction in ambient coordinates. Direction data cannot determine global translation or scale, so a usable graph needs gauge constraints.

The class is currently C++-only, but Python users can construct it indirectly with LocationRecovery.buildGraph(edges, bilinear=False) or TranslationRecovery in its default mode.

C++ example¶

#include <gtsam/sfm/TranslationFactor.h>

using namespace gtsam;

const auto noise = noiseModel::Isotropic::Sigma(3, 0.01);
TranslationFactor factor(Symbol('t', 0), Symbol('t', 1),
                         Unit3(1.0, 0.0, 0.0), noise);

const Vector3 residual = factor.evaluateError(Point3(0, 0, 0),
                                              Point3(2, 0, 0));
// residual is zero: only direction, not the distance 2, is measured.

Do not evaluate at coincident points: normalizing a zero baseline is undefined.

Source¶

TranslationFactor.h