MagPoseFactor + wrapping
							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