MagPoseFactor + wrapping
Frank Dellaert
parent
f9c981d106
commit
f4485db21c
|
|
@ -77,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
|
|||
const Point& direction,
|
||||
const Point& bias,
|
||||
const SharedNoiseModel& model,
|
||||
const std::optional<POSE>& body_P_sensor)
|
||||
const std::optional<POSE>& body_P_sensor = {})
|
||||
: Base(model, pose_key),
|
||||
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
|
||||
nM_(scale * direction.normalized()),
|
||||
|
|
|
|||
|
|
@ -0,0 +1,205 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# MagPoseFactor\n",
|
||||
"\n",
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/MagPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
|
||||
"\n",
|
||||
"## Overview\n",
|
||||
"\n",
|
||||
"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.\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<Pose3>:\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<Pose3>:\")\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
|
||||
}
|
||||
|
|
@ -518,6 +518,21 @@ class MagFactor1: gtsam::NonlinearFactor {
|
|||
Vector evaluateError(const gtsam::Rot3& nRb);
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/navigation/MagPoseFactor.h>
|
||||
template <POSE = {gtsam::Pose2, gtsam::Pose3}>
|
||||
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 <gtsam/navigation/Scenario.h>
|
||||
virtual class Scenario {
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
|
|
|
|||
Loading…
Reference in New Issue