MagFactor + wrapping
Frank Dellaert
parent
ae87ca690d
commit
f9c981d106
|
|
@ -0,0 +1,226 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# MagFactor Family\n",
|
||||
"\n",
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/MagFactor.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 `MagFactor` family provides factors for incorporating magnetometer measurements, which sense the Earth's magnetic field, into a GTSAM factor graph. These factors are primarily used to constrain the orientation (mostly \"yaw\") of a body or sensor.\n",
|
||||
"\n",
|
||||
"Magnetometers measure the local magnetic field vector in the sensor's own frame. The factors relate this measurement to the known (or estimated) magnetic field direction in the navigation frame via the body's rotation.\n",
|
||||
"\n",
|
||||
"Several variants exist depending on what is known and what is being estimated:"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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 general model assumed by these factors is:\n",
|
||||
"$$ bM_{measured} = R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b $$ \n",
|
||||
"where:\n",
|
||||
"- $bM_{measured}$: The 3D magnetic field vector measured by the sensor, in the **body frame**.\n",
|
||||
"- $R_{bn}$: The rotation matrix from the navigation frame ($n$) to the body frame ($b$). This is the *inverse* of the rotation usually stored in `Pose3` or `NavState` ($R_{nb}$).\n",
|
||||
"- $s$: A scale factor relating the magnetic field strength to the magnetometer's output units (e.g., nT).\n",
|
||||
"- $\\hat{d}_n$: The unit vector representing the direction of the Earth's magnetic field in the **navigation frame**.\n",
|
||||
"- $b$: A 3D additive bias vector in the **body frame**.\n",
|
||||
"\n",
|
||||
"The factor error is typically calculated as:\n",
|
||||
"$$ e = bM_{measured} - [ R_{bn} \\cdot (s \\cdot \\hat{d}_n) + b ] $$ \n",
|
||||
"Different factors treat different components ($R_{bn}$, $s$, $\\hat{d}_n$, $b$) as known constants or as variables to be estimated."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Factor Variants\n",
|
||||
"\n",
|
||||
"- **`MagFactor`**: \n",
|
||||
" - Estimates: `Rot2` (intended for yaw $R_{nb}$).\n",
|
||||
" - Assumes Known: `scale`, `direction`, `bias`.\n",
|
||||
" - Note: Uses `Rot2` which might be limiting unless the sensor is always level.\n",
|
||||
"\n",
|
||||
"- **`MagFactor1`**: \n",
|
||||
" - Estimates: `Rot3` ($R_{nb}$).\n",
|
||||
" - Assumes Known: `scale`, `direction`, `bias`.\n",
|
||||
" - This is often the most practical factor for direct orientation estimation when calibration is known.\n",
|
||||
"\n",
|
||||
"- **`MagFactor2`**: \n",
|
||||
" - Estimates: `nM` (`Point3`, the scaled field vector $s \\cdot \\hat{d}_n$ in nav frame), `bias` (`Point3`).\n",
|
||||
" - Assumes Known: `Rot3` ($R_{nb}$).\n",
|
||||
" - Useful for calibrating the local field and bias if orientation is known (e.g., from other sensors).\n",
|
||||
"\n",
|
||||
"- **`MagFactor3`**: \n",
|
||||
" - Estimates: `scale` (`double`), `direction` (`Unit3`), `bias` (`Point3`).\n",
|
||||
" - Assumes Known: `Rot3` ($R_{nb}$).\n",
|
||||
" - Provides full calibration of scale, direction, and bias when orientation is known.\n",
|
||||
"\n",
|
||||
"- **`MagPoseFactor<POSE>`**: (Separate Header: `MagPoseFactor.h`)\n",
|
||||
" - Estimates: `Pose2` or `Pose3`.\n",
|
||||
" - Assumes Known: `scale`, `direction`, `bias`, optional `body_P_sensor`.\n",
|
||||
" - Similar to `MagFactor1` but works directly on `Pose` types and handles sensor-to-body transforms."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Key Functionality / API (MagFactor1 Example)\n",
|
||||
"\n",
|
||||
"- **Constructor**: `MagFactor1(key, measured, scale, direction, bias, model)`: Creates the factor connecting the `Rot3` key, given the measurement, known calibration parameters, and noise model.\n",
|
||||
"- **`evaluateError(nRb)`**: Calculates the 3D error vector $e$ based on the current `Rot3` estimate $R_{nb}$."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Usage Example (MagFactor1)\n",
|
||||
"\n",
|
||||
"Using a magnetometer to help estimate a `Rot3` orientation, assuming known calibration."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Created MagFactor1:\n",
|
||||
" keys = { r0 }\n",
|
||||
"isotropic dim=3 sigma=50\n",
|
||||
"\n",
|
||||
"Error at ground truth rotation (should be zero): [0. 0. 0.]\n",
|
||||
"Error at test rotation: [1034.79105955 1628.05638524 0. ]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import gtsam\n",
|
||||
"import numpy as np\n",
|
||||
"from gtsam.symbol_shorthand import R # Rotation key\n",
|
||||
"\n",
|
||||
"# --- Assumed Known Calibration & Field ---\n",
|
||||
"# Local magnetic field direction in navigation frame (e.g., NED)\n",
|
||||
"# Example: From NOAA for specific location/date\n",
|
||||
"# Field Strength: 48343.4 nT\n",
|
||||
"# Declination: -4.94 deg -> Angle from North towards West\n",
|
||||
"# Inclination: 62.78 deg -> Angle below horizontal\n",
|
||||
"declination_rad = np.deg2rad(-4.94)\n",
|
||||
"inclination_rad = np.deg2rad(62.78)\n",
|
||||
"field_strength_nT = 48343.4\n",
|
||||
"\n",
|
||||
"# Convert Dec/Inc to NED vector components\n",
|
||||
"north_comp = np.cos(inclination_rad) * np.cos(declination_rad)\n",
|
||||
"east_comp = np.cos(inclination_rad) * np.sin(declination_rad)\n",
|
||||
"down_comp = np.sin(inclination_rad)\n",
|
||||
"n_direction = gtsam.Unit3(np.array([north_comp, east_comp, down_comp]))\n",
|
||||
"\n",
|
||||
"# Assume scale factor converts unit vector to nT (can be absorbed if field strength is used)\n",
|
||||
"mag_scale = field_strength_nT\n",
|
||||
"\n",
|
||||
"# Assume known magnetometer bias in body frame (nT)\n",
|
||||
"mag_bias_body = gtsam.Point3(10.0, -5.0, 2.0) \n",
|
||||
"\n",
|
||||
"# --- Simulation: Generate Measurement ---\n",
|
||||
"# Assume a ground truth rotation (e.g., 30 deg yaw right in NED)\n",
|
||||
"truth_nRb = gtsam.Rot3.Yaw(np.deg2rad(30)) \n",
|
||||
"truth_bRn = truth_nRb.inverse()\n",
|
||||
"\n",
|
||||
"# Calculate the expected magnetic field in the body frame (ideal, before bias)\n",
|
||||
"n_field_vector = mag_scale * n_direction.point3()\n",
|
||||
"b_field_ideal = truth_bRn.rotate(n_field_vector)\n",
|
||||
"\n",
|
||||
"# Calculate the measured value including bias\n",
|
||||
"b_measured = b_field_ideal + mag_bias_body\n",
|
||||
"\n",
|
||||
"# --- Factor Creation ---\n",
|
||||
"rot_key = R(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 MagFactor1\n",
|
||||
"mag_factor = gtsam.MagFactor1(rot_key, b_measured, mag_scale, \n",
|
||||
" n_direction, mag_bias_body, noise_model)\n",
|
||||
"\n",
|
||||
"print(\"Created MagFactor1:\")\n",
|
||||
"mag_factor.print()\n",
|
||||
"\n",
|
||||
"# --- Evaluate Error ---\n",
|
||||
"# Evaluate at the ground truth rotation (error should be zero)\n",
|
||||
"error_at_truth = mag_factor.evaluateError(truth_nRb)\n",
|
||||
"print(\"\\nError at ground truth rotation (should be zero):\", error_at_truth)\n",
|
||||
"\n",
|
||||
"# Evaluate at a different rotation (error should be non-zero)\n",
|
||||
"test_nRb = gtsam.Rot3.Yaw(np.deg2rad(25)) # Slightly off\n",
|
||||
"error_at_test = mag_factor.evaluateError(test_nRb)\n",
|
||||
"print(\"Error at test rotation:\", error_at_test)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Important Notes\n",
|
||||
"- **Coordinate Frames**: Be very careful with navigation frame (NED vs ENU) and body frame conventions. Ensure the `direction` vector and the `Rot3`/`Pose3` variables use the same navigation frame. The `measured` and `bias` are typically in the body frame.\n",
|
||||
"- **Units**: Ensure consistency between the `scale`, `bias`, `measured` values and the noise model sigma (e.g., all in nanoTesla (nT)).\n",
|
||||
"- **Calibration**: Accurate knowledge of `scale`, `direction`, and `bias` is crucial for `MagFactor1` and `MagPoseFactor`. If these are unknown, consider using `MagFactor2`/`MagFactor3` or online calibration techniques."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Source\n",
|
||||
"- [MagFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/MagFactor.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
|
||||
}
|
||||
|
|
@ -502,6 +502,22 @@ class ConstantVelocityFactor : gtsam::NonlinearFactor {
|
|||
gtsam::Vector evaluateError(const gtsam::NavState &x1, const gtsam::NavState &x2) const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/MagFactor.h>
|
||||
|
||||
class MagFactor: gtsam::NonlinearFactor {
|
||||
MagFactor(size_t key, const gtsam::Point3& measured, double scale,
|
||||
const gtsam::Unit3& direction, const gtsam::Point3& bias,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
Vector evaluateError(const gtsam::Rot2& nRb);
|
||||
};
|
||||
|
||||
class MagFactor1: gtsam::NonlinearFactor {
|
||||
MagFactor1(size_t key, const gtsam::Point3& measured, double scale,
|
||||
const gtsam::Unit3& direction, const gtsam::Point3& bias,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
Vector evaluateError(const gtsam::Rot3& nRb);
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
virtual class Scenario {
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
|
|
|
|||
Loading…
Reference in New Issue