MagPoseFactor - GTSAM Docs

Overview¶

The MagPoseFactor<POSE> is a templated factor designed to constrain the orientation component of a Pose2 or Pose3 variable using magnetometer readings. It’s similar in purpose to MagFactor1 but operates directly on pose types and explicitly handles an optional transformation between the body frame and the sensor frame.

It assumes the magnetometer calibration parameters (scale, bias) and the local magnetic field direction are known.

Measurement Model¶

The underlying model is the same as for MagFactor:

bM_{measured} = R_{bn} \cdot (s \cdot \hat{d}_n) + b

(1)

However, MagPoseFactor allows specifying an optional body_P_sensor transform. If provided, the factor internally adjusts the measurement and bias to be consistent with the body frame before calculating the error.

Let:

$T_{nb} = (R_{nb}, p_{nb})$ be the Pose state.
$T_{bs}$ be the pose of the sensor in the body frame (body_P_sensor). If not given, $T_{bs}$ is identity.
$sM_{measured}$ be the raw measurement in the sensor frame.
$sB$ be the bias in the sensor frame.
$s$ , $\hat{d}_n$ be the known scale and nav-frame direction.

The factor computes the predicted measurement in the body frame:

bM_{predicted} = R_{bs} [ R_{sn} (s \cdot \hat{d}_n) + sB ]

(2)

where $R_{sn} = R_{sb} R_{bn} = R_{bs}^T \cdot R_{nb}^T$ .

Alternatively, and perhaps more clearly, it predicts the field in the sensor frame and compares it to the measurement:

sM_{predicted} = R_{sn} (s \cdot \hat{d}_n) + sB

(3)

e = sM_{measured} - sM_{predicted}

(4)

The implementation transforms the measurement and bias to the body frame first for efficiency if body_P_sensor is provided. The error $e$ is calculated in the body frame.

Key Functionality / API¶

Template Parameter: POSE (must be gtsam.Pose2 or gtsam.Pose3).
Constructor: MagPoseFactor(poseKey, measured, scale, direction, bias, model, body_P_sensor=None)
- poseKey: Key of the Pose2 or Pose3 variable.
- measured: Measured magnetic field vector (Point2 or Point3) in the sensor frame.
- scale: Known scale factor.
- direction: Known magnetic field direction (Point2 or Point3) in the navigation frame.
- bias: Known bias vector (Point2 or Point3) in the sensor frame.
- model: Noise model (2D or 3D).
- body_P_sensor: Optional Pose2 or Pose3 describing the sensor’s pose relative to the body frame.
evaluateError(nPb): Calculates the error vector (2D or 3D) based on the current pose estimate nPb.

Usage Example (Pose3)¶

Using a magnetometer to help estimate a Pose3 orientation, assuming known calibration and a sensor offset.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import X

# --- Assumed Known Calibration & Field (NED Frame) ---
n_direction_point = gtsam.Point3(0.7, 0.1, 0.7) # Example simplified direction in NED
mag_scale = 50000.0 # nT
mag_bias_sensor = gtsam.Point3(15.0, 10.0, -5.0) # Bias in sensor frame (nT)

# --- Sensor Pose --- 
# Assume sensor is rotated 90 deg yaw right w.r.t body
body_P_sensor = gtsam.Pose3(gtsam.Rot3.Yaw(np.deg2rad(90)), gtsam.Point3(0.1, 0, 0))
sensor_P_body = body_P_sensor.inverse()

# --- Simulation: Generate Measurement ---
# Assume a ground truth body pose (e.g., 10 deg pitch up in NED)
truth_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(10)), gtsam.Point3(1,2,3))
truth_nRb = truth_nPb.rotation()
truth_bRn = truth_nRb.inverse()

# Calculate field in nav frame
n_field_vector = mag_scale * (n_direction_point / np.linalg.norm(n_direction_point))

# Calculate field in sensor frame 
truth_nRs = truth_nRb * body_P_sensor.rotation()
truth_sRn = truth_nRs.inverse()
s_field_ideal = truth_sRn.rotate(n_field_vector)

# Calculate the measured value including bias (in sensor frame)
s_measured = s_field_ideal + mag_bias_sensor

# --- Factor Creation ---
pose_key = X(0)

# Noise model for the magnetometer measurement (nT)
mag_noise_sigma = 50.0 # nT
noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)

# Create MagPoseFactor (providing body_P_sensor)
# Note: measurement and bias are in SENSOR frame when body_P_sensor is specified
mag_factor = gtsam.MagPoseFactorPose3(pose_key, s_measured, mag_scale, 
                                      n_direction_point, mag_bias_sensor, 
                                      noise_model, body_P_sensor)

print("Created MagPoseFactor<Pose3>:")
mag_factor.print()

# --- Evaluate Error ---
# Evaluate at the ground truth pose (error should be zero)
error_at_truth = mag_factor.evaluateError(truth_nPb)
print("\nError at ground truth pose (should be zero):", error_at_truth)

# Evaluate at a different pose (error should be non-zero)
test_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(15)), gtsam.Point3(1,2,3))
error_at_test = mag_factor.evaluateError(test_nPb)
print("Error at test pose:", error_at_test)

Created MagPoseFactor<Pose3>:
  keys = { x0 }
isotropic dim=3 sigma=50
local field (nM): [35176.3235; 5025.18908; 35176.3235];
measured field (bM): [28523.6117; 5040.18908; 40745.2206];
magnetometer bias: [-10; 15; -5];

Error at ground truth pose (should be zero): [0. 0. 0.]
Error at test pose: [-3660.19475195     0.          2331.80122523]

Important Considerations¶

Frame Consistency: Ensure direction is in the nav frame, while measured and bias are in the sensor frame when body_P_sensor is provided. If body_P_sensor is None, then measured and bias are assumed to be in the body frame.
Units: Maintain consistent units (e.g., nT) for scale, bias, measurement, and noise sigma.

Source¶

MagPoseFactor.h