Merging factors

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 {
+NonlinearFactor::shared_ptr ImuFactor::clone() const {
-  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+  return boost::static_pointer_cast<NonlinearFactor>(
-      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+      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<ImuFactor>(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,

gtsam/navigation/ImuFactor.h

@@ -225,11 +225,14 @@ public:
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
           boost::none, boost::optional<Matrix&> 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;

gtsam/navigation/tests/testImuFactor.cpp

@@ -829,8 +829,8 @@ struct ImuFactorMergeTest {
     ImuFactor::shared_ptr factor02_expected = boost::make_shared<ImuFactor>(
         X(0), V(0), X(2), V(2), B(0), pim02_expected);
 
-//    ImuFactor::shared_ptr factor02_merged = factor01.mergeWith(factor12);
+    ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
-//    EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
+    EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
     return result_.getFailureCount();
   }