Detailed Description
class gtsam::PreintegratedCombinedMeasurementsT< PreintegrationType >
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix.
The measurements are then used to build the CombinedImuFactor. 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 | |
Constructors | |
| PreintegratedCombinedMeasurementsT () | |
| Default constructor only for serialization and wrappers. | |
| PreintegratedCombinedMeasurementsT (const std::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias(), const Eigen::Matrix< double, 15, 15 > &preintMeasCov=Eigen::Matrix< double, 15, 15 >::Zero()) | |
| Default constructor, initializes the class with no measurements. | |
| PreintegratedCombinedMeasurementsT (const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov) | |
| Construct preintegrated directly from members: base class and preintMeasCov. | |
| ~PreintegratedCombinedMeasurementsT () override | |
| Virtual destructor. | |
Basic utilities | |
| void | resetIntegration () override |
| Re-initialize PreintegratedCombinedMeasurements. | |
| Params & | p () const |
| const reference to params, shadows definition in base class | |
Access instance variables | |
Return pre-integrated measurement covariance | |
| Matrix | preintMeasCov () const |
| Matrix | residualCovariance () const |
| Express the propagated covariance in the combined IMU factor residual chart. | |
| Matrix | residualCovarianceAt (const Rot3 &predictedAttitude) const |
| Physical endpoint covariance at a fixed nominal predicted attitude. | |
Testable | |
| void | print (const std::string &s="Preintegrated Measurements:") const override |
| bool | equals (const PreintegratedCombinedMeasurementsT< PreintegrationType > &expected, double tol=1e-9) const |
| equals | |
Main functionality | |
| void | integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override |
| Add a single IMU measurement to the preintegration. | |
Public Types | |
| typedef PreintegrationCombinedParams | Params |
Protected Attributes | |
| Eigen::Matrix< double, 15, 15 > | preintMeasCov_ |
Friends | |
| template<class PIM> | |
| class | CombinedImuFactorT |
Constructor & Destructor Documentation
◆ PreintegratedCombinedMeasurementsT() [1/2]
|
inline |
Default 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 preintMeasCov Covariance matrix used in noise model.
◆ PreintegratedCombinedMeasurementsT() [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 two consecutive IMU measurements
◆ residualCovariance()
|
inline |
Express the propagated covariance in the combined IMU factor residual chart.
The first nine rows use the tangent shared by both supported factor-error charts. TangentPreintegration propagates them in additive \((\theta,p,v)\) coordinates, whose differential into that chart is
\[J_9 = \operatorname{diag} \left(J_r(\theta),\Delta R^T,\Delta R^T\right). \]
The component-wise and \(SE_2(3)\) Logmap errors have the same first-order tangent at zero, so this conversion is independent of ImuFactorErrorMode. The other backends already propagate these rows in that tangent, so their \(J_9\) is identity.
The final six propagated coordinates follow the bias change \(b_j-b_i\), whereas the factor residual is \(b_i-b_j\). The backend chart and bias-sign conversion is
\[J_{15}=\operatorname{diag}(J_9,-I_6), \qquad P_{\mathrm{res}}=J_{15}P_{\mathrm{native}}J_{15}^T. \]
The bias sign leaves its marginal covariance unchanged but reverses the state–bias cross-covariances. With a nonzero omegaCoriolis, the inverse rotating-frame lift is also applied. This method changes neither the nonlinear residual nor the raw covariance returned by preintMeasCov(). The endpoint attitude defaults to prediction from identity at biasHat(); use residualCovarianceAt() for a known nominal initial state. If omegaCoriolis is unset or zero, the attitude choice has no effect.
◆ 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 and bias-sign conversions. 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.
The documentation for this class was generated from the following files:
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/CombinedImuFactor.h
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/navigation/CombinedImuFactor.cpp
