ShonanFactor - GTSAM Docs

Created by Codex.

Lift a relative Rot2 or Rot3 measurement into one level of the Shonan relaxation.

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

See LICENSE for the license information

Mathematical idea¶

At relaxation level $p$ , each variable $Q_i\in\mathrm{SO}(p)$ projects to its first $d$ columns, $Y_i=Q_i[:,1\!:\!d]$ . A relative rotation $R_{ij}\in\mathrm{SO}(d)$ gives the lifted residual

r_{ij}=\operatorname{vec}(Y_iR_{ij}-Y_j).

(1)

Here $\mathrm{SO}(p)$ is the special orthogonal group and the projected matrices $Y_i$ lie on a Stiefel manifold.

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¶

ShonanFactor<d> connects two SO(p) variables while measuring a rotation in SO(d). Projection onto the first d columns produces the Stiefel-manifold residual. GTSAM instantiates d=2 and d=3; the Python wrapper currently exposes ShonanFactor3.

factor = gtsam.ShonanFactor3(X(0), X(1), gtsam.Rot3(), 3)
Q1 = gtsam.SOn.FromMatrix(np.eye(3))
Q2 = gtsam.SOn.FromMatrix(np.eye(3))

print("zero-measurement residual:", factor.evaluateError(Q1, Q2))
print("factor dimension:", factor.dim())

zero-measurement residual: [0. 0. 0. 0. 0. 0. 0. 0. 0.]
factor dimension: 9

Practical notes¶

Most users should construct ShonanAveraging2 or ShonanAveraging3, which build these factors and manage the relaxation sequence. Use ShonanFactor directly for research on a fixed relaxation level p.

Source¶

ShonanFactor.h