mergeWith prototype

gtsam/navigation/ImuFactor.cpp

@@ -158,26 +158,26 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
 PreintegratedImuMeasurements ImuFactor::Merge(
     const PreintegratedImuMeasurements& pim01,
     const PreintegratedImuMeasurements& pim12) {
-  if(!pim01.matchesParamsWith(pim12))
+  if (!pim01.matchesParamsWith(pim12))
-    throw std::domain_error("Cannot merge PreintegratedImuMeasurements with different params");
+    throw std::domain_error(
+        "Cannot merge PreintegratedImuMeasurements with different params");
 
-  if(pim01.params()->body_P_sensor)
+  if (pim01.params()->body_P_sensor)
-    throw std::domain_error("Cannot merge PreintegratedImuMeasurements with sensor pose yet");
+    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.params(), pim01.biasHat());
+  PreintegratedImuMeasurements pim02 = pim01;
 
-  const double& t01 = pim01.deltaTij();
+  Matrix9 H1, H2;
-  const double& t12 = pim12.deltaTij();
+  pim02.mergeWith(pim12, &H1, &H2);
-  pim02.deltaTij_ = t01 + t12;
 
-  const Rot3 R01 = pim01.deltaRij(), R12 = pim12.deltaRij();
+  pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() +
-  pim02.preintegrated_ << Rot3::Logmap(R01 * R12),
+                         H2 * pim12.preintMeasCov_ * H2.transpose();
-      pim01.deltaPij() + pim01.deltaVij() * t12 + R01 * pim12.deltaPij(),
-      pim01.deltaVij() + R01 * pim12.deltaVij();
 
   return 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/PreintegrationBase.cpp

@@ -308,6 +308,63 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   return error;
 }
 
+//------------------------------------------------------------------------------
+// sugar for derivative blocks
+#define D_R_R(H) (H)->block<3,3>(0,0)
+#define D_R_t(H) (H)->block<3,3>(0,3)
+#define D_R_v(H) (H)->block<3,3>(0,6)
+#define D_t_R(H) (H)->block<3,3>(3,0)
+#define D_t_t(H) (H)->block<3,3>(3,3)
+#define D_t_v(H) (H)->block<3,3>(3,6)
+#define D_v_R(H) (H)->block<3,3>(6,0)
+#define D_v_t(H) (H)->block<3,3>(6,3)
+#define D_v_v(H) (H)->block<3,3>(6,6)
+
+//------------------------------------------------------------------------------
+void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
+                                   Matrix9* H2) {
+  if (!matchesParamsWith(pim12))
+    throw std::domain_error(
+        "Cannot merge preintegrated measurements with different params");
+
+  if (params()->body_P_sensor)
+    throw std::domain_error(
+        "Cannot merge preintegrated measurements with sensor pose yet");
+
+  const double& t01 = deltaTij();
+  const double& t12 = pim12.deltaTij();
+  deltaTij_ = t01 + t12;
+
+  Matrix3 R01_H_theta01, R12_H_theta12;
+  const Rot3 R01 = Rot3::Expmap(theta(), R01_H_theta01);
+  const Rot3 R12 = Rot3::Expmap(pim12.theta(), R12_H_theta12);
+
+  Matrix3 R02_H_R01, R02_H_R12;
+  const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
+
+  Matrix3 theta02_H_R02;
+  preintegrated_ << Rot3::Logmap(R02, theta02_H_R02),
+      deltaPij() + deltaVij() * t12 + R01 * pim12.deltaPij(),
+      deltaVij() + R01 * pim12.deltaVij();
+
+  H1->setZero();
+  D_R_R(H1) = theta02_H_R02 * R02_H_R01 * R01_H_theta01;
+  D_t_t(H1) = I_3x3;
+  D_t_v(H1) = I_3x3 * t12;
+  D_v_v(H1) = I_3x3;
+
+  H2->setZero();
+  D_R_R(H2) = theta02_H_R02 * R02_H_R12 * R12_H_theta12;  // I_3x3 ??
+  D_t_t(H2) = R01.matrix();  // + rotated_H_theta1 ??
+  D_v_v(H2) = R01.matrix();  // + rotated_H_theta1 ??
+
+  preintegrated_H_biasAcc_ =
+      (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
+
+  preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
+                               (*H2) * pim12.preintegrated_H_biasOmega_;
+}
+
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,

gtsam/navigation/PreintegrationBase.h

@@ -211,6 +211,9 @@ public:
       OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
           boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
 
+  /// Merge in a different set of measurements and update bias derivatives accordingly
+  /// The derivatives apply to the preintegrated Vector9
+  void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
   /// @}
 
   /** Dummy clone for MATLAB */