ShonanAveraging2 - GTSAM Docs

Created by Codex.

Solve and optionally certify planar 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 planar rotations $R_i\in\mathrm{SO}(2)$ , the chordal rotation-averaging objective is

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

(1)

$\mathrm{SO}(2)$ is the two-dimensional special orthogonal group. The Shonan staircase lifts these rotations to $\mathrm{SO}(p)$ , increasing $p$ only when the current solution cannot yet be certified.

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 planar interface accepts BetweenFactorPose2 measurements or a two-dimensional general graph optimization (g2o) file. Only the rotational component contributes to rotation averaging; factor noise supplies measurement confidence.

noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
factors = [
    gtsam.BetweenFactorPose2(0, 1, gtsam.Pose2(1.0, 0.0, 0.2), noise),
    gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(1.0, 0.0, -0.1), noise),
    gtsam.BetweenFactorPose2(0, 2, gtsam.Pose2(2.0, 0.0, 0.1), noise),
]
parameters = gtsam.ShonanAveragingParameters2(gtsam.LevenbergMarquardtParams())
shonan = gtsam.ShonanAveraging2(factors, parameters)

rotations, certificate = shonan.run(2, 5)
print("measurements:", shonan.numberMeasurements())
print("certificate value:", certificate)
print("R(2) angle:", rotations.atRot2(2).theta())

measurements: 3
certificate value: -9.094947017729282e-13
R(2) angle: 3.141592653589793

Interpreting the result¶

The returned Values contains Rot2 estimates at the original keys. The accompanying scalar is the certificate quantity reported by the staircase. Keep max_p modest initially and increase it if certification is not reached.

Source¶

ShonanAveraging.h