Simple Rotation - GTSAM Docs

This is the smallest possible GTSAM optimization: a single variable, a single factor. The variable is a 2D rotation (Rot2); the factor is a prior -- a measurement from a sensor, with a noise model describing how much we trust it.

The story: a sensor measured a rotation to be close to 30 degrees. We start from a wrong initial guess of 20 degrees, and ask GTSAM to find the best estimate.

Authors: Frank Dellaert, Alex Cunningham, 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 numpy as np
import gtsam
from gtsam.symbol_shorthand import X

1. Keys¶

GPSFactorExample and OdometryExample label variables with plain integer keys (1, 2, 3, ...). Here we instead use symbol_shorthand.X, which packs a character ('x') and an index into the same underlying Key type -- a more readable choice once a problem has several kinds of variables (poses, landmarks, calibration, ...) that would otherwise collide under a single integer counter.

key = X(1)

2. Create the prior factor¶

In general, creating a factor requires:

a key or set of keys labeling the variables it acts on,
a measurement value, and
a measurement model with the correct dimensionality.

Here the “measurement” is the goal angle of 30 degrees, and the noise model says we trust it to within about 1 degree (Isotropic.Sigma since a Rot2 has a single degree of freedom).

prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
print(prior)
model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
factor = gtsam.PriorFactorRot2(key, prior, model)

theta: 0.523599

3. Build the graph¶

Before optimizing, every factor needs to be added to a graph container. In a practical problem many factors would be added; here there is exactly one.

graph = gtsam.NonlinearFactorGraph()
graph.push_back(factor)
print(graph)

NonlinearFactorGraph: size: 1

Factor 0: PriorFactor on x1
  prior mean: : 0.523599
isotropic dim=1 sigma=0.0174533

4. Initial estimate¶

Optimization needs a starting linearization point for every variable in the graph. We deliberately start 10 degrees away from the prior’s goal angle, so the optimizer has something to do.

initial = gtsam.Values()
initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
print(initial)

Values with 1 values:
Value x1: (gtsam::Rot2)
: 0.349066

5. Optimize¶

As in GPSFactorExample and OdometryExample, we solve with LevenbergMarquardtOptimizer.

result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
print(result)

Values with 1 values:
Value x1: (gtsam::Rot2)
: 0.523599

With only one factor in the graph, there is nothing else to compromise against, so the optimizer converges exactly to the prior’s goal angle of 30 degrees -- unlike the GPS factor notebook, where two competing factors (a prior and a GPS measurement) pulled the result to a noise-weighted blend between them. A single-factor graph like this one just returns that factor’s own measurement.