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.
|
|
| PreintegratedCombinedMeasurements () |
| Default constructor only for serialization and wrappers.
|
|
| PreintegratedCombinedMeasurements (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) |
| Default constructor, initializes the class with no measurements.
|
|
| PreintegratedCombinedMeasurements (const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov) |
| Construct preintegrated directly from members: base class and preintMeasCov.
|
|
| ~PreintegratedCombinedMeasurements () override |
| Virtual destructor.
|
|
|
void | resetIntegration () override |
| Re-initialize PreintegratedCombinedMeasurements.
|
|
Params & | p () const |
| const reference to params, shadows definition in base class
|
|
|
Return pre-integrated measurement covariance
|
Matrix | preintMeasCov () const |
|
|
|
void | print (const std::string &s="Preintegrated Measurements:") const override |
|
bool | equals (const PreintegratedCombinedMeasurements &expected, double tol=1e-9) const |
| equals
|
|
|
void | integrateMeasurement (const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override |
| Add a single IMU measurement to the preintegration.
|
|
| ManifoldPreintegration (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) |
| Constructor, initializes the variables in the base class.
|
|
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 |
| Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose NOTE(frank): implementation is different in two versions.
|
|
Vector9 | biasCorrectedDelta (const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const override |
| Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU measurements so far NOTE(frank): implementation is different in two versions.
|
|
virtual boost::shared_ptr< ManifoldPreintegration > | clone () const |
| Dummy clone for MATLAB.
|
|
| PreintegrationBase (const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) |
| Constructor, initializes the variables in the base class.
|
|
void | resetIntegrationAndSetBias (const Bias &biasHat) |
|
bool | matchesParamsWith (const PreintegrationBase &other) const |
| check parameters equality: checks whether shared pointer points to same Params object.
|
|
const boost::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=boost::none, OptionalJacobian< 3, 3 > correctedAcc_H_unbiasedOmega=boost::none, OptionalJacobian< 3, 3 > correctedOmega_H_unbiasedOmega=boost::none) const |
| Subtract estimate and correct for sensor pose Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) Ignore D_correctedOmega_measuredAcc as it is trivially zero.
|
|
NavState | predict (const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 6 > H2=boost::none) 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=boost::none, OptionalJacobian< 9, 3 > H2=boost::none, OptionalJacobian< 9, 6 > H3=boost::none, OptionalJacobian< 9, 3 > H4=boost::none, OptionalJacobian< 9, 6 > H5=boost::none) const |
| Compute errors w.r.t.
|
|
|
Eigen::Matrix< double, 15, 15 > | preintMeasCov_ |
|
NavState | deltaXij_ |
| Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into account velocity at time i, see deltap+, in [2] Note: velocity is now also in frame i, as opposed to deltaVij in [2].
|
|
Matrix3 | delRdelBiasOmega_ |
| Jacobian of preintegrated rotation w.r.t. angular rate bias.
|
|
Matrix3 | delPdelBiasAcc_ |
| Jacobian of preintegrated position w.r.t. acceleration bias.
|
|
Matrix3 | delPdelBiasOmega_ |
| Jacobian of preintegrated position w.r.t. angular rate bias.
|
|
Matrix3 | delVdelBiasAcc_ |
| Jacobian of preintegrated velocity w.r.t. acceleration bias.
|
|
Matrix3 | delVdelBiasOmega_ |
| Jacobian of preintegrated velocity w.r.t. angular rate bias.
|
|
boost::shared_ptr< Params > | p_ |
|
Bias | biasHat_ |
| Acceleration and gyro bias used for preintegration.
|
|
double | deltaTij_ |
| Time interval from i to j.
|
|