Detailed Description
class gtsam::PreintegratedImuMeasurementsT< PreintegrationType >
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix.
The measurements are then used to build the Preintegrated IMU factor. Integration is done incrementally (ideally, one integrates the measurement as soon as it is received from the IMU) so as to avoid costly integration at time of factor construction.
Public Member Functions | |
| PreintegratedImuMeasurementsT () | |
| Default constructor with default preintegration parameters. | |
| PreintegratedImuMeasurementsT (const std::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) | |
| Constructor, initializes the class with no measurements. | |
| PreintegratedImuMeasurementsT (const PreintegrationType &base, const Matrix9 &preintMeasCov) | |
| Construct preintegrated directly from members: base class and preintMeasCov. | |
| ~PreintegratedImuMeasurementsT () override | |
| Virtual destructor. | |
| void | print (const std::string &s="Preintegrated Measurements:") const override |
| print | |
| bool | equals (const PreintegratedImuMeasurementsT< PreintegrationType > &expected, double tol=1e-9) const |
| equals | |
| void | resetIntegration () override |
| Re-initialize PreintegratedImuMeasurements. | |
| void | integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override |
| Add a single IMU measurement to the preintegration. | |
| Matrix | preintMeasCov () const |
| Return pre-integrated measurement covariance. | |
| Matrix9 | residualCovariance () const |
| Express the covariance propagated by the selected backend in the chart used by the IMU factor residual. | |
| Matrix9 | residualCovarianceAt (const Rot3 &predictedAttitude) const |
| Physical endpoint covariance at a fixed nominal predicted attitude. | |
| template<typename PB = PreintegrationType, typename = typename std::enable_if<std::is_same<PB, TangentPreintegration>::value>::type> | |
| void | mergeWith (const PreintegratedImuMeasurementsT< TangentPreintegration > &pim12, Matrix9 *H1, Matrix9 *H2) |
| Merge in a different set of measurements and update bias derivatives accordingly This method is specific to TangentPreintegration backend. | |
Protected Attributes | |
| Matrix9 | preintMeasCov_ |
| COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY]. | |
Friends | |
| template<class PIM> | |
| class | ImuFactorT |
| template<class PIM> | |
| class | ImuFactor2T |
Constructor & Destructor Documentation
◆ PreintegratedImuMeasurementsT() [1/2]
|
inline |
Constructor, initializes the class with no measurements.
- Parameters
-
p Parameters, typically fixed in a single application biasHat Current estimate of acceleration and rotation rate biases
◆ PreintegratedImuMeasurementsT() [2/2]
|
inline |
Construct preintegrated directly from members: base class and preintMeasCov.
- Parameters
-
Base PreintegrationType instance preintMeasCov Covariance matrix used in noise model.
Member Function Documentation
◆ integrateMeasurement()
|
override |
Add a single IMU measurement to the preintegration.
Both accelerometer and gyroscope measurements are taken to be in the sensor frame and conversion to the body frame is handled by body_P_sensor in PreintegrationParams.
- Parameters
-
measuredAcc Measured acceleration (as given by the sensor) measuredOmega Measured angular velocity (as given by the sensor) dt Time interval between this and the last IMU measurement
◆ residualCovariance()
|
inline |
Express the covariance propagated by the selected backend in the chart used by the IMU factor residual.
TangentPreintegration propagates covariance for additive perturbations of \(\zeta=(\theta,p,v)\). Its factor residual instead uses the configured factor-error chart at the predicted state. At zero residual, the differential from the additive chart to either supported residual chart is
\[J = \operatorname{diag}\left(J_r(\theta),\Delta R^T,\Delta R^T\right), \qquad \Delta R=\operatorname{Exp}(\theta), \]
where \(J_r\) is the SO(3) right Jacobian. Therefore its covariance is converted as \(J P J^T\). The component-wise and \(SE_2(3)\) Logmap errors have the same first-order tangent at zero, so this covariance is independent of ImuFactorErrorMode. The other backends already propagate covariance in this tangent.
When a nonzero omegaCoriolis is configured, the inverse rotating-frame lift is applied after the backend chart conversion. This no-argument overload uses the endpoint attitude predicted from identity at biasHat(). Prefer residualCovarianceAt() when a nominal initial state is known. If omegaCoriolis is unset or zero, the attitude choice has no effect.
Neither conversion alters the raw covariance returned by preintMeasCov().
◆ residualCovarianceAt()
|
inline |
Physical endpoint covariance at a fixed nominal predicted attitude.
With a nonzero omegaCoriolis, the inverse transported-velocity lift acts after the backend chart conversion. Freeze this covariance when constructing a factor; it is not differentiated with respect to subsequently optimized states. If omegaCoriolis is unset or zero, predictedAttitude has no effect.
Member Data Documentation
◆ preintMeasCov_
|
protected |
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
(first-order propagation from measurementCovariance).
The documentation for this class was generated from the following files:
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/ImuFactor.h
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/ImuFactor.cpp
