ShonanAveraging - GTSAM Docs

Created by Codex.

Implement the common staircase relaxation and optimality-certificate machinery for Shonan rotation averaging.

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¶

Rotation averaging seeks absolute rotations whose relative rotations agree with measurements:

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

(1)

$\mathrm{SO}(d)$ is the special orthogonal group. Shonan averaging lifts the problem from $\mathrm{SO}(d)$ to successively larger $\mathrm{SO}(p)$ manifolds, searches for a certifiable optimum, and rounds the lifted solution back to dimension $d$ .

Purpose and availability¶

ShonanAveraging<d> is the templated C++ implementation shared by the concrete ShonanAveraging2 and ShonanAveraging3 interfaces. It builds and optimizes problems on increasing SO(p) levels, checks the minimum eigenvalue certificate, descends through a negative-curvature direction when needed, and rounds a lifted solution back to rotations.

Use the concrete classes in application code. The base template itself is not directly exposed in Python.

Configure the staircase¶

Python exposes dimension-specific parameter classes. Both wrap ordinary LevenbergMarquardtParams and add anchoring, robust-loss, and optimality-certificate controls. Robust loss and optimality certification are alternative modes: disable Huber loss before requesting a certificate.

lm = gtsam.LevenbergMarquardtParams()
lm.setMaxIterations(100)

parameters2 = gtsam.ShonanAveragingParameters2(lm)
parameters3 = gtsam.ShonanAveragingParameters3(lm)
parameters3.setAnchor(0, gtsam.Rot3())
parameters3.setUseHuber(True)
assert parameters3.getAnchor()[0] == 0
assert parameters3.getUseHuber()
parameters3.setUseHuber(False)
parameters3.setCertifyOptimality(True)
print("3D certification enabled:", parameters3.getCertifyOptimality())

Planar rotation averaging¶

ShonanAveraging2 accepts planar pose-between factors and returns Rot2 values at the original keys. Only the rotational part of each pose measurement contributes to the objective.

pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
planar_measurements = [
    gtsam.BetweenFactorPose2(0, 1, gtsam.Pose2(1.0, 0.0, 0.2), pose_noise),
    gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(1.0, 0.0, -0.1), pose_noise),
    gtsam.BetweenFactorPose2(0, 2, gtsam.Pose2(2.0, 0.0, 0.1), pose_noise),
]
shonan2 = gtsam.ShonanAveraging2(planar_measurements, parameters2)
rotations2, certificate2 = shonan2.run(2, 5)

assert shonan2.numberMeasurements() == 3
print("R(2) angle:", rotations2.atRot2(2).theta())
print("2D certificate:", certificate2)

Spatial rotation averaging¶

ShonanAveraging3 accepts BinaryMeasurementRot3 or BetweenFactorPose3 measurements. The returned Values contains Rot3 estimates; the scalar reports the staircase certificate quantity.

rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)
spatial_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
    ),
]
shonan3 = gtsam.ShonanAveraging3(spatial_measurements, parameters3)
rotations3, certificate3 = shonan3.run(3, 6)

assert shonan3.numberMeasurements() == 3
print("R(2):\n", rotations3.atRot3(2).matrix())
print("3D certificate:", certificate3)

Source¶

ShonanAveraging.h