diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 2d58534aa..f3647fcc0 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -145,15 +145,16 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; - boost::shared_ptr p = boost::make_shared(); + boost::shared_ptr p = Params::MakeSharedFRD(); p->gyroscopeCovariance = measuredOmegaCovariance; p->accelerometerCovariance = measuredAccCovariance; p->integrationCovariance = integrationErrorCovariance; p->biasAccCovariance = biasAccCovariance; p->biasOmegaCovariance = biasOmegaCovariance; p->biasAccOmegaInit = biasAccOmegaInit; - p->use2ndOrderIntegration = use2ndOrderIntegration; p_ = p; resetIntegration(); preintMeasCov_.setZero(); @@ -259,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor( Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& pim, const Vector3& gravity, + const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, @@ -267,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor( _PIM_(pim) { boost::shared_ptr p = boost::make_shared(pim.p()); - p->gravity = gravity; + p->b_gravity = b_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; p->use2ndOrderCoriolis = use2ndOrderCoriolis; @@ -278,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, CombinedPreintegratedMeasurements& pim, - const Vector3& gravity, + const Vector3& b_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict - PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity, omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6bd2f7867..142e8706f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -66,9 +66,26 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration - Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {} + /// See two named constructors below for good values of b_gravity in body frame + Params(const Vector3& b_gravity) : + PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + I_3x3), biasAccOmegaInit(I_6x6) { + } + + // Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis + static boost::shared_ptr MakeSharedFRD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis + static boost::shared_ptr MakeSharedFLU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } private: + /// Default constructor makes unitialized params struct + Params() {} + /** Serialization function */ friend class boost::serialization::access; template @@ -134,12 +151,13 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase { Matrix preintMeasCov() const { return preintMeasCov_; } /// deprecated constructor + /// NOTE(frank): assumes FRD convention, only second order integration supported PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false); + const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); private: /// Serialization function @@ -245,7 +263,7 @@ public: /// @deprecated constructor CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& pim, - const Vector3& gravity, const Vector3& omegaCoriolis, + const Vector3& b_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); @@ -253,7 +271,7 @@ public: static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, CombinedPreintegratedMeasurements& pim, - const Vector3& gravity, const Vector3& omegaCoriolis, + const Vector3& b_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 058ea1538..1b3736bec 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -38,8 +38,8 @@ void PreintegratedImuMeasurements::print(const string& s) const { //------------------------------------------------------------------------------ bool PreintegratedImuMeasurements::equals( const PreintegratedImuMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) && - equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); + return PreintegrationBase::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ @@ -51,8 +51,7 @@ void PreintegratedImuMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - OptionalJacobian<9, 9> F_test, - OptionalJacobian<9, 9> G_test) { + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, @@ -60,16 +59,17 @@ void PreintegratedImuMeasurements::integrateMeasurement( // rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr // rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // Update Jacobians - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, + deltaT); // Update preintegrated measurements (also get Jacobian) - const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test - Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) + const Matrix3 dRij = deltaRij_.matrix(); // store this, which is useful to compute G_test + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); // first order covariance propagation: @@ -88,14 +88,13 @@ void PreintegratedImuMeasurements::integrateMeasurement( * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; - if (p().use2ndOrderIntegration) { - F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; - preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc - * p().accelerometerCovariance * F_pos_noiseacc.transpose(); - Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT - preintMeasCov_.block<3, 3>(0, 3) += temp; - preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); - } + F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; + preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + * p().accelerometerCovariance * F_pos_noiseacc.transpose(); + Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance + * dRij.transpose(); // has 1/deltaT + preintMeasCov_.block<3, 3>(0, 3) += temp; + preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); // F_test and G_test are given as output for testing purposes and are not needed by the factor if (F_test) { @@ -103,13 +102,10 @@ void PreintegratedImuMeasurements::integrateMeasurement( } if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise - if (!p().use2ndOrderIntegration) - F_pos_noiseacc = Z_3x3; - // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, dRij * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, dRij * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } //------------------------------------------------------------------------------ @@ -117,12 +113,13 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; - boost::shared_ptr p = boost::make_shared(); + boost::shared_ptr p = Params::MakeSharedFRD(); p->gyroscopeCovariance = measuredOmegaCovariance; p->accelerometerCovariance = measuredAccCovariance; p->integrationCovariance = integrationErrorCovariance; - p->use2ndOrderIntegration = use2ndOrderIntegration; p_ = p; resetIntegration(); } @@ -131,10 +128,10 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedImuMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias), - _PIM_(pim) {} + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -151,7 +148,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { Base::print(""); _PIM_.print(" preintegrated measurements:"); // Print standard deviations on covariance only - cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() + << endl; } //------------------------------------------------------------------------------ @@ -168,21 +166,19 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const { return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - H1, H2, H3, H4, H5); + H1, H2, H3, H4, H5); } //------------------------------------------------------------------------------ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& pim, - const Vector3& gravity, const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor, - const bool use2ndOrderCoriolis) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias), - _PIM_(pim) { - boost::shared_ptr p = - boost::make_shared(pim.p()); - p->gravity = gravity; + const PreintegratedMeasurements& pim, const Vector3& b_gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { + boost::shared_ptr p = boost::make_shared< + PreintegratedMeasurements::Params>(pim.p()); + p->b_gravity = b_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; p->use2ndOrderCoriolis = use2ndOrderCoriolis; @@ -191,16 +187,14 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, //------------------------------------------------------------------------------ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - PreintegratedMeasurements& pim, - const Vector3& gravity, const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, + PreintegratedMeasurements& pim, const Vector3& b_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // use deprecated predict - PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, gravity, + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity, omegaCoriolis, use2ndOrderCoriolis); pose_j = pvb.pose; vel_j = pvb.velocity; } -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 0b86472f5..c739120d3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -108,11 +108,12 @@ public: Matrix preintMeasCov() const { return preintMeasCov_; } /// @deprecated constructor + /// NOTE(frank): assumes FRD convention, only second order integration supported PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, - bool use2ndOrderIntegration = false); + bool use2ndOrderIntegration = true); private: @@ -209,14 +210,14 @@ public: /// @deprecated constructor ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, + const Vector3& b_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); /// @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, - PreintegratedMeasurements& pim, const Vector3& gravity, + PreintegratedMeasurements& pim, const Vector3& b_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index b63f25b4d..8ce8a200b 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -248,25 +248,19 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, } //------------------------------------------------------------------------------ -Vector9 NavState::predictXi(const Vector9& pim, double dt, - const Vector3& gravity, const boost::optional& omegaCoriolis, - bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) const { +Vector9 NavState::integrateTangent(const Vector9& pim, double dt, + const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! const double dt2 = dt * dt; Vector9 delta; - Matrix3 D_dP_Ri, D_dP_dP, D_dV_Ri, D_dV_dV; + Matrix3 D_dP_Ri, D_dP_nv; dR(delta) = dR(pim); - // TODO(frank): - // - why do we integrate n_v here ? - // - the dP and dV should be in body ! How come semi-retract works out ?? - // - should we rename t_ to p_? if not, we should rename dP do dT - dP(delta) = nRb.rotate(dP(pim), H1 ? &D_dP_Ri : 0, H2 ? &D_dP_dP : 0) - + n_v * dt + 0.5 * gravity * dt2; - dV(delta) = nRb.rotate(dV(pim), H1 ? &D_dV_Ri : 0, H2 ? &D_dV_dV : 0) - + gravity * dt; + dP(delta) = dP(pim) + + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0); + dV(delta) = dV(pim); if (omegaCoriolis) { delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); @@ -278,15 +272,11 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt, if (H1) { if (!omegaCoriolis) H1->setZero(); // if coriolis H1 is already initialized - D_t_R(H1) += D_dP_Ri; - D_t_v(H1) += I_3x3 * dt * Ri; - D_v_R(H1) += D_dV_Ri; + D_t_R(H1) += dt * D_dP_Ri; + D_t_v(H1) += dt * D_dP_nv * Ri; } if (H2) { - H2->setZero(); - D_R_R(H2) << I_3x3; - D_t_t(H2) << D_dP_dP; - D_v_v(H2) << D_dV_dV; + H2->setIdentity(); } } @@ -294,13 +284,12 @@ Vector9 NavState::predictXi(const Vector9& pim, double dt, } //------------------------------------------------------------------------------ NavState NavState::predict(const Vector9& pim, double dt, - const Vector3& gravity, const boost::optional& omegaCoriolis, - bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) const { + const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { Matrix9 D_delta_state, D_delta_pim; - Vector9 delta = predictXi(pim, dt, gravity, omegaCoriolis, - use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0); + Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis, + H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0); Matrix9 D_predicted_state, D_predicted_delta; NavState predicted = retract(delta, H1 ? &D_predicted_state : 0, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index ed0345ada..7326b8df7 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -33,6 +33,9 @@ typedef Vector3 Velocity3; */ class NavState: public LieGroup { private: + + // TODO(frank): + // - should we rename t_ to p_? if not, we should rename dP do dT Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav Point3 t_; ///< position n_t, in nav frame Velocity3 v_; ///< velocity n_v in nav frame @@ -207,16 +210,16 @@ public: Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, OptionalJacobian<9, 9> H = boost::none) const; - /// Combine preintegrated measurements, in the form of a tangent space vector, - /// with gravity, velocity, and Coriolis forces into a tangent space update. - Vector9 predictXi(const Vector9& pim, double dt, const Vector3& gravity, + /// Integrate a tangent vector forwards on tangent space, taking into account + /// Coriolis forces if omegaCoriolis is given. + Vector9 integrateTangent(const Vector9& pim, double dt, const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; - /// Combine preintegrated measurements, in the form of a tangent space vector, - /// with gravity, velocity, and Coriolis forces into a new state. - NavState predict(const Vector9& pim, double dt, const Vector3& gravity, + /// Integrate a tangent vector forwards on tangent space, taking into account + /// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState + NavState predict(const Vector9& pim, double dt, const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = false, OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = boost::none) const; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index e2f1466a6..2c4694e41 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -64,14 +64,11 @@ void PreintegrationBase::updatePreintegratedMeasurements( OptionalJacobian<9, 9> F) { const Matrix3 dRij = deltaRij_.matrix(); // expensive - const Vector3 temp = dRij * correctedAcc * deltaT; + const Vector3 j_acc = dRij * correctedAcc; // acceleration in current frame - if (!p().use2ndOrderIntegration) { - deltaPij_ += deltaVij_ * deltaT; - } else { - deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; - } - deltaVij_ += temp; + double dt22 = 0.5 * deltaT * deltaT; + deltaPij_ += deltaVij_ * deltaT + dt22 * j_acc; + deltaVij_ += deltaT * j_acc; Matrix3 R_i, F_angles_angles; if (F) @@ -81,10 +78,7 @@ void PreintegrationBase::updatePreintegratedMeasurements( if (F) { const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_pos_angles; - if (p().use2ndOrderIntegration) - F_pos_angles = 0.5 * F_vel_angles * deltaT; - else - F_pos_angles = Z_3x3; + F_pos_angles = 0.5 * F_vel_angles * deltaT; // pos vel angle *F << // @@ -101,13 +95,8 @@ void PreintegrationBase::updatePreintegratedJacobians( const Matrix3 dRij = deltaRij_.matrix(); // expensive const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - if (!p().use2ndOrderIntegration) { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - } else { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; - delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); - } + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasOmega_ += temp; update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); @@ -135,10 +124,9 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( //------------------------------------------------------------------------------ Vector9 PreintegrationBase::biasCorrectedDelta( const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - Matrix3 D_deltaRij_bias; - Rot3 deltaRij = PreintegratedRotation::biascorrectedDeltaRij( - biasIncr.gyroscope(), H ? &D_deltaRij_bias : 0); + Rot3 deltaRij = biascorrectedDeltaRij(biasIncr.gyroscope()); Vector9 xi; Matrix3 D_dR_deltaRij; @@ -147,9 +135,10 @@ Vector9 PreintegrationBase::biasCorrectedDelta( + delPdelBiasOmega_ * biasIncr.gyroscope(); NavState::dV(xi) = deltaVij_ + delVdelBiasAcc_ * biasIncr.accelerometer() + delVdelBiasOmega_ * biasIncr.gyroscope(); + if (H) { Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; - D_dR_bias << Z_3x3, D_dR_deltaRij * D_deltaRij_bias; + D_dR_bias << Z_3x3, D_dR_deltaRij * delRdelBiasOmega_; D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; (*H) << D_dR_bias, D_dP_bias, D_dV_bias; @@ -161,13 +150,23 @@ Vector9 PreintegrationBase::biasCorrectedDelta( NavState PreintegrationBase::predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const { + // correct for bias Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); + + // integrate on tangent space Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.predictXi(biasCorrected, deltaTij_, p().gravity, + Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0); + + // Correct for gravity + double dt = deltaTij_, dt2 = dt * dt; + NavState::dP(xi) += 0.5 * p().b_gravity * dt2; + NavState::dV(xi) += p().b_gravity * dt; + + // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); if (H1) @@ -313,11 +312,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, //------------------------------------------------------------------------------ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, + const Vector3& b_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility boost::shared_ptr q = boost::make_shared(p()); - q->gravity = gravity; + q->b_gravity = b_gravity; q->omegaCoriolis = omegaCoriolis; q->use2ndOrderCoriolis = use2ndOrderCoriolis; p_ = q; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 2ee6b79e6..3079cd5c8 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace gtsam { @@ -32,11 +33,16 @@ struct PoseVelocityBias { Pose3 pose; Vector3 velocity; imuBias::ConstantBias bias; - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) - : pose(_pose), velocity(_velocity), bias(_bias) {} - PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) - : pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {} - NavState navState() const { return NavState(pose,velocity);} + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } + PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) : + pose(navState.pose()), velocity(navState.velocity()), bias(_bias) { + } + NavState navState() const { + return NavState(pose, velocity); + } }; /** @@ -45,29 +51,41 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase : public PreintegratedRotation { +class PreintegrationBase: public PreintegratedRotation { - public: +public: /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params : PreintegratedRotation::Params { - Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + struct Params: PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty /// (to compensate errors in Euler integration) - bool use2ndOrderIntegration; ///< Controls the order of integration /// (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) - bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration - Vector3 gravity; ///< Gravity constant + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 b_gravity; ///< Gravity constant in body frame - Params() - : accelerometerCovariance(I_3x3), - integrationCovariance(I_3x3), - use2ndOrderIntegration(false), - use2ndOrderCoriolis(false), - gravity(0, 0, 9.8) {} + /// The Params constructor insists on the user passing in gravity in the *body* frame. + /// For convenience, two commonly used conventions are provided by named constructors below + Params(const Vector3& b_gravity) : + accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( + false), b_gravity(b_gravity) { + } + + // Default Params for Front-Right-Down convention: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedFRD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for Front-Left-Up convention: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedFLU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + protected: + /// Default constructor for serialization only: uninitialized! + Params(); - private: /** Serialization function */ friend class boost::serialization::access; template @@ -75,29 +93,29 @@ class PreintegrationBase : public PreintegratedRotation { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); ar & BOOST_SERIALIZATION_NVP(integrationCovariance); - ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); - ar & BOOST_SERIALIZATION_NVP(gravity); + ar & BOOST_SERIALIZATION_NVP(b_gravity); } }; - protected: +protected: /// Acceleration and gyro bias used for preintegration imuBias::ConstantBias biasHat_; - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - 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 + 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 /// Default constructor for serialization - PreintegrationBase() {} + PreintegrationBase() { + } - public: +public: /** * Constructor, initializes the variables in the base class @@ -105,27 +123,45 @@ class PreintegrationBase : public PreintegratedRotation { * @param p Parameters, typically fixed in a single application */ PreintegrationBase(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) - : PreintegratedRotation(p), biasHat_(biasHat) { + const imuBias::ConstantBias& biasHat) : + PreintegratedRotation(p), biasHat_(biasHat) { resetIntegration(); } /// Re-initialize PreintegratedMeasurements void resetIntegration(); - const Params& p() const { return *boost::static_pointer_cast(p_);} + const Params& p() const { + return *boost::static_pointer_cast(p_); + } /// getters - const imuBias::ConstantBias& biasHat() const { return biasHat_; } - const Vector3& deltaPij() const { return deltaPij_; } - const Vector3& deltaVij() const { return deltaVij_; } - const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } - const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } - const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } - const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } + const imuBias::ConstantBias& biasHat() const { + return biasHat_; + } + const Vector3& deltaPij() const { + return deltaPij_; + } + const Vector3& deltaVij() const { + return deltaVij_; + } + const Matrix3& delPdelBiasAcc() const { + return delPdelBiasAcc_; + } + const Matrix3& delPdelBiasOmega() const { + return delPdelBiasOmega_; + } + const Matrix3& delVdelBiasAcc() const { + return delVdelBiasAcc_; + } + const Matrix3& delVdelBiasOmega() const { + return delVdelBiasOmega_; + } // Exposed for MATLAB - Vector6 biasHatVector() const { return biasHat_.vector(); } + Vector6 biasHatVector() const { + return biasHat_.vector(); + } /// print void print(const std::string& s) const; @@ -134,18 +170,16 @@ class PreintegrationBase : public PreintegratedRotation { bool equals(const PreintegrationBase& other, double tol) const; /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, - const double deltaT, OptionalJacobian<9, 9> F); + void updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F); /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT); + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT); void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, - Vector3* correctedAcc, - Vector3* correctedOmega); + const Vector3& measuredOmega, Vector3* correctedAcc, + Vector3* correctedOmega); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far @@ -154,23 +188,23 @@ class PreintegrationBase : public PreintegratedRotation { /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = boost::none) const; + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = + boost::none) 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, - 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; + 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; /// @deprecated predict 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 imuBias::ConstantBias& bias_i, const Vector3& b_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - private: +private: /** Serialization function */ friend class boost::serialization::access; template @@ -187,4 +221,4 @@ class PreintegrationBase : public PreintegratedRotation { } }; -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 33409a7f8..a2746f1ea 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -57,15 +57,11 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& correctedAcc, const Vector3& correctedOmega, - const double deltaT, const bool use2ndOrderIntegration_) { + const double deltaT) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * correctedAcc * deltaT; Vector3 deltaPij_new; - if (!use2ndOrderIntegration_) { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - } else { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - } + deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; Vector3 deltaVij_new = deltaVij_old + temp; Vector result(6); @@ -93,11 +89,9 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; /* ************************************************************************* */ PreintegratedImuMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs, - const bool use2ndOrderIntegration = false) { + const list& measuredOmegas, const list& deltaTs) { PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance, - use2ndOrderIntegration); + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -142,27 +136,22 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { } // namespace /* ************************************************************************* */ -namespace straight { -// Set up IMU measurements -static const double g = 10; // make this easy to check -static const double a = 0.2, dt = 3.0; -const double exact = dt * dt / 2; -Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); +TEST(ImuFactor, StraightLine) { + // Set up IMU measurements + static const double g = 10; // make this easy to check + static const double a = 0.2, dt = 3.0; + const double exact = dt * dt / 2; + Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); -// Set up body pointing towards y axis, and start at 10,20,0 with zero velocity -// TODO(frank): clean up Rot3 mess -static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -static const Point3 initial_position(10, 20, 0); -static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0)); -} - -TEST(ImuFactor, StraightLineSecondOrder) { - using namespace straight; + // Set up body pointing towards y axis, and start at 10,20,0 with zero velocity + // TODO(frank): clean up Rot3 mess + static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + static const Point3 initial_position(10, 20, 0); + static const NavState state1(nRb, initial_position, Velocity3(0, 0, 0)); imuBias::ConstantBias biasHat, bias; boost::shared_ptr p = PreintegratedImuMeasurements::Params::MakeSharedFRD(); - p->use2ndOrderIntegration = true; p->b_gravity = Vector3(0, 0, g); // FRD convention PreintegratedImuMeasurements pim(p, biasHat); @@ -186,39 +175,6 @@ TEST(ImuFactor, StraightLineSecondOrder) { EXPECT(assert_equal(expected, pim.predict(state1, bias))); } -TEST(ImuFactor, StraightLineFirstOrder) { - using namespace straight; - - imuBias::ConstantBias biasHat, bias; - boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedFRD(); - p->use2ndOrderIntegration = false; - p->b_gravity = Vector3(0, 0, g); // FRD convention - - // We do a rough approximation: - PreintegratedImuMeasurements pim(p, biasHat); - for (size_t i = 0; i < 10; i++) - pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); - - // Check integrated quantities in body frame: gravity measured by IMU is integrated! - const double approx = exact * 0.9; // approximation for dt split into 10 intervals - EXPECT(assert_equal(Rot3(), pim.deltaRij())); - EXPECT(assert_equal(Vector3(a * approx, 0, -g * approx), pim.deltaPij())); - EXPECT(assert_equal(Vector3(a * dt, 0, -g * dt), pim.deltaVij())); // still correct - - // Check bias-corrected delta: also in body frame - Vector9 expectedBC; - expectedBC << 0, 0, 0, a * approx, 0, -g * approx, a * dt, 0, -g * dt; - EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(bias))); - - // Check prediction, note we move along x in body, along y in nav - // In this instance the position is just an approximation, - // but gravity should be subtracted out exactly - NavState expected(nRb, Point3(10, 20 + a * approx, 0), Velocity3(0, a * dt, 0)); - GTSAM_PRINT(pim); - EXPECT(assert_equal(expected, pim.predict(state1, bias))); -} - /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { // Linearization point @@ -236,11 +192,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); - bool use2ndOrderIntegration = true; // Actual preintegrated values PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance, - use2ndOrderIntegration); + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT( @@ -294,12 +248,11 @@ double deltaT = 1.0; TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; boost::shared_ptr p = - PreintegratedImuMeasurements::Params::MakeSharedFRD(); + PreintegratedImuMeasurements::Params::MakeSharedFRD(); p->gyroscopeCovariance = kMeasuredOmegaCovariance; p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); p->accelerometerCovariance = kMeasuredAccCovariance; p->integrationCovariance = kIntegrationErrorCovariance; - p->use2ndOrderIntegration = false; p->use2ndOrderCoriolis = true; PreintegratedImuMeasurements pim(p, bias); @@ -332,10 +285,8 @@ TEST(ImuFactor, PreintegrationBaseMethods) { /* ************************************************************************* */ TEST(ImuFactor, ErrorAndJacobians) { using namespace common; - bool use2ndOrderIntegration = true; PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance, - use2ndOrderIntegration); + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6)); @@ -631,11 +582,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } - bool use2ndOrderIntegration = false; // Actual preintegrated values PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs, use2ndOrderIntegration); + deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians @@ -658,20 +608,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { // Compute expected f_pos_vel wrt positions Matrix dfpv_dpos = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaPij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old); // Compute expected f_pos_vel wrt velocities Matrix dfpv_dvel = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaVij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old); // Compute expected f_pos_vel wrt angles Matrix dfpv_dangle = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaRij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old); Matrix FexpectedTop6(6, 9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; @@ -698,14 +645,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { // Compute jacobian wrt acc noise Matrix dgpv_daccNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); + deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc); // Compute expected F wrt gyro noise Matrix dgpv_domegaNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), - newMeasuredOmega); + deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega); Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; @@ -754,11 +699,10 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } - bool use2ndOrderIntegration = true; // Actual preintegrated values PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs, use2ndOrderIntegration); + deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians @@ -781,20 +725,17 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { // Compute expected f_pos_vel wrt positions Matrix dfpv_dpos = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaPij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old); // Compute expected f_pos_vel wrt velocities Matrix dfpv_dvel = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaVij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old); // Compute expected f_pos_vel wrt angles Matrix dfpv_dangle = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaRij_old); + newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old); Matrix FexpectedTop6(6, 9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; @@ -821,14 +762,12 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { // Compute jacobian wrt acc noise Matrix dgpv_daccNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); + deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc); // Compute expected F wrt gyro noise Matrix dgpv_domegaNoise = numericalDerivative11( boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), - newMeasuredOmega); + deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega); Matrix GexpectedTop6(6, 9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 683a97d50..e5f25ffa5 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -178,12 +178,12 @@ TEST(NavState, PredictXi) { xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 actualH1, actualH2; - boost::function predictXi = - boost::bind(&NavState::predictXi, _1, _2, dt, kGravity, kOmegaCoriolis, + boost::function integrateTangent = + boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis, false, boost::none, boost::none); - kState1.predictXi(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); - EXPECT(assert_equal(numericalDerivative21(predictXi, kState1, xi), actualH1)); - EXPECT(assert_equal(numericalDerivative22(predictXi, kState1, xi), actualH2)); + kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1)); + EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2)); } /* ************************************************************************* */