//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_ = Z_3x3;
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_.setZero();
delPdelBiasAcc_.setZero();
delPdelBiasOmega_.setZero();
delVdelBiasAcc_.setZero();
delVdelBiasOmega_.setZero();
}
//------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->n_gravity = n_gravity;
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}