ShonanAveraging3 - GTSAM Docs

Created by Codex.

Solve and optionally certify spatial rotation averaging through the Shonan staircase.

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

See LICENSE for the license information

Mathematical idea¶

For spatial rotations $R_i\in\mathrm{SO}(3)$ , Shonan averaging minimizes weighted relative-rotation disagreement,

\min_{R_i}\sum_{(i,j)}w_{ij}\left\lVert R_iR_{ij}-R_j\right\rVert_F^2,

(1)

then lifts to $\mathrm{SO}(p)$ levels when necessary for certification. $\mathrm{SO}(3)$ is the three-dimensional special orthogonal group; one global rotation remains unobservable unless an anchor fixes the world frame.

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

Inputs¶

The 3D interface accepts BinaryMeasurementRot3, BetweenFactorPose3, or a three-dimensional general graph optimization (g2o) file. Measurements must follow the ordered edge convention: R_ij maps from key i to key j under GTSAM’s between-measurement convention.

rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)
measurements = [
    gtsam.BinaryMeasurementRot3(0, 1, gtsam.Rot3.RzRyRx(0.1, 0.0, 0.0), rotation_noise),
    gtsam.BinaryMeasurementRot3(1, 2, gtsam.Rot3.RzRyRx(0.0, -0.1, 0.0), rotation_noise),
    gtsam.BinaryMeasurementRot3(0, 2, gtsam.Rot3.RzRyRx(0.1, -0.1, 0.0), rotation_noise),
]
parameters = gtsam.ShonanAveragingParameters3(gtsam.LevenbergMarquardtParams())
shonan = gtsam.ShonanAveraging3(measurements, parameters)

rotations, certificate = shonan.run(3, 6)
print("measurements:", shonan.numberMeasurements())
print("certificate value:", certificate)
print("R(2):")
print(rotations.atRot3(2).matrix())

measurements: 3
certificate value: 0.0
R(2):
[[-1.  0.  0.]
 [ 0. -1.  0.]
 [ 0.  0.  1.]]

Workflow¶

Inspect residuals and connectivity before running the staircase. Rotation averaging has one global rotation gauge; set an anchor in ShonanAveragingParameters3 if the output must align with a chosen world frame.

Source¶

ShonanAveraging.h