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

Add files and links

release/4.3a0
Frank Dellaert 2025-04-07 17:45:29 -04:00
parent a68d3ffbd2
commit 5eaba687de
2 changed files with 109 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
gtsam/navigation/doc/ImuFactor.ipynb
View File

@ -10,6 +10,8 @@
"\n",
"## Overview\n",
"\n",
"### ImuFactor (5-way Factor)\n",
"\n",
"The `ImuFactor` is the standard GTSAM factor for incorporating preintegrated IMU measurements into a factor graph. It's a 5-way factor connecting:\n",
"\n",
"1. Pose at time $i$ (`Pose3`)\n",
@ -20,7 +22,21 @@
"\n",
"It takes a `PreintegratedImuMeasurements` object, which summarizes the IMU readings between times $i$ and $j$. The factor's error function measures the discrepancy between the relative motion predicted by the preintegrated measurements (corrected for the *current* estimate of the bias at time $i$) and the relative motion implied by the state variables ($Pose_i, Vel_i, Pose_j, Vel_j$) connected to the factor.\n",
"\n",
"**Important Note:** This factor assumes the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. It does *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead."
"### ImuFactor2: NavState Variant\n",
"\n",
"The `ImuFactor2` is ternary variant of the `ImuFactor` that operates directly on `NavState` objects instead of separate `Pose3` and `Vector3` variables for pose and velocity. This simplifies the factor graph by reducing the number of connected variables and can make the graph more efficient to optimize.\n",
"\n",
"Instead of connecting five variables (`Pose_i`, `Vel_i`, `Pose_j`, `Vel_j`, `Bias_i`), the `ImuFactor2` connects three:\n",
"\n",
"1. `NavState` at time $i$ (`NavState` combines pose and velocity)\n",
"2. `NavState` at time $j`\n",
"3. IMU Bias at time $i$ (`imuBias::ConstantBias`)\n",
"\n",
"### Modeling Bias\n",
"\n",
"Both factors assume that the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. That is typically a very good assumption, as bias evolves slowly over time.\n",
"\n",
"The factors do *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead."
]
},
{
@ -94,7 +110,7 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 2,
"metadata": {},
"outputs": [
{
@ -188,6 +204,77 @@
"print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can also use `ImuFactor2`, with `NavState`, giving exactly the same result:"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Created ImuFactor2:\n",
"ImuFactor2(x0,x1,b0)\n",
"preintegrated measurements:\n",
"\n",
" deltaTij = 0.1\n",
" deltaRij.ypr = ( 0 -0 0)\n",
" deltaPij = 0 0 -0.04905\n",
" deltaVij = 0 0 -0.981\n",
" gyrobias = 0 0 0\n",
" acc_bias = 0 0 0\n",
"\n",
" preintMeasCov \n",
"[ 1e-05 0 0 0 1.39793e-07 0 0 4.4145e-06 0\n",
" 0 1e-05 0 -1.39793e-07 0 0 -4.4145e-06 0 0\n",
" 0 0 1e-05 0 0 0 0 0 0\n",
" 0 -1.39793e-07 0 3.32969e-06 0 0 5.00974e-05 0 0\n",
" 1.39793e-07 0 0 0 3.32969e-06 0 0 5.00974e-05 0\n",
" 0 0 0 0 0 3.326e-06 0 0 5e-05\n",
" 0 -4.4145e-06 0 5.00974e-05 0 0 0.00100274 0 0\n",
" 4.4145e-06 0 0 0 5.00974e-05 0 0 0.00100274 0\n",
" 0 0 0 0 0 5e-05 0 0 0.001]\n",
" noise model sigmas: 0.00316228 0.00316228 0.00316228 0.00182474 0.00182474 0.00182373 0.0316661 0.0316661 0.0316228\n",
"\n",
"Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
"Factor error (0.5 * ||error||^2_Sigma): 0.0\n"
]
}
],
"source": [
"from gtsam import ImuFactor2\n",
"\n",
"# 1. Create the ImuFactor2\n",
"# The noise model is automatically derived from pim.preintMeasCov()\n",
"imu_factor2 = ImuFactor2(X(0), X(1), B(0), pim)\n",
"\n",
"print(\"Created ImuFactor2:\")\n",
"imu_factor2.print()\n",
"\n",
"# 2. Example: Evaluate error with perfect states (should be near zero)\n",
"graph = NonlinearFactorGraph()\n",
"graph.add(imu_factor2)\n",
"\n",
"values = Values()\n",
"nav_state_i = NavState(pose_i, vel_i)\n",
"nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat\n",
"\n",
"values.insert(X(0), nav_state_i)\n",
"values.insert(X(1), nav_state_j)\n",
"values.insert(B(0), bias_i)\n",
"\n",
"error_vector = imu_factor2.evaluateError(nav_state_i, nav_state_j, bias_i)\n",
"print(\"\\nError vector (should be near zero):\", error_vector)\n",
"print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))"
]
},
{
"cell_type": "markdown",
"metadata": {},

34
gtsam/navigation/navigation.md
View File

@ -10,29 +10,35 @@ The `navigation` module in GTSAM provides specialized tools for inertial navigat
### 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.
- **[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.
- **[PreintegrationParams](doc/PreintegrationParams.ipynb)**: Parameters for IMU preintegration.
- **[PreintegratedRotation](doc/PreintegratedRotation.ipynb)**: Handles gyroscope measurements to track rotation changes.
- **[AHRSFactor](doc/AHRSFactor.ipynb)**: Attitude and Heading Reference System factor for orientation estimation.
- **[AttitudeFactor](doc/AttitudeFactor.ipynb)**: Factors for attitude estimation from reference directions.
### IMU Preintegration (See also below)
- **[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.
- **[TangentPreintegration](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/TangentPreintegration.h)**: Implements IMU preintegration using tangent space methods, developed at Skydio.
- **[CombinedImuFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h)**: Improved IMU factor with bias evolution.
- **[ImuFactor](doc/ImuFactor.ipynb)**: IMU factor.
- **[CombinedImuFactor](doc/CombinedImuFactor.ipynb)**: IMU factor with built-in bias evolution.
### GNSS Integration
- **[GPSFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.h)**: Factor for incorporating GPS position measurements.
- **[BarometricFactor](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/BarometricFactor.h)**: Incorporates barometric altitude measurements.
- **[GPSFactor](doc/GPSFactor.ipynb)**: Factor for incorporating GPS position measurements.
- **[BarometricFactor](doc/BarometricFactor.ipynb)**: Incorporates barometric altitude measurements.
### Magnetic Field Integration
- **[MagFactor](doc/MagFactor.ipynb)**: Factor for incorporating magnetic field measurements.
- **[MagPoseFactor](doc/MagPoseFactor.ipynb)**: Factor for incorporating magnetic field measurements with pose constraints.
### Simulation Tools
- **[Scenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Base class for defining motion scenarios.
- **[Scenario](doc/Scenario.ipynb)**: Base class for defining motion scenarios.
- **[ConstantTwistScenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Implements constant twist (angular and linear velocity) motion.
- **[AcceleratingScenario](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Scenario.h)**: Implements constantly accelerating motion.
- **[ScenarioRunner](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.h)**: Executes scenarios and generates IMU measurements.
- **[ScenarioRunner](doc/ScenarioRunner.ipynb)**: Executes scenarios and generates IMU measurements.
## AHRSFactor and Preintegration
@ -73,7 +79,7 @@ The key components are:
1. **Parameters (`PreintegratedRotationParams`)**:
* Stores parameters specific to gyroscope integration, including gyro noise covariance, optional Coriolis terms, and the sensor's pose relative to the body frame.
2. **Rotation Preintegration (`PreintegratedRotation`)**:
2. **Rotation Preintegration ([PreintegratedRotation](doc/PreintegratedRotation.ipynb))**:
* Handles the core logic for integrating gyroscope measurements over time to estimate the change in orientation (`deltaRij`).
* Calculates the Jacobian of this integrated rotation with respect to gyroscope bias (`delRdelBiasOmega`).
@ -81,7 +87,7 @@ The key components are:
* Inherits from `PreintegratedRotation` and adds the calculation and storage of the covariance matrix (`preintMeasCov_`) associated with the preintegrated rotation.
* This class specifically accumulates the information needed by the `AHRSFactor`.
4. **AHRS Factor (`AHRSFactor`)**:
4. **AHRS Factor ([AHRSFactor](doc/AHRSFactor.ipynb))**:
* A factor that constrains two `Rot3` orientation variables and a `Vector3` bias variable using the information accumulated in a `PreintegratedAhrsMeasurements` object.
* It effectively measures the consistency between the orientation change predicted by the integrated gyro measurements and the orientation change implied by the factor's connected state variables.
@ -189,9 +195,9 @@ The key components are:
* `PreintegratedCombinedMeasurements`: Similar, but designed for the `CombinedImuFactor`. Stores the larger 15x15 covariance matrix (`preintMeasCov_`) that includes correlations with the bias terms.
5. **IMU Factors (`...Factor`)**:
* `ImuFactor`: A 5-way factor connecting previous pose/velocity, current pose/velocity, and a single (constant during the interval) bias estimate. Does *not* model bias evolution between factors.
* `ImuFactor2`: A 3-way factor connecting previous `NavState`, current `NavState`, and a single bias estimate. Functionally similar to `ImuFactor` but uses the combined `NavState` type.
* `CombinedImuFactor`: A 6-way factor connecting previous pose/velocity, current pose/velocity, previous bias, and current bias. *Includes* a model for bias random walk evolution between the two bias states.
* [ImuFactor](doc/ImuFactor.ipynb): A 5-way factor connecting previous pose/velocity, current pose/velocity, and a single (constant during the interval) bias estimate. Does *not* model bias evolution between factors.
* [ImuFactor2](doc/ImuFactor.ipynb): A 3-way factor connecting previous `NavState`, current `NavState`, and a single bias estimate. Functionally similar to `ImuFactor` but uses the combined `NavState` type.
* [CombinedImuFactor](doc/CombinedImuFactor.ipynb): A 6-way factor connecting previous pose/velocity, current pose/velocity, previous bias, and current bias. *Includes* a model for bias random walk evolution between the two bias states.
### Important notes
- Which implementation is used for `PreintegrationType` depends on the compile flag `GTSAM_TANGENT_PREINTEGRATION`, which is true by default.