From 2315c68510ed886b33e1e28a80578c3158aad06a Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Mon, 10 Feb 2025 19:20:39 +0100 Subject: [PATCH 1/7] Added jacobian for gravity in correctPIM --- gtsam/navigation/NavState.cpp | 20 +++++++++++++++----- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 2 +- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 116efe90a2..4a6936a72f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -439,24 +439,26 @@ 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, 3> H3) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! const double dt22 = 0.5 * dt * dt; 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 +471,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,3>(0,0) = Matrix3::Zero(); + // The position part: derivative is dt22 * (∂(nRb.unrotate(n_gravity))/∂n_gravity) + H3->block<3,3>(3,0) = dt22 * D_nRb_unrot_grav_P; + // The velocity part: derivative is dt * D_nRb_unrot_grav_V + H3->block<3,3>(6,0) = dt * D_nRb_unrot_grav_V; + } } return xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e246cbe271..9af027d5a1 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,3> H3 = {}) const; /// @} 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)); From d7ceca02c076360e511506b3362ad17c3b5325b2 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Mon, 10 Feb 2025 19:32:11 +0100 Subject: [PATCH 2/7] IMU factor with gravity --- gtsam/navigation/ImuFactor.cpp | 97 ++++++++++++++++ gtsam/navigation/ImuFactor.h | 104 ++++++++++++++++++ gtsam/navigation/PreintegrationBase.cpp | 55 +++++++-- gtsam/navigation/PreintegrationBase.h | 19 ++++ gtsam/navigation/tests/testImuFactor.cpp | 21 +++- .../tests/testManifoldPreintegration.cpp | 11 +- .../tests/testTangentPreintegration.cpp | 10 +- 7 files changed, 301 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 5697d54d42..309c02617b 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -255,5 +255,102 @@ 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 boost::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 Vector3& gravity, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4, boost::optional H5, boost::optional H6) const { + std::cout << "Evaluating error" << std::endl; + 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 boost::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..c77decebb9 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,106 @@ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { +private: + + typedef ImuFactorWithGravity This; + typedef NoiseModelFactorN Base; + + PreintegratedImuMeasurements _PIM_; + +public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::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 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 Vector3& gravity, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none, boost::optional H4 = + boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) 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 */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + } +}; +// class ImuFactorWithGravity + template <> struct traits : public Testable {}; @@ -340,4 +441,7 @@ struct traits : public Testable {}; template <> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index dd86b829c5..438744a9de 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -115,17 +115,18 @@ 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 Vector3& n_gravity, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2, OptionalJacobian<9, 3> H3) const { Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : nullptr); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + Matrix93 D_delta_gravity; + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, n_gravity, 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 +137,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, p().n_gravity, H1, H2, boost::none); +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, - OptionalJacobian<9, 6> H3) const { + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4) const { // Predict state at time j Matrix9 D_predict_state_i; Matrix96 D_predict_bias_i; + Matrix93 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, p().n_gravity, H1, H2, H3, boost::none); +} + //------------------------------------------------------------------------------ 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 Vector3& 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, 3> 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 +203,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 +221,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, p().n_gravity, H1, H2, H3, H4, H5, boost::none); + } + } // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index de3a380cfe..ffd83aa03d 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 Vector3& n_gravity, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 6> H2 = boost::none, + OptionalJacobian<9, 3> H3 = boost::none) 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 Vector3& n_gravity, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> 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 Vector3& n_gravity, 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, OptionalJacobian<9, 3> H6 = boost::none) 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/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); From 416d4e3845466e9aeb21515f60ea0f8af83275c9 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Mon, 10 Feb 2025 20:10:35 +0100 Subject: [PATCH 3/7] Fixing compatibility issues --- gtsam/navigation/ImuFactor.cpp | 8 ++++---- gtsam/navigation/ImuFactor.h | 11 ++++++----- gtsam/navigation/PreintegrationBase.cpp | 6 +++--- gtsam/navigation/PreintegrationBase.h | 12 ++++++------ 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 309c02617b..c1bf01b864 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -266,7 +266,7 @@ ImuFactorWithGravity::ImuFactorWithGravity(Key pose_i, Key vel_i, Key pose_j, Ke //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactorWithGravity::clone() const { -return boost::static_pointer_cast( +return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } @@ -297,9 +297,9 @@ 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 Vector3& gravity, boost::optional H1, - boost::optional H2, boost::optional H3, - boost::optional H4, boost::optional H5, boost::optional H6) const { + const imuBias::ConstantBias& bias_i, const Vector3& gravity, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const { std::cout << "Evaluating error" << std::endl; return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, gravity, H1, H2, H3, H4, H5, H6); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c77decebb9..32ea052c6a 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -403,11 +403,12 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN H1 = - boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none, boost::optional H4 = - boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const override; + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const Vector3& 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 diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 438744a9de..1cf6b80607 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -146,7 +146,7 @@ NavState PreintegrationBase::predict(const NavState& state_i, 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, p().n_gravity, H1, H2, boost::none); + return predict(state_i, bias_i, p().n_gravity, H1, H2, nullptr); } //------------------------------------------------------------------------------ @@ -186,7 +186,7 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const { - return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, boost::none); + return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, nullptr); } //------------------------------------------------------------------------------ @@ -227,7 +227,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, 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, p().n_gravity, H1, H2, H3, H4, H5, boost::none); + return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, p().n_gravity, H1, H2, H3, H4, H5, nullptr); } } // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index ffd83aa03d..e2f0bba346 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -152,9 +152,9 @@ class GTSAM_EXPORT PreintegrationBase { /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 6> H2 = boost::none, - OptionalJacobian<9, 3> H3 = boost::none) const; + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 6> H2 = {}, + OptionalJacobian<9, 3> H3 = {}) const; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -187,9 +187,9 @@ class GTSAM_EXPORT PreintegrationBase { Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, 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, OptionalJacobian<9, 3> H6 = boost::none) const; + {}, OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = + {}, OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 3> H6 = {}) const; private: #if GTSAM_ENABLE_BOOST_SERIALIZATION From 0a71c9c250d5d906d943437e0ac540e2cb0204c5 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Tue, 11 Feb 2025 10:40:44 +0100 Subject: [PATCH 4/7] fix more compatibility issues to make the new class identical to ImuFactor --- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.h | 21 ++++++++++++--------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index c1bf01b864..e29e3cd33a 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -342,7 +342,7 @@ throw std::domain_error( // return new factor auto pim02 = Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); -return boost::make_shared(f01->key<1>(), // P0 +return std::make_shared(f01->key<1>(), // P0 f01->key<2>(), // V0 f12->key<3>(), // P2 f12->key<4>(), // V2 diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 32ea052c6a..edf7912752 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -358,9 +358,9 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ @@ -373,6 +373,7 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(_PIM_); + 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 From d7629ae2c9aabd4f3a7cb968f9d99a6822237c70 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Tue, 11 Feb 2025 10:54:15 +0100 Subject: [PATCH 5/7] Fix more compatibility issues --- gtsam/navigation/ImuFactor.h | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index edf7912752..5147644783 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -356,11 +356,14 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN 5 - typedef typename std::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; #else - typedef std::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ @@ -422,15 +425,15 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + 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_); + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); } #endif }; From 5f183050431f681560099ff949540439cd611dd3 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Tue, 11 Feb 2025 19:04:07 +0100 Subject: [PATCH 6/7] Convert the gravity to Unit3 with a known magnitude --- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.h | 6 +++--- gtsam/navigation/NavState.cpp | 11 ++++++---- gtsam/navigation/NavState.h | 2 +- gtsam/navigation/PreintegrationBase.cpp | 27 ++++++++++++++----------- gtsam/navigation/PreintegrationBase.h | 12 +++++------ 6 files changed, 33 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index e29e3cd33a..f347650de9 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -297,7 +297,7 @@ 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 Vector3& gravity, OptionalMatrixType H1, + const imuBias::ConstantBias& bias_i, const Unit3& gravity, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const { std::cout << "Evaluating error" << std::endl; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 5147644783..2138e38c0e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -345,12 +345,12 @@ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { + imuBias::ConstantBias, Unit3> { private: typedef ImuFactorWithGravity This; typedef NoiseModelFactorN Base; + imuBias::ConstantBias, Unit3> Base; PreintegratedImuMeasurements _PIM_; @@ -409,7 +409,7 @@ class GTSAM_EXPORT ImuFactorWithGravity: public NoiseModelFactorN& omegaCoriolis, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2, OptionalJacobian<9, 3> H3) 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 @@ -473,11 +476,11 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, } if (H3) { // The rotation part of xi does not depend on gravity: - H3->block<3,3>(0,0) = Matrix3::Zero(); + H3->block<3,2>(0,0) = Matrix32::Zero(); // The position part: derivative is dt22 * (∂(nRb.unrotate(n_gravity))/∂n_gravity) - H3->block<3,3>(3,0) = dt22 * D_nRb_unrot_grav_P; + 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,3>(6,0) = dt * D_nRb_unrot_grav_V; + H3->block<3,2>(6,0) = dt * D_nRb_unrot_grav_V * D_grav_gdirection * n_gravity.norm(); } } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 9af027d5a1..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 = {}, OptionalJacobian<9,3> H3 = {}) const; + OptionalJacobian<9, 9> H2 = {}, OptionalJacobian<9,2> H3 = {}) const; /// @} diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 1cf6b80607..2aed5c140e 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -115,16 +115,19 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2, OptionalJacobian<9, 3> H3) 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; - Matrix93 D_delta_gravity; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, 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, H3 ? &D_delta_gravity : nullptr); @@ -146,22 +149,22 @@ NavState PreintegrationBase::predict(const NavState& state_i, 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, p().n_gravity, H1, H2, nullptr); + 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 Vector3& n_gravity, + const Unit3& n_gravity, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3, - OptionalJacobian<9, 3> H4) const { + OptionalJacobian<9, 2> H4) const { // Predict state at time j Matrix9 D_predict_state_i; Matrix96 D_predict_bias_i; - Matrix93 D_predict_gravity; + Matrix92 D_predict_gravity; NavState predictedState_j = predict( state_i, bias_i, n_gravity, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0, H4 ? &D_predict_gravity : 0); @@ -186,15 +189,15 @@ Vector9 PreintegrationBase::computeError(const NavState& state_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const { - return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, nullptr); + 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, const Vector3& n_gravity, 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, OptionalJacobian<9, 3> H6) 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() @@ -227,7 +230,7 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, 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, p().n_gravity, H1, H2, H3, H4, H5, nullptr); + 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 e2f0bba346..abbdd14488 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -151,10 +151,10 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 6> H = {}) const = 0; /// Predict state at time j - NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + 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, 3> H3 = {}) const; + OptionalJacobian<9, 2> H3 = {}) const; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -169,9 +169,9 @@ class GTSAM_EXPORT PreintegrationBase { /// Calculate error given navStates and gravity Vector9 computeError(const NavState& state_i, const NavState& state_j, - const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + const imuBias::ConstantBias& bias_i, const Unit3& n_gravity, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, - OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4) const; + OptionalJacobian<9, 6> H3, OptionalJacobian<9, 2> H4) const; /** * Compute errors w.r.t. preintegrated measurements and jacobians @@ -186,10 +186,10 @@ class GTSAM_EXPORT PreintegrationBase { Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, 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 = {}, OptionalJacobian<9, 3> H6 = {}) const; + {}, OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 2> H6 = {}) const; private: #if GTSAM_ENABLE_BOOST_SERIALIZATION From c952ef961bc1743c21fb187a27dbc219e29c9440 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Wed, 25 Jun 2025 17:21:14 +0200 Subject: [PATCH 7/7] Remove debug line --- gtsam/navigation/ImuFactor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index f347650de9..08ff1c871c 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -300,7 +300,6 @@ Vector ImuFactorWithGravity::evaluateError(const Pose3& pose_i, const Vector3& v const imuBias::ConstantBias& bias_i, const Unit3& gravity, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const { - std::cout << "Evaluating error" << std::endl; return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, gravity, H1, H2, H3, H4, H5, H6); }