Derivatives match!

gtsam/navigation/PreintegrationBase.cpp

@@ -221,9 +221,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc,
                                 const Vector3& measuredOmega, double dt,
                                 Matrix3* D_incrR_integratedOmega, Matrix9* A,
                                 Matrix93* B, Matrix93* C) {
-  // Save current rotation for updating Jacobians
-  const Rot3 oldRij = deltaRij();
-
   // Do update
   deltaTij_ += dt;
   deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C);
@@ -235,7 +232,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc,
   delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3);
   delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6);
 
-#ifdef USE_NEW_WAY_FOR_OMEGA
   // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias
   Matrix93 D_zeta_wbias, D_plus_wbias;
   D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_;
@@ -243,24 +239,6 @@ void PreintegrationBase::update(const Vector3& measuredAcc,
   delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0);
   delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3);
   delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6);
-#else
-  // The old way matches the derivatives of deltaRij()
-  Vector3 correctedAcc, correctedOmega;
-  boost::tie(correctedAcc, correctedOmega) =
-      correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
-
-  Matrix3 D_acc_R;
-  double dt22 = 0.5 * dt * dt;
-  oldRij.rotate(correctedAcc, D_acc_R);
-  const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
-  const Vector3 integratedOmega = correctedOmega * dt;
-  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega);
-  const Matrix3 incrRt = incrR.transpose();
-
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
-  delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
-  delVdelBiasOmega_ += D_acc_biasOmega * dt;
-#endif
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -39,6 +39,10 @@ using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
+typedef imuBias::ConstantBias Bias;
+static const Vector3 Z_3x1 = Vector3::Zero();
+static const Bias kZeroBiasHat, kZeroBias;
+
 namespace {
 
 // Auxiliary functions to test preintegrated Jacobians
@@ -73,14 +77,6 @@ Vector3 evaluatePreintegratedMeasurementsVelocity(
       deltaTs).deltaVij();
 }
 
-Rot3 evaluatePreintegratedMeasurementsRotation(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
-    const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
-  return Rot3(
-      evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
-          deltaTs).deltaRij());
-}
-
 }
 
 /* ************************************************************************* */
@@ -197,6 +193,17 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
           deltaTs);
 
+  // Check derivative of rotation
+  boost::function<Vector3(const Vector3&, const Vector3&)> theta = [=](
+      const Vector3& a, const Vector3& w) {
+    return evaluatePreintegratedMeasurements(
+               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta();
+  };
+  EXPECT(
+      assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
+  EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1),
+                      pim.delRdelBiasOmega(), 1e-7));
+
   // Compute numerical derivatives
   Matrix expectedDelPdelBias = numericalDerivative11<Vector,
       imuBias::ConstantBias>(
@@ -212,20 +219,11 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
   Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
 
-  Matrix expectedDelRdelBias =
-      numericalDerivative11<Rot3, imuBias::ConstantBias>(
-          boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
-              measuredAccs, measuredOmegas, deltaTs), bias);
-  Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
-  Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
-
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc()));
   EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega(),1e-8));
   EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc()));
   EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega(),1e-8));
-  EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
-  EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 }
 
 /* ************************************************************************* */

gtsam/navigation/tests/testImuFactor.cpp

@@ -34,19 +34,19 @@
 using namespace std;
 using namespace gtsam;
 
-typedef imuBias::ConstantBias Bias;
-
 // Convenience for named keys
 using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
+typedef imuBias::ConstantBias Bias;
+static const Vector3 Z_3x1 = Vector3::Zero();
+static const Bias kZeroBiasHat, kZeroBias;
+
 static const double kGravity = 10;
 static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
 static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
 static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
-static const Bias kZeroBiasHat, kZeroBias;
-static const Vector3 Z_3x1 = Vector3::Zero();
 
 /* ************************************************************************* */
 namespace {
@@ -500,10 +500,10 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
                                         deltaTs);
 
   // Check derivative of rotation
-  boost::function<Rot3(const Vector3&, const Vector3&)> theta = [=](
+  boost::function<Vector3(const Vector3&, const Vector3&)> theta = [=](
       const Vector3& a, const Vector3& w) {
     return evaluatePreintegratedMeasurements(
-               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).deltaRij();
+               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).theta();
   };
   EXPECT(
       assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));