AttitudeFactor
Frank Dellaert
parent
e84b1c4ec0
commit
13a74a4af0
|
|
@ -0,0 +1,209 @@
|
||||||
|
{
|
||||||
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"# AttitudeFactor\n",
|
||||||
|
"\n",
|
||||||
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AttitudeFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
|
||||||
|
"\n",
|
||||||
|
"## Introduction\n",
|
||||||
|
"\n",
|
||||||
|
"The `AttitudeFactor` family in GTSAM provides factors that constrain the orientation (attitude) of a body (`Rot3` or `Pose3`) based on directional measurements. A common use case is constraining roll and pitch using an accelerometer (measuring gravity) or constraining yaw using a magnetometer (measuring the Earth's magnetic field).\n",
|
||||||
|
"\n",
|
||||||
|
"This notebook explains the mathematical foundation and usage of these factors."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 1,
|
||||||
|
"metadata": {
|
||||||
|
"tags": [
|
||||||
|
"remove-cell"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
|
||||||
|
"\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
|
||||||
|
"\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"%pip install --quiet gtsam-develop"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Mathematical Foundation\n",
|
||||||
|
"\n",
|
||||||
|
"### Concept\n",
|
||||||
|
"\n",
|
||||||
|
"The `AttitudeFactor` constrains the rotation $R_{nb}$ (from the body frame $b$ to the navigation frame $n$) such that a known reference direction in the navigation frame (`nRef`) aligns with a measured direction in the body frame (`bMeasured`), when rotated by $R_{nb}$.\n",
|
||||||
|
"\n",
|
||||||
|
"The factor enforces that:\n",
|
||||||
|
"$$ \\text{nRef} \\approx R_{nb} \\cdot \\text{bMeasured} $$ \n",
|
||||||
|
"\n",
|
||||||
|
"where:\n",
|
||||||
|
"- $R_{nb}$ is the rotation matrix representing the orientation from body to navigation frame.\n",
|
||||||
|
"- `bMeasured` is the direction *vector* measured by the sensor in the *body* frame (e.g., the accelerometer reading, typically normalized to a `Unit3`).\n",
|
||||||
|
"- `nRef` is the known direction *vector* of the corresponding physical quantity in the *navigation* frame (e.g., the gravity vector, typically normalized to a `Unit3`).\n",
|
||||||
|
"\n",
|
||||||
|
"### Error Function\n",
|
||||||
|
"\n",
|
||||||
|
"The error function computes the angular difference between the reference direction (`nRef`) and the rotated measured direction ($R_{nb} \\cdot \\text{bMeasured}$). GTSAM uses the `Unit3::error` method, which typically calculates a 2-dimensional tangent space error.\n",
|
||||||
|
"\n",
|
||||||
|
"$$ e = \\text{nRef}.\\text{error}(R_{nb} \\cdot \\text{bMeasured}) $$ \n",
|
||||||
|
"\n",
|
||||||
|
"This error is minimal (zero) when the rotated body measurement aligns perfectly with the navigation reference direction.\n",
|
||||||
|
"\n",
|
||||||
|
"The `attitudeError` function within the base class implements this:\n",
|
||||||
|
"```cpp\n",
|
||||||
|
"Vector AttitudeFactor::attitudeError(const Rot3& nRb) const {\n",
|
||||||
|
" // measured direction in body frame rotated into nav frame\n",
|
||||||
|
" Unit3 nPredicted = nRb * bMeasured_;\n",
|
||||||
|
" // error between predicted and reference direction\n",
|
||||||
|
" return nRef_.error(nPredicted); \n",
|
||||||
|
"} \n",
|
||||||
|
"```\n",
|
||||||
|
"\n",
|
||||||
|
"### Jacobians\n",
|
||||||
|
"\n",
|
||||||
|
"For optimization, the $2 \\times 3$ Jacobian of the error function with respect to the rotation parameters ($R_{nb}$) is required. This is computed using the chain rule, involving the derivative of the rotated vector and the derivative of the `Unit3::error` function.\n",
|
||||||
|
"\n",
|
||||||
|
"**Note:** The Jacobian for this specific error function can become zero or ill-defined when the angle between the predicted and reference directions is exactly 180 degrees. The factor behaves best when the initial estimate for $R_{nb}$ is reasonably close (i.e., within the correct hemisphere)."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Available Factors\n",
|
||||||
|
"\n",
|
||||||
|
"- `Rot3AttitudeFactor`: Constrains a `Rot3` variable.\n",
|
||||||
|
"- `Pose3AttitudeFactor`: Constrains the rotational part of a `Pose3` variable."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Usage Example\n",
|
||||||
|
"\n",
|
||||||
|
"Let's constrain the roll and pitch of a `Pose3` using an accelerometer reading. We assume an ENU navigation frame (Z is up) and the accelerometer measures gravity when stationary."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 1,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"Created Pose3AttitudeFactor:\n",
|
||||||
|
"Pose3AttitudeFactor on x0\n",
|
||||||
|
" reference direction in nav frame: : 0\n",
|
||||||
|
" 0\n",
|
||||||
|
"-1\n",
|
||||||
|
" measured direction in body frame: :0.0102036\n",
|
||||||
|
" 0\n",
|
||||||
|
"-0.999948\n",
|
||||||
|
"isotropic dim=2 sigma=0.1\n",
|
||||||
|
"\n",
|
||||||
|
"Error at identity pose: [ 0. -0.01020355]\n",
|
||||||
|
"Error near expected zero pose: [ 0.00999931 -0.01020355]\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"import numpy as np\n",
|
||||||
|
"from gtsam import Pose3, Unit3, Rot3, Pose3AttitudeFactor\n",
|
||||||
|
"from gtsam.symbol_shorthand import X\n",
|
||||||
|
"from gtsam.noiseModel import Isotropic\n",
|
||||||
|
"\n",
|
||||||
|
"# Define the reference direction in the navigation (ENU) frame\n",
|
||||||
|
"# Gravity points along the negative Z axis in ENU.\n",
|
||||||
|
"nRef_gravity = Unit3(np.array([0.0, 0.0, -1.0]))\n",
|
||||||
|
"\n",
|
||||||
|
"# Define the measured direction in the body frame\n",
|
||||||
|
"# Assume the accelerometer reading is [0.1, 0.0, -9.8]. \n",
|
||||||
|
"# GTSAM's Unit3 constructor normalizes this automatically.\n",
|
||||||
|
"# The factor expects the *direction* the sensor measures in the *body* frame.\n",
|
||||||
|
"bMeasured_acc = Unit3(np.array([0.1, 0.0, -9.8]))\n",
|
||||||
|
"\n",
|
||||||
|
"# Define the noise model for the measurement (2-dimensional error)\n",
|
||||||
|
"# Example: 0.1 radians standard deviation on the tangent plane error\n",
|
||||||
|
"attitude_noise_sigma = 0.1\n",
|
||||||
|
"noise_model = Isotropic.Sigma(2, attitude_noise_sigma)\n",
|
||||||
|
"\n",
|
||||||
|
"# Create the factor\n",
|
||||||
|
"attitude_factor = Pose3AttitudeFactor(X(0), nRef_gravity, \n",
|
||||||
|
" noise_model, bMeasured_acc)\n",
|
||||||
|
"\n",
|
||||||
|
"print(\"Created Pose3AttitudeFactor:\")\n",
|
||||||
|
"attitude_factor.print()\n",
|
||||||
|
"\n",
|
||||||
|
"# Example: Evaluate the error at the identity pose (likely non-zero error)\n",
|
||||||
|
"identity_pose = Pose3()\n",
|
||||||
|
"error = attitude_factor.evaluateError(identity_pose)\n",
|
||||||
|
"print(\"\\nError at identity pose:\", error)\n",
|
||||||
|
"\n",
|
||||||
|
"# For zero error, the rotated measurement should align with the reference\n",
|
||||||
|
"# nRef = R_nb * bMeas => R_nb = nRef * bMeas.inverse() (approx for Unit3)\n",
|
||||||
|
"# This is complex to solve directly, but optimization finds the R_nb where error is zero.\n",
|
||||||
|
"# Let's try a pose that should roughly align Z_body with Z_nav (small roll)\n",
|
||||||
|
"zero_error_pose = Pose3(Rot3.Roll(-0.01), np.zeros(3)) # Approx -0.1/9.8 rad\n",
|
||||||
|
"error_near_zero = attitude_factor.evaluateError(zero_error_pose)\n",
|
||||||
|
"print(\"Error near expected zero pose:\", error_near_zero)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Conclusion\n",
|
||||||
|
"\n",
|
||||||
|
"The `AttitudeFactor` is a crucial tool for incorporating absolute orientation information from sensors like accelerometers and magnetometers into GTSAM factor graphs. It helps constrain orientation estimates, particularly roll and pitch (using gravity) and potentially yaw (using magnetic north), improving navigation accuracy, especially in GPS-denied scenarios."
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"## Source\n",
|
||||||
|
"- [AttitudeFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h)\n",
|
||||||
|
"- [AttitudeFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.cpp)"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"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
|
||||||
|
}
|
||||||
|
|
@ -346,6 +346,7 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor {
|
||||||
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
||||||
gtsam::Unit3 nRef() const;
|
gtsam::Unit3 nRef() const;
|
||||||
gtsam::Unit3 bMeasured() const;
|
gtsam::Unit3 bMeasured() const;
|
||||||
|
gtsam::Vector evaluateError(const gtsam::Rot3& nRb);
|
||||||
|
|
||||||
// enable serialization functionality
|
// enable serialization functionality
|
||||||
void serialize() const;
|
void serialize() const;
|
||||||
|
|
@ -363,6 +364,7 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
|
||||||
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
||||||
gtsam::Unit3 nRef() const;
|
gtsam::Unit3 nRef() const;
|
||||||
gtsam::Unit3 bMeasured() const;
|
gtsam::Unit3 bMeasured() const;
|
||||||
|
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
|
||||||
|
|
||||||
// enable serialization functionality
|
// enable serialization functionality
|
||||||
void serialize() const;
|
void serialize() const;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue