More sync between versions

gtsam/navigation/CombinedImuFactor.cpp

@@ -70,13 +70,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
   // Update preintegrated measurements.
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
-#else
-  Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
-  PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt,
-      &D_incrR_integratedOmega, &A, &B, &C);
-#endif
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
   // order propagation that can be seen as a prediction phase in an EKF

gtsam/navigation/ImuFactor.cpp

@@ -55,13 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   // Update preintegrated measurements (also get Jacobian)
   Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
   PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
-#else
-  Matrix3 D_incrR_integratedOmega;
-  PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt,
-      &D_incrR_integratedOmega, &A, &B, &C);
-#endif
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a

gtsam/navigation/PreintegrationBase.cpp

@@ -253,7 +253,7 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
 //------------------------------------------------------------------------------
 void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
     const Vector3& measuredOmega, const double dt,
-    Matrix3* D_incrR_integratedOmega, Matrix9* A, Matrix93* B, Matrix93* C) {
+    Matrix9* A, Matrix93* B, Matrix93* C) {
 
   // Correct for bias in the sensor frame
   Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
@@ -287,10 +287,10 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
   const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
 
   const Vector3 integratedOmega = omega * dt;
+  Matrix3 D_incrR_integratedOmega;
   const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
-      - *D_incrR_integratedOmega * dt;
 
   double dt22 = 0.5 * dt * dt;
   const Matrix3 dRij = oldRij.matrix(); // expensive

gtsam/navigation/PreintegrationBase.h

@@ -199,35 +199,24 @@ public:
                                      OptionalJacobian<9, 3> B = boost::none,
                                      OptionalJacobian<9, 3> C = boost::none);
 
-  /// Update preintegrated measurements and get derivatives
-  /// It takes measured quantities in the j frame
-  /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
-  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
-                            Matrix9* A, Matrix93* B, Matrix93* C);
-
   // Version without derivatives
   void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt);
 
-  /// Given the estimate of the bias, return a NavState tangent vector
+#endif
-  /// summarizing the preintegrated IMU measurements so far
-  Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 6> H = boost::none) const;
-
-#else
 
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
+  /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
+  /// NOTE(frank): implementation is different in two versions
   void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
-      Matrix3* D_incrR_integratedOmega,
+                            Matrix9* A, Matrix93* B, Matrix93* C);
-      Matrix9* A, Matrix93* B, Matrix93* C);
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
+  /// NOTE(frank): implementation is different in two versions
   Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
       OptionalJacobian<9, 6> H = boost::none) const;
 
-#endif
-
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
                    OptionalJacobian<9, 9> H1 = boost::none,

gtsam/navigation/tests/testPreintegrationBase.cpp

@@ -26,9 +26,9 @@
 
 #include "imuFactorTesting.h"
 
+#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 static const double kDt = 0.1;
 
-#ifdef GTSAM_IMU_MANIFOLD_INTEGRATION
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
   return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
 }