//------------------------------------------------------------------------------ void ManifoldPreintegration::update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, Matrix93* C) { // Correct for bias in the sensor frame Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); Vector3 omega = biasHat_.correctGyroscope(measuredOmega); // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; if (p().body_P_sensor) boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); // Do update deltaTij_ += dt; deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional if (p().body_P_sensor) { // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; if (!p().body_P_sensor->translation().isZero()) *C += *B * D_correctedAcc_omega; *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } // Update Jacobians // TODO(frank): Try same simplification as in new approach Matrix3 D_acc_R; oldRij.rotate(acc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; const Vector3 integratedOmega = omega * dt; Matrix3 D_incrR_integratedOmega; const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; double dt22 = 0.5 * dt * dt; const Matrix3 dRij = oldRij.matrix(); // expensive delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; delVdelBiasAcc_ += -dRij * dt; delVdelBiasOmega_ += D_acc_biasOmega * dt; }
//------------------------------------------------------------------------------ void PreintegrationBase::update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, const double dt, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); // Do update deltaTij_ += dt; deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional // Update Jacobians // TODO(frank): we are repeating some computation here: accessible in F ? Vector3 j_correctedAcc, j_correctedOmega; boost::tie(j_correctedAcc, j_correctedOmega) = correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); Matrix3 D_acc_R; oldRij.rotate(j_correctedAcc, D_acc_R); const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; const Vector3 integratedOmega = j_correctedOmega * dt; const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! const Matrix3 incrRt = incrR.transpose(); delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt; double dt22 = 0.5 * dt * dt; const Matrix3 dRij = oldRij.matrix(); // expensive delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; delVdelBiasAcc_ += -dRij * dt; delVdelBiasOmega_ += D_acc_biasOmega * dt; }