diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a65a4d3f7..7fdafd8ce 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -226,17 +226,13 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + // @deprecated The following function has been deprecated, use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB( - PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected_out, - deltaVij_biascorrected_out)); + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { + PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); pose_j = PVB.pose; vel_j = PVB.velocity; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 50e6c835f..57f4d0e89 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -206,17 +206,13 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + /// @deprecated The following function has been deprecated, use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB( - PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected_out, - deltaVij_biascorrected_out)); + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { + PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); pose_j = PVB.pose; vel_j = PVB.velocity; } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index be604e784..c02aa01c6 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -146,57 +146,50 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( /// Predict state at time j //------------------------------------------------------------------------------ -PoseVelocityBias PreintegrationBase::predict( - const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, - boost::optional deltaPij_biascorrected_out, - boost::optional deltaVij_biascorrected_out) const { - - const imuBias::ConstantBias biasIncr = bias_i - biasHat(); - const Vector3& biasAccIncr = biasIncr.accelerometer(); - const Vector3& biasOmegaIncr = biasIncr.gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Matrix3 Rot_i_matrix = Rot_i.matrix(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------*/ - const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr - + delPdelBiasOmega() * biasOmegaIncr; - if (deltaPij_biascorrected_out) // if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; +PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, + const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, + const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected, + const Vector3& deltaVij_biascorrected, + const bool use2ndOrderCoriolis) const { const double dt = deltaTij(), dt2 = dt * dt; - Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt - - omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * dt2; - const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr - + delVdelBiasOmega() * biasOmegaIncr; - if (deltaVij_biascorrected_out) // if desired, store this - *deltaVij_biascorrected_out = deltaVij_biascorrected; + // Rotation + const Matrix3 Ri = pose_i.rotation().matrix(); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + const Vector3 dR = biascorrectedOmega + - Ri.transpose() * omegaCoriolis * dt; // Coriolis term - Vector3 vel_j = Vector3(vel_i + Rot_i_matrix * deltaVij_biascorrected - - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term - + gravity * dt); + // Translation + Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * gravity * dt2 + - omegaCoriolis.cross(vel_i) * dt2; // Coriolis term - we got rid of the 2 wrt INS paper + + // Velocity + Vector3 dV = Ri * deltaVij_biascorrected + gravity * dt + - 2 * omegaCoriolis.cross(vel_i) * dt; // Coriolis term if (use2ndOrderCoriolis) { - pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // for position - vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // for velocity + Vector3 v = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector())); + dP -= 0.5 * v * dt2; + dV -= v * dt; } - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + // TODO(frank): pose update below is separate expmap for R,t. Is that kosher? + const Pose3 pose_j = Pose3(pose_i.rotation().expmap(dR), pose_i.translation() + Point3(dP)); + return PoseVelocityBias(pose_j, vel_i + dV, bias_i); // bias is predicted as a constant +} - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - const Vector3 correctedOmega = biascorrectedOmega - - Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term - const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); - - const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant +/// Predict state at time j +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict( + const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) const { + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + return predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + biascorrectedDeltaRij(biasIncr.gyroscope()), + biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr), + use2ndOrderCoriolis); } /// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j @@ -213,9 +206,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { - // We need the mismatch w.r.t. the biases used for preintegration - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - // we give some shorter name to rotations and translations const Rot3& Ri = pose_i.rotation(); const Matrix3 Ri_transpose_matrix = Ri.transpose(); @@ -225,10 +215,13 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected, - deltaVij_biascorrected); + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope()); + const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr); + const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr); + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, deltaRij_biascorrected, deltaPij_biascorrected, + deltaVij_biascorrected, use2ndOrderCoriolis); // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector()); @@ -242,8 +235,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect + // TODO(frank): biascorrectedThetaRij calculates deltaRij_biascorrected, which we already have Matrix3 D_cThetaRij_biasOmegaIncr; - const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, + const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasIncr.gyroscope(), H5 ? &D_cThetaRij_biasOmegaIncr : 0); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 85ffae8a2..722091b40 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -138,12 +138,27 @@ class PreintegrationBase : public PreintegratedRotation { Vector3& correctedOmega, boost::optional body_P_sensor); + Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const { + return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + } + + Vector3 biascorrectedDeltaVij(const imuBias::ConstantBias& biasIncr) const { + return deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + } + + /// Predict state at time j, with bias-corrected quantities given + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, + const Rot3& deltaRij_biascorrected, + const Vector3& deltaPij_biascorrected, const Vector3& deltaVij_biascorrected, + const bool use2ndOrderCoriolis = false) const; + /// Predict state at time j - PoseVelocityBias predict( - const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) const; + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,