diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 2f8e42917..145359d91 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -113,9 +113,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, } //------------------------------------------------------------------------------ -gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +NonlinearFactor::shared_ptr ImuFactor::clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); } //------------------------------------------------------------------------------ @@ -178,6 +178,29 @@ PreintegratedImuMeasurements ImuFactor::Merge( return pim02; } +//------------------------------------------------------------------------------ +ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, + const shared_ptr& f12) { + // IMU bias keys must be the same. + if (f01->key5() != f12->key5()) + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + + // expect intermediate pose, velocity keys to matchup. + if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + + // return new factor + auto pim02 = + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(), // P0 + f01->key2(), // V0 + f12->key3(), // P2 + f12->key4(), // V2 + f01->key5(), // B + pim02); +} + //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 73b5c8987..73a2f05d2 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -225,11 +225,14 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - // Merge two pre-integrated measurement classes + /// 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); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename typedef PreintegratedImuMeasurements PreintegratedMeasurements; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 02a5bb870..066f96831 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -829,8 +829,8 @@ struct ImuFactorMergeTest { ImuFactor::shared_ptr factor02_expected = boost::make_shared( X(0), V(0), X(2), V(2), B(0), pim02_expected); -// ImuFactor::shared_ptr factor02_merged = factor01.mergeWith(factor12); -// EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol)); + ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12); + EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol)); return result_.getFailureCount(); }