TrajectoryAlignerSim3

Created by Codex.

Align one or more child pose trajectories to a parent trajectory with a similarity transform.

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¶

A three-dimensional similarity transform $S=(s,R,t)\in\mathrm{Sim}(3)$ maps a child position $p_k^c$ to its parent frame by

p_k^p\approx sRp_k^c+t.

(1)

$\mathrm{Sim}(3)$ denotes rotation, translation, and uniform scale. Pose orientation residuals additionally compare the rotated child orientation with the parent orientation. Graduated Non-Convexity (GNC) can down-weight mismatched correspondences.

Model¶

Parent and child poses are matched by key. Each child trajectory gets one Similarity3 variable under key S(child_index). The optimizer can initialize it from overlapping poses, accept an explicit initial similarity, use Graduated Non-Convexity (GNC) for robustness, and optionally add overlapping 3D-point pairs.

pose_noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.01)

def pose(x):
    return gtsam.Pose3(gtsam.Rot3(), np.array([x, 0.0, 0.0]))

parent = [gtsam.UnaryMeasurementPose3(X(i), pose(float(i)), pose_noise)
          for i in range(3)]
child = [gtsam.UnaryMeasurementPose3(X(i), pose(float(i)), pose_noise)
         for i in range(3)]

aligner = gtsam.TrajectoryAlignerSim3(parent, [child])
solution = aligner.solve()
similarity_key = gtsam.Symbol('S', 0).key()
alignment = solution.atSimilarity3(similarity_key)

print("estimated scale:", alignment.scale())
print("estimated rotation:")
print(alignment.rotation().matrix())

estimated scale: 1.0
estimated rotation:
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]

Practical notes¶

At least two spatially distinct overlapping poses are needed to infer scale; richer nondegenerate motion is preferable.
Set use_gnc_optimizer=True when overlap associations may contain outliers.
marginalize(solution) computes uncertainty for the constructed alignment graph after solving.

Source¶

TrajectoryAlignerSim3.h