diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 5697d54d42..08ff1c871c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -255,5 +255,101 @@ Vector ImuFactor2::evaluateError(const NavState& state_i, //------------------------------------------------------------------------------ +//------------------------------------------------------------------------------ +// ImuFactorWithGravity methods +//------------------------------------------------------------------------------ +ImuFactorWithGravity::ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, Key gravity, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias, gravity), _PIM_(pim) { +} + +//------------------------------------------------------------------------------ +NonlinearFactor::shared_ptr ImuFactorWithGravity::clone() const { +return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactorWithGravity& f) { +f._PIM_.print("preintegrated measurements:\n"); +os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); +return os; +} + +//------------------------------------------------------------------------------ +void ImuFactorWithGravity::print(const string& s, const KeyFormatter& keyFormatter) const { +cout << (s.empty() ? s : s + "\n") << "ImuFactorWithGravity(" << keyFormatter(this->key<1>()) + << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) + << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>()) + << ")\n"; +cout << *this << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactorWithGravity::equals(const NonlinearFactor& other, double tol) const { +const This *e = dynamic_cast(&other); +const bool base = Base::equals(*e, tol); +const bool pim = _PIM_.equals(e->_PIM_, tol); +return e != nullptr && base && pim; +} + +//------------------------------------------------------------------------------ +Vector ImuFactorWithGravity::evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Unit3& gravity, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const { + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, gravity, + H1, H2, H3, H4, H5, H6); +} + +//------------------------------------------------------------------------------ +#ifdef GTSAM_TANGENT_PREINTEGRATION +PreintegratedImuMeasurements ImuFactorWithGravity::Merge( + const PreintegratedImuMeasurements& pim01, + const PreintegratedImuMeasurements& pim12) { +if (!pim01.matchesParamsWith(pim12)) +throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); + +if (pim01.params()->body_P_sensor) +throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + +// the bias for the merged factor will be the bias from 01 +PreintegratedImuMeasurements pim02 = pim01; + +Matrix9 H1, H2; +pim02.mergeWith(pim12, &H1, &H2); + +return pim02; +} + +//------------------------------------------------------------------------------ +ImuFactorWithGravity::shared_ptr ImuFactorWithGravity::Merge(const shared_ptr& f01, + const shared_ptr& f12) { +// IMU bias keys must be the same. +if (f01->key<5>() != f12->key<5>()) +throw std::domain_error("ImuFactorWithGravity::Merge: IMU bias keys must be the same"); + +// expect intermediate pose, velocity keys to matchup. +if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>()) +throw std::domain_error( + "ImuFactorWithGravity::Merge: intermediate pose, velocity keys need to match up"); + +// return new factor +auto pim02 = +Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); +return std::make_shared(f01->key<1>(), // P0 + f01->key<2>(), // V0 + f12->key<3>(), // P2 + f12->key<4>(), // V2 + f01->key<5>(), // B + f01->key<6>(), // G + pim02); +} +#endif + } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2e6dff0dc8..2138e38c0e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -69,6 +69,7 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { friend class ImuFactor; friend class ImuFactor2; + friend class ImuFactorWithGravity; protected: @@ -331,6 +332,113 @@ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { +private: + + typedef ImuFactorWithGravity This; + typedef NoiseModelFactorN Base; + + PreintegratedImuMeasurements _PIM_; + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename std::shared_ptr shared_ptr; +#else + typedef std::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + ImuFactorWithGravity() {} + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param gravity Gravity key + * @param preintegratedMeasurements The preintegreated measurements since the + * last pose. + */ + ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, Key gravity, + const PreintegratedImuMeasurements& preintegratedMeasurements); + + ~ImuFactorWithGravity() override { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override; + + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactorWithGravity&); + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + /// @} + + /** Access the preintegrated measurements. */ + + const PreintegratedImuMeasurements& preintegratedMeasurements() const { + return _PIM_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const Unit3& gravity, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, + OptionalMatrixType H6) const override; + +#ifdef GTSAM_TANGENT_PREINTEGRATION + /// Merge two pre-integrated measurement classes + static PreintegratedImuMeasurements Merge( + const PreintegratedImuMeasurements& pim01, + const PreintegratedImuMeasurements& pim12); + + /// Merge two factors + static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); +#endif + + private: + /** Serialization function */ +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + } +#endif +}; +// class ImuFactorWithGravity + template <> struct traits : public Testable {}; @@ -340,4 +448,7 @@ struct traits : public Testable {}; template <> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 116efe90a2..fec45b444b 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -439,24 +439,29 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, const std::optional& omegaCoriolis, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) const { + OptionalJacobian<9, 9> H2, OptionalJacobian<9, 2> H3) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! const double dt22 = 0.5 * dt * dt; + // The gravity is actually made of two components - the constant magnitude and variable direction + Matrix32 D_grav_gdirection = Unit3(n_gravity).basis(); + Vector9 xi; Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; + Matrix3 D_nRb_unrot_grav_P, D_nRb_unrot_grav_V; // Both of these are actually the same dR(xi) = dR(pim); dP(xi) = dP(pim) - + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) - + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); - dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) + + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0, H3 ? &D_nRb_unrot_grav_P : 0); + dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0, + H3 ? &D_nRb_unrot_grav_V : 0); if (omegaCoriolis) { xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); } - if (H1 || H2) { + if (H1 || H2 || H3) { Matrix3 Ri = nRb.matrix(); if (H1) { @@ -469,6 +474,14 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, if (H2) { H2->setIdentity(); } + if (H3) { + // The rotation part of xi does not depend on gravity: + H3->block<3,2>(0,0) = Matrix32::Zero(); + // The position part: derivative is dt22 * (∂(nRb.unrotate(n_gravity))/∂n_gravity) + H3->block<3,2>(3,0) = dt22 * D_nRb_unrot_grav_P * D_grav_gdirection * n_gravity.norm(); + // The velocity part: derivative is dt * D_nRb_unrot_grav_V + H3->block<3,2>(6,0) = dt * D_nRb_unrot_grav_V * D_grav_gdirection * n_gravity.norm(); + } } return xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e246cbe271..5a389b680c 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -262,7 +262,7 @@ class GTSAM_EXPORT NavState : public LieGroup { Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, const std::optional& omegaCoriolis, bool use2ndOrderCoriolis = false, OptionalJacobian<9, 9> H1 = {}, - OptionalJacobian<9, 9> H2 = {}) const; + OptionalJacobian<9, 9> H2 = {}, OptionalJacobian<9,2> H3 = {}) const; /// @} diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index dd86b829c5..2aed5c140e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -115,17 +115,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { + const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2, OptionalJacobian<9, 2> H3) const { Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : nullptr); + + // Compute gravity vector + const Vector3 n_gravity_vec = n_gravity.unitVector() * p().n_gravity.norm(); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + Matrix92 D_delta_gravity; + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, n_gravity_vec, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, - H2 ? &D_delta_biasCorrected : nullptr); + H2 ? &D_delta_biasCorrected : nullptr, H3 ? &D_delta_gravity : nullptr); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; @@ -136,41 +140,64 @@ NavState PreintegrationBase::predict(const NavState& state_i, *H1 = D_predict_state + D_predict_delta * D_delta_state; if (H2) *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + if (H3) + *H3 = D_predict_delta * D_delta_gravity; return state_j; } +//------------------------------------------------------------------------------ +NavState PreintegrationBase::predict(const NavState& state_i, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + return predict(state_i, bias_i, Unit3(p().n_gravity), H1, H2, nullptr); +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, + const Unit3& n_gravity, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, - OptionalJacobian<9, 6> H3) const { + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 2> H4) const { // Predict state at time j Matrix9 D_predict_state_i; Matrix96 D_predict_bias_i; + Matrix92 D_predict_gravity; NavState predictedState_j = predict( - state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0); + state_i, bias_i, n_gravity, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0, H4 ? &D_predict_gravity : 0); // Calculate error Matrix9 D_error_state_j, D_error_predict; Vector9 error = state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, - H1 || H3 ? &D_error_predict : 0); + H1 || H3 || H4 ? &D_error_predict : 0); if (H1) *H1 << D_error_predict * D_predict_state_i; if (H2) *H2 << D_error_state_j; if (H3) *H3 << D_error_predict * D_predict_bias_i; + if (H4) *H4 << D_error_predict * D_predict_gravity; return error; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::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 { + return computeError(state_i, state_j, bias_i, Unit3(p().n_gravity), H1, H2, H3, nullptr); +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::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, + const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, - OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5, OptionalJacobian<9, 2> H6) const { // Note that derivative of constructors below is not identity for velocity, but // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() @@ -179,8 +206,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, // Predict state at time j Matrix9 D_error_state_i, D_error_state_j; - Vector9 error = computeError(state_i, state_j, bias_i, - H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5); + Vector9 error = computeError(state_i, state_j, bias_i, n_gravity, + H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5, H6); // Separate out derivatives in terms of 5 arguments // Note that doing so requires special treatment of velocities, as when treated as @@ -197,4 +224,13 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, //------------------------------------------------------------------------------ +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::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, + OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, Unit3(p().n_gravity), H1, H2, H3, H4, H5, nullptr); + } + } // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index de3a380cfe..abbdd14488 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -150,6 +150,12 @@ class GTSAM_EXPORT PreintegrationBase { virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = {}) const = 0; + /// Predict state at time j + NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 6> H2 = {}, + OptionalJacobian<9, 2> H3 = {}) const; + /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1 = {}, @@ -161,6 +167,12 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const; + /// Calculate error given navStates and gravity + Vector9 computeError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3, OptionalJacobian<9, 2> H4) const; + /** * Compute errors w.r.t. preintegrated measurements and jacobians * wrt pose_i, vel_i, bias_i, pose_j, bias_j @@ -172,6 +184,13 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, OptionalJacobian<9, 6> H5 = {}) const; + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, OptionalJacobian<9, 6> H1 = + {}, OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = + {}, OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 2> H6 = {}) const; + private: #if GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 2e1528ae87..088784ac6e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -118,6 +118,15 @@ TEST(ImuFactor, Accelerating) { /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { + // Define a type alias for the overload without the gravity parameter: + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState &, + const NavState &, + const imuBias::ConstantBias &, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + // Measurements const double a = 0.1, w = M_PI / 100.0; Vector3 measuredAcc(a, 0.0, 0.0); @@ -146,7 +155,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); std::function f = - std::bind(&PreintegrationBase::computeError, actual, + std::bind(static_cast(&PreintegrationBase::computeError), actual, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -191,6 +200,12 @@ static const NavState state2(x2, v2); /* ************************************************************************* */ TEST(ImuFactor, PreintegrationBaseMethods) { using namespace common; + using PredictNoGravity = + NavState (PreintegrationBase::*)(const NavState&, + const imuBias::ConstantBias&, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + auto p = testing::Params(); p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); p->use2ndOrderCoriolis = true; @@ -211,11 +226,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) { Matrix96 aH2; NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, + std::bind(static_cast(&PreintegrationBase::predict), pim, std::placeholders::_1, kZeroBias, nullptr, nullptr), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - std::bind(&PreintegrationBase::predict, pim, state1, + std::bind(static_cast(&PreintegrationBase::predict), pim, state1, std::placeholders::_1, nullptr, nullptr), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index f9c6ebacb7..b541b0aa66 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -91,6 +91,15 @@ TEST(ManifoldPreintegration, BiasCorrectionJacobians) { /* ************************************************************************* */ TEST(ManifoldPreintegration, computeError) { + // Define a type alias for the overload without the gravity parameter: + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState &, + const NavState &, + const imuBias::ConstantBias &, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + ManifoldPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; @@ -99,7 +108,7 @@ TEST(ManifoldPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); std::function - f = std::bind(&ManifoldPreintegration::computeError, pim, + f = std::bind(static_cast(&ManifoldPreintegration::computeError), pim, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 4275314154..fb4371766f 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -404,7 +404,7 @@ TEST(NavState, CorrectPIM) { std::function correctPIM = std::bind(&NavState::correctPIM, std::placeholders::_1, std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, - nullptr, nullptr); + nullptr, nullptr, nullptr); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 73cea9f717..f8e3491ae5 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -98,6 +98,14 @@ TEST(ImuFactor, BiasCorrectionJacobians) { /* ************************************************************************* */ TEST(TangentPreintegration, computeError) { + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState &, + const NavState &, + const imuBias::ConstantBias &, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + TangentPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; @@ -106,7 +114,7 @@ TEST(TangentPreintegration, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); std::function - f = std::bind(&TangentPreintegration::computeError, pim, + f = std::bind(static_cast(&TangentPreintegration::computeError), pim, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr);