|
| PreintegratedImuMeasurements () |
| Default constructor for serialization and wrappers.
|
|
| PreintegratedImuMeasurements (const std::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) |
|
| PreintegratedImuMeasurements (const PreintegrationType &base, const Matrix9 &preintMeasCov) |
|
| ~PreintegratedImuMeasurements () override |
| Virtual destructor.
|
|
void | print (const std::string &s="Preintegrated Measurements:") const override |
| print
|
|
bool | equals (const PreintegratedImuMeasurements &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 |
|
void | integrateMeasurements (const Matrix &measuredAccs, const Matrix &measuredOmegas, const Matrix &dts) |
| Add multiple measurements, in matrix columns.
|
|
Matrix | preintMeasCov () const |
| Return pre-integrated measurement covariance.
|
|
|
NavState | deltaXij () const override |
|
Rot3 | deltaRij () const override |
|
Vector3 | deltaPij () const override |
|
Vector3 | deltaVij () const override |
|
Matrix3 | delRdelBiasOmega () const |
|
Matrix3 | delPdelBiasAcc () const |
|
Matrix3 | delPdelBiasOmega () const |
|
Matrix3 | delVdelBiasAcc () const |
|
Matrix3 | delVdelBiasOmega () const |
|
|
bool | equals (const ManifoldPreintegration &other, double tol) const |
|
|
void | update (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C) override |
|
Vector9 | biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H={}) const override |
|
virtual std::shared_ptr< ManifoldPreintegration > | clone () const |
|
|
Re-initialize PreintegratedMeasurements and set new bias
|
void | resetIntegrationAndSetBias (const Bias &biasHat) |
|
bool | matchesParamsWith (const PreintegrationBase &other) const |
| check parameters equality: checks whether shared pointer points to same Params object.
|
|
const std::shared_ptr< Params > & | params () const |
| shared pointer to params
|
|
Params & | p () const |
| const reference to params
|
|
|
const imuBias::ConstantBias & | biasHat () const |
|
double | deltaTij () const |
|
Vector6 | biasHatVector () const |
|
|
std::pair< Vector3, Vector3 > | correctMeasurementsBySensorPose (const Vector3 &unbiasedAcc, const Vector3 &unbiasedOmega, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedAcc={}, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega={}, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega={}) const |
|
NavState | predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 6 > H2={}) const |
| Predict state at time j.
|
|
Vector9 | computeError (const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1, OptionalJacobian< 9, 9 > H2, OptionalJacobian< 9, 6 > H3) const |
| Calculate error given navStates.
|
|
Vector9 | computeErrorAndJacobians (const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 6 > H3={}, OptionalJacobian< 9, 3 > H4={}, OptionalJacobian< 9, 6 > H5={}) const |
|
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.