diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index ea0b30c75..f2bdc2ba4 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -77,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN { const Point& direction, const Point& bias, const SharedNoiseModel& model, - const std::optional& body_P_sensor) + const std::optional& body_P_sensor = {}) : Base(model, pose_key), measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured), nM_(scale * direction.normalized()), diff --git a/gtsam/navigation/doc/MagPoseFactor.ipynb b/gtsam/navigation/doc/MagPoseFactor.ipynb new file mode 100644 index 000000000..5ef0b8132 --- /dev/null +++ b/gtsam/navigation/doc/MagPoseFactor.ipynb @@ -0,0 +1,205 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# MagPoseFactor\n", + "\n", + "\"Open\n", + "\n", + "## Overview\n", + "\n", + "The `MagPoseFactor` 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.\n", + "\n", + "It assumes the magnetometer calibration parameters (scale, bias) and the local magnetic field direction are known." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Measurement Model\n", + "\n", + "The underlying model is the same as for `MagFactor`:\n", + "$$ bM_{measured} = R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b $$ \n", + "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.\n", + "\n", + "Let:\n", + "- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose` state.\n", + "- $T_{bs}$ be the pose of the sensor in the body frame (`body_P_sensor`). If not given, $T_{bs}$ is identity.\n", + "- $sM_{measured}$ be the raw measurement in the sensor frame.\n", + "- $sB$ be the bias in the sensor frame.\n", + "- $s$, $\\hat{d}_n$ be the known scale and nav-frame direction.\n", + "\n", + "The factor computes the predicted measurement *in the body frame*:\n", + "$$ bM_{predicted} = R_{bs} [ R_{sn} (s \\cdot \\hat{d}_n) + sB ] $$ \n", + "where $R_{sn} = R_{sb} R_{bn} = R_{bs}^T \\cdot R_{nb}^T$.\n", + "\n", + "Alternatively, and perhaps more clearly, it predicts the field in the sensor frame and compares it to the measurement:\n", + "$$ sM_{predicted} = R_{sn} (s \\cdot \\hat{d}_n) + sB $$ \n", + "$$ e = sM_{measured} - sM_{predicted} $$ \n", + "The implementation transforms the measurement and bias to the body frame first for efficiency if `body_P_sensor` is provided.\n", + "The error $e$ is calculated in the body frame." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Key Functionality / API\n", + "\n", + "- **Template Parameter**: `POSE` (must be `gtsam.Pose2` or `gtsam.Pose3`).\n", + "- **Constructor**: `MagPoseFactor(poseKey, measured, scale, direction, bias, model, body_P_sensor=None)`\n", + " - `poseKey`: Key of the `Pose2` or `Pose3` variable.\n", + " - `measured`: Measured magnetic field vector (`Point2` or `Point3`) **in the sensor frame**.\n", + " - `scale`: Known scale factor.\n", + " - `direction`: Known magnetic field direction (`Point2` or `Point3`) **in the navigation frame**.\n", + " - `bias`: Known bias vector (`Point2` or `Point3`) **in the sensor frame**.\n", + " - `model`: Noise model (2D or 3D).\n", + " - `body_P_sensor`: Optional `Pose2` or `Pose3` describing the sensor's pose relative to the body frame.\n", + "- **`evaluateError(nPb)`**: Calculates the error vector (2D or 3D) based on the current pose estimate `nPb`." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Usage Example (Pose3)\n", + "\n", + "Using a magnetometer to help estimate a `Pose3` orientation, assuming known calibration and a sensor offset." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Created MagPoseFactor:\n", + " keys = { x0 }\n", + "isotropic dim=3 sigma=50\n", + "local field (nM): [35176.3235; 5025.18908; 35176.3235];\n", + "measured field (bM): [28523.6117; 5040.18908; 40745.2206];\n", + "magnetometer bias: [-10; 15; -5];\n", + "\n", + "Error at ground truth pose (should be zero): [0. 0. 0.]\n", + "Error at test pose: [-3660.19475195 0. 2331.80122523]\n" + ] + } + ], + "source": [ + "import gtsam\n", + "import numpy as np\n", + "from gtsam.symbol_shorthand import X\n", + "\n", + "# --- Assumed Known Calibration & Field (NED Frame) ---\n", + "n_direction_point = gtsam.Point3(0.7, 0.1, 0.7) # Example simplified direction in NED\n", + "mag_scale = 50000.0 # nT\n", + "mag_bias_sensor = gtsam.Point3(15.0, 10.0, -5.0) # Bias in sensor frame (nT)\n", + "\n", + "# --- Sensor Pose --- \n", + "# Assume sensor is rotated 90 deg yaw right w.r.t body\n", + "body_P_sensor = gtsam.Pose3(gtsam.Rot3.Yaw(np.deg2rad(90)), gtsam.Point3(0.1, 0, 0))\n", + "sensor_P_body = body_P_sensor.inverse()\n", + "\n", + "# --- Simulation: Generate Measurement ---\n", + "# Assume a ground truth body pose (e.g., 10 deg pitch up in NED)\n", + "truth_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(10)), gtsam.Point3(1,2,3))\n", + "truth_nRb = truth_nPb.rotation()\n", + "truth_bRn = truth_nRb.inverse()\n", + "\n", + "# Calculate field in nav frame\n", + "n_field_vector = mag_scale * (n_direction_point / np.linalg.norm(n_direction_point))\n", + "\n", + "# Calculate field in sensor frame \n", + "truth_nRs = truth_nRb * body_P_sensor.rotation()\n", + "truth_sRn = truth_nRs.inverse()\n", + "s_field_ideal = truth_sRn.rotate(n_field_vector)\n", + "\n", + "# Calculate the measured value including bias (in sensor frame)\n", + "s_measured = s_field_ideal + mag_bias_sensor\n", + "\n", + "# --- Factor Creation ---\n", + "pose_key = X(0)\n", + "\n", + "# Noise model for the magnetometer measurement (nT)\n", + "mag_noise_sigma = 50.0 # nT\n", + "noise_model = gtsam.noiseModel.Isotropic.Sigma(3, mag_noise_sigma)\n", + "\n", + "# Create MagPoseFactor (providing body_P_sensor)\n", + "# Note: measurement and bias are in SENSOR frame when body_P_sensor is specified\n", + "mag_factor = gtsam.MagPoseFactorPose3(pose_key, s_measured, mag_scale, \n", + " n_direction_point, mag_bias_sensor, \n", + " noise_model, body_P_sensor)\n", + "\n", + "print(\"Created MagPoseFactor:\")\n", + "mag_factor.print()\n", + "\n", + "# --- Evaluate Error ---\n", + "# Evaluate at the ground truth pose (error should be zero)\n", + "error_at_truth = mag_factor.evaluateError(truth_nPb)\n", + "print(\"\\nError at ground truth pose (should be zero):\", error_at_truth)\n", + "\n", + "# Evaluate at a different pose (error should be non-zero)\n", + "test_nPb = gtsam.Pose3(gtsam.Rot3.Pitch(np.deg2rad(15)), gtsam.Point3(1,2,3))\n", + "error_at_test = mag_factor.evaluateError(test_nPb)\n", + "print(\"Error at test pose:\", error_at_test)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Important Considerations\n", + "- **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.\n", + "- **Units**: Maintain consistent units (e.g., nT) for scale, bias, measurement, and noise sigma." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Source\n", + "- [MagPoseFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/MagPoseFactor.h)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index a560439c9..6ab3eb7a9 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -518,6 +518,21 @@ class MagFactor1: gtsam::NonlinearFactor { Vector evaluateError(const gtsam::Rot3& nRb); }; +#include +#include +template +virtual class MagPoseFactor : gtsam::NoiseModelFactor { + MagPoseFactor(size_t pose_key, + const POSE::Translation& measured, double scale, + const POSE::Translation& direction, const POSE::Translation& bias, + const gtsam::noiseModel::Base* noiseModel); + MagPoseFactor(size_t pose_key, + const POSE::Translation& measured, double scale, + const POSE::Translation& direction, const POSE::Translation& bias, + const gtsam::noiseModel::Base* noiseModel, const POSE& body_P_sensor); + Vector evaluateError(const POSE& nRb); +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const;