AttitudeFactor - GTSAM Docs

Introduction¶

The AttitudeFactor family in GTSAM provides factors that constrain the orientation (attitude) of a body (Rot3 or Pose3) based on directional measurements. A common use case is constraining roll and pitch using an accelerometer (measuring gravity) or constraining yaw using a magnetometer (measuring the Earth’s magnetic field).

This notebook explains the mathematical foundation and usage of these factors.

%pip install --quiet gtsam-develop

ERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)
ERROR: No matching distribution found for gtsam-develop
Note: you may need to restart the kernel to use updated packages.

Mathematical Foundation¶

Concept¶

The AttitudeFactor constrains the rotation $R_{nb}$ (from the body frame $b$ to the navigation frame $n$ ) such that a known reference direction in the navigation frame (nRef) aligns with a measured direction in the body frame (bMeasured), when rotated by $R_{nb}$ .

The factor enforces that:

\text{nRef} \approx R_{nb} \cdot \text{bMeasured}

(1)

where:

$R_{nb}$ is the rotation matrix representing the orientation from body to navigation frame.
bMeasured is the direction vector measured by the sensor in the body frame (e.g., the accelerometer reading, typically normalized to a Unit3).
nRef is the known direction vector of the corresponding physical quantity in the navigation frame (e.g., the gravity vector, typically normalized to a Unit3).

Error Function¶

The error function computes the angular difference between the reference direction (nRef) and the rotated measured direction ( $R_{nb} \cdot \text{bMeasured}$ ). GTSAM uses the Unit3::error method, which typically calculates a 2-dimensional tangent space error.

e = \text{nRef}.\text{error}(R_{nb} \cdot \text{bMeasured})

(2)

This error is minimal (zero) when the rotated body measurement aligns perfectly with the navigation reference direction.

The attitudeError function within the base class implements this:

Vector AttitudeFactor::attitudeError(const Rot3& nRb) const {
  // measured direction in body frame rotated into nav frame
  Unit3 nPredicted = nRb * bMeasured_;
  // error between predicted and reference direction
  return nRef_.error(nPredicted); 
}

Jacobians¶

For optimization, the $2 \times 3$ Jacobian of the error function with respect to the rotation parameters ( $R_{nb}$ ) is required. This is computed using the chain rule, involving the derivative of the rotated vector and the derivative of the Unit3::error function.

Note: The Jacobian for this specific error function can become zero or ill-defined when the angle between the predicted and reference directions is exactly 180 degrees. The factor behaves best when the initial estimate for $R_{nb}$ is reasonably close (i.e., within the correct hemisphere).

Available Factors¶

Rot3AttitudeFactor: Constrains a Rot3 variable.
Pose3AttitudeFactor: Constrains the rotational part of a Pose3 variable.

Usage Example¶

Let’s constrain the roll and pitch of a Pose3 using an accelerometer reading. We assume an ENU navigation frame (Z is up) and the accelerometer measures gravity when stationary.

import numpy as np
from gtsam import Pose3, Unit3, Rot3, Pose3AttitudeFactor
from gtsam.symbol_shorthand import X
from gtsam.noiseModel import Isotropic

# Define the reference direction in the navigation (ENU) frame
# Gravity points along the negative Z axis in ENU.
nRef_gravity = Unit3(np.array([0.0, 0.0, -1.0]))

# Define the measured direction in the body frame
# Assume the accelerometer reading is [0.1, 0.0, -9.8]. 
# GTSAM's Unit3 constructor normalizes this automatically.
# The factor expects the *direction* the sensor measures in the *body* frame.
bMeasured_acc = Unit3(np.array([0.1, 0.0, -9.8]))

# Define the noise model for the measurement (2-dimensional error)
# Example: 0.1 radians standard deviation on the tangent plane error
attitude_noise_sigma = 0.1
noise_model = Isotropic.Sigma(2, attitude_noise_sigma)

# Create the factor
attitude_factor = Pose3AttitudeFactor(X(0), nRef_gravity, 
                                      noise_model, bMeasured_acc)

print("Created Pose3AttitudeFactor:")
attitude_factor.print()

# Example: Evaluate the error at the identity pose (likely non-zero error)
identity_pose = Pose3()
error = attitude_factor.evaluateError(identity_pose)
print("\nError at identity pose:", error)

# For zero error, the rotated measurement should align with the reference
# nRef = R_nb * bMeas => R_nb = nRef * bMeas.inverse() (approx for Unit3)
# This is complex to solve directly, but optimization finds the R_nb where error is zero.
# Let's try a pose that should roughly align Z_body with Z_nav (small roll)
zero_error_pose = Pose3(Rot3.Roll(-0.01), np.zeros(3)) # Approx -0.1/9.8 rad
error_near_zero = attitude_factor.evaluateError(zero_error_pose)
print("Error near expected zero pose:", error_near_zero)

Created Pose3AttitudeFactor:
Pose3AttitudeFactor on x0
  reference direction in nav frame: : 0
 0
-1
  measured direction in body frame: :0.0102036
        0
-0.999948
isotropic dim=2 sigma=0.1

Error at identity pose: [ 0.         -0.01020355]
Error near expected zero pose: [ 0.00999931 -0.01020355]

Conclusion¶

The AttitudeFactor is a crucial tool for incorporating absolute orientation information from sensors like accelerometers and magnetometers into GTSAM factor graphs. It helps constrain orientation estimates, particularly roll and pitch (using gravity) and potentially yaw (using magnetic north), improving navigation accuracy, especially in GPS-denied scenarios.

Source¶