12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

More details and code example

release/4.3a0
Frank Dellaert 2025-04-05 22:33:14 -04:00
parent 796364d4fb
commit eb705c6a70
5 changed files with 239 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
gtsam/navigation/doc/AHRSFactor.ipynb
View File

@ -6,6 +6,8 @@
"source": [ "source": [
"# AHRSFactor\n", "# AHRSFactor\n",
"\n", "\n",
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AHRSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
"\n",
"## Overview\n", "## Overview\n",
"\n", "\n",
"The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n", "The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n",
@ -13,6 +15,27 @@
"The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately." "The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately."
] ]
}, },
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam plotly"
]
},
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
@ -72,19 +95,119 @@
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
"source": [ "source": [
"## Typical Use Case\n", "## Usage\n",
"\n", "\n",
"`AHRSFactor` is typically applied within factor-graph-based orientation estimators where orientation states are discretized in time. Its primary role is to incorporate gyroscope information in a computationally efficient and numerically stable way, especially useful in IMU-centric navigation scenarios like:\n", "First, create a PreintegratedAhrsMeasurements (PAM) object by supplying the necessary IMU parameters. "
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from gtsam import PreintegrationParams, PreintegratedAhrsMeasurements\n",
"\n", "\n",
"- Robotics and autonomous vehicles\n", "params = PreintegrationParams.MakeSharedU(9.81)\n",
"- UAV navigation systems\n", "params.setGyroscopeCovariance(np.deg2rad(1)*np.eye(3))\n",
"- Motion capture and tracking systems" "params.setAccelerometerCovariance(0.01*np.eye(3))\n",
"\n",
"biasHat = np.zeros(3) # Assuming no bias for simplicity\n",
"pam = PreintegratedAhrsMeasurements(params, biasHat) "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Next, integrate each gyroscope measurement along with its corresponding time interval to accumulate the preintegrated rotation."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import Point3\n",
"np.random.seed(42) # For reproducibility\n",
"for _ in range(15): # Add 15 random measurements, biased to move around z-axis\n",
" omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3) # Random angular velocity vector\n",
" pam.integrateMeasurement(omega, deltaT=0.1)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, construct the AHRSFactor using the accumulated PAM and the keys representing the rotation states at the two time instants."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import AHRSFactor\n",
"bias_key = 0 # Key for the bias\n",
"i, j = 1, 2 # Indices of the two attitude unknowns\n",
"ahrs_factor = AHRSFactor(i, j, bias_key, pam)"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"AHRSFactor(1,2,0, preintegrated measurements: deltaTij [1.5]\n",
" deltaRij.ypr = ( -0.82321 -0.0142842 0.0228577)\n",
"biasHat [0 0 0]\n",
" PreintMeasCov [ 0.0261799 1.73472e-18 1.35525e-20\n",
"1.73472e-18 0.0261799 9.23266e-20\n",
"1.35525e-20 1.17738e-19 0.0261799 ]\n",
" noise model: diagonal sigmas [0.161802159; 0.161802159; 0.161802159];\n",
"\n"
]
}
],
"source": [
"print(ahrs_factor)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [AHRSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)\n",
"- [AHRSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.cpp)"
] ]
} }
], ],
"metadata": { "metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": { "language_info": {
"name": "python" "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": 4,

32
gtsam/navigation/doc/PreintegratedRotation.ipynb
View File

@ -21,7 +21,11 @@
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 1,
"metadata": {}, "metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [ "outputs": [
{ {
"name": "stdout", "name": "stdout",
@ -118,6 +122,23 @@
"print(params)" "print(params)"
] ]
}, },
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can (and should) configure using your gyroscope's noise covariance matrix and the optional Coriolis acceleration compensation vector:\n",
"\n",
"### Setters\n",
"- `setGyroscopeCovariance`: Sets the 3×3 continuous-time noise covariance matrix for gyroscope measurements.\n",
"- `setOmegaCoriolis`: Sets an optional Coriolis acceleration compensation vector.\n",
"- `setBodyPSensor`: Sets an optional pose transformation between the body frame and the sensor frame.\n",
"\n",
"### Getters\n",
"- `getGyroscopeCovariance`: Returns the gyroscope covariance matrix.\n",
"- `getOmegaCoriolis`: Returns the optional Coriolis acceleration vector.\n",
"- `getBodyPSensor`: Returns the optional body-to-sensor pose transformation."
]
},
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
@ -9017,6 +9038,15 @@
"fig.update_layout(scene_camera=dict(eye=dict(x=1.1, y=-1.1, z=0.2)), margin=dict(l=0, r=0, t=0, b=0))\n", "fig.update_layout(scene_camera=dict(eye=dict(x=1.1, y=-1.1, z=0.2)), margin=dict(l=0, r=0, t=0, b=0))\n",
"fig.show()" "fig.show()"
] ]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [PreintegratedRotation.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.h)\n",
"- [PreintegratedRotation.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.cpp)"
]
} }
], ],
"metadata": { "metadata": {

70
gtsam/navigation/doc/PreintegrationParams.ipynb Normal file
View File

@ -0,0 +1,70 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PreintegrationParams\n",
"\n",
"The `PreintegrationParams` class extends `PreintegratedRotationParams` to provide a complete configuration for IMU preintegration, including accelerometer measurements:\n",
"\n",
"| Parameter | Description |\n",
"|-----------|-------------|\n",
"| `accelerometerCovariance` | 3×3 continuous-time noise covariance matrix for accelerometer measurements (units: m²/s⁴/Hz) |\n",
"| `integrationCovariance` | 3×3 covariance matrix representing additional uncertainty during integration |\n",
"| `use2ndOrderCoriolis` | Boolean flag to enable more accurate Coriolis effect compensation |\n",
"| `n_gravity` | Gravity vector in the navigation frame (units: m/s²) |\n",
"\n",
"## Convenience API\n",
"\n",
"The class provides convenient factory methods for the two most common navigation frame conventions:\n",
"- `MakeSharedD()`: Creates parameters for a Z-down frame (like NED - North-East-Down)\n",
"- `MakeSharedU()`: Creates parameters for a Z-up frame (like ENU - East-North-Up)\n",
"\n",
"Both methods also take a [local gravity constant](https://www.sensorsone.com/local-gravity-calculator/), which decreases as one gets closer to the equator. The default is $9.81m/s^2$."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Detailed API\n",
"\n",
"- The constructor requires the navigation frame gravity vector as an argument.\n",
"\n",
"### Setters\n",
"- `setGyroscopeCovariance`: Sets the 3×3 continuous-time noise covariance matrix for gyroscope measurements.\n",
"- `setOmegaCoriolis`: Sets an optional Coriolis acceleration compensation vector.\n",
"- `setBodyPSensor`: Sets an optional pose transformation between the body frame and the sensor frame.\n",
"- `setAccelerometerCovariance`: Sets the accelerometer covariance matrix.\n",
"- `setIntegrationCovariance`: Sets the integration covariance matrix.\n",
"- `setUse2ndOrderCoriolis`: Sets the flag controlling the use of second order Coriolis compensation.\n",
"\n",
"### Getters\n",
"- `getGyroscopeCovariance`: Returns the gyroscope covariance matrix.\n",
"- `getOmegaCoriolis`: Returns the optional Coriolis acceleration vector.\n",
"- `getBodyPSensor`: Returns the optional body-to-sensor pose transformation.\n",
"- `getAccelerometerCovariance`: Returns the current accelerometer covariance matrix.\n",
"- `getIntegrationCovariance`: Returns the current integration covariance matrix.\n",
"- `getGravity`: Returns the navigation frame gravity vector.\n",
"- `isUsing2ndOrderCoriolis`: Returns the status of the second order Coriolis flag."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Source\n",
"- [PreintegrationParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)\n",
"- [PreintegrationParams.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.cpp)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 2
}

12
gtsam/navigation/navigation.i
View File

@ -314,10 +314,7 @@ class PreintegratedAhrsMeasurements {
virtual class AHRSFactor : gtsam::NonlinearFactor { virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis); const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface // Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
@ -329,6 +326,13 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
// enable serialization functionality // enable serialization functionality
void serialize() const; void serialize() const;
// deprecated:
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
}; };
#include <gtsam/navigation/AttitudeFactor.h> #include <gtsam/navigation/AttitudeFactor.h>

2
gtsam/navigation/navigation.md
View File

@ -9,13 +9,13 @@ The `navigation` module in GTSAM provides specialized tools for inertial navigat
## Attitude Estimation ## Attitude Estimation
- **[PreintegrationParams](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)**: Parameters for IMU preintegration.
- **[PreintegratedRotation](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.h)**: Handles gyroscope measurements to track rotation changes. - **[PreintegratedRotation](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegratedRotation.h)**: Handles gyroscope measurements to track rotation changes.
- **[AHRSFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)**: Attitude and Heading Reference System factor for orientation estimation. - **[AHRSFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)**: Attitude and Heading Reference System factor for orientation estimation.
- **[AttitudeFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h)**: Factors for attitude estimation from reference directions. - **[AttitudeFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AttitudeFactor.h)**: Factors for attitude estimation from reference directions.
## IMU Preintegration ## IMU Preintegration
- **[PreintegrationParams](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationParams.h)**: Base parameters for IMU preintegration.
- **[PreintegrationBase](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationBase.h)**: Base class for IMU preintegration classes. - **[PreintegrationBase](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/PreintegrationBase.h)**: Base class for IMU preintegration classes.
- **[ManifoldPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldPreintegration.h)**: Implements IMU preintegration using manifold-based methods as in the Forster et al paper. - **[ManifoldPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldPreintegration.h)**: Implements IMU preintegration using manifold-based methods as in the Forster et al paper.
- **[TangentPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h)**: Implements IMU preintegration using tangent space methods, developed at Skydio. - **[TangentPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h)**: Implements IMU preintegration using tangent space methods, developed at Skydio.