Now testing bias-correction Jacobians in right place

gtsam/navigation/tests/testImuFactor.cpp

@@ -444,28 +444,6 @@ TEST(ImuFactor, fistOrderExponential) {
   EXPECT(assert_equal(expectedRot, actualRot));
 }
 
-/* ************************************************************************* */
-#ifdef GTSAM_TANGENT_PREINTEGRATION
-TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
-  testing::SomeMeasurements measurements;
-
-  boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
-      [=](const Vector3& a, const Vector3& w) {
-        PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w));
-        testing::integrateMeasurements(measurements, &pim);
-        return pim.preintegrated();
-      };
-
-  // Actual pre-integrated values
-  PreintegratedImuMeasurements pim(testing::Params());
-  testing::integrateMeasurements(measurements, &pim);
-
-  EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
-                      pim.preintegrated_H_biasAcc()));
-  EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
-                      pim.preintegrated_H_biasOmega(), 1e-3));
-}
-#endif
 /* ************************************************************************* */
 Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
     const Vector3& measuredAcc, const Vector3& measuredOmega) {

gtsam/navigation/tests/testManifoldPreintegration.cpp

@@ -26,17 +26,6 @@
 
 #include "imuFactorTesting.h"
 
-static const double kDt = 0.1;
-
-Vector9 f(ManifoldPreintegration pim, const Vector3& a, const Vector3& w) {
-  NavState state = pim.deltaXij();
-  Matrix9 aH1;
-  Matrix93 aH2, aH3;
-  pim.update(a, w, kDt, &aH1, &aH2, &aH3);
-  NavState newNavState = pim.deltaXij();
-  return state.localCoordinates(newNavState);
-}
-
 namespace testing {
 // Create default parameters with Z-down and above noise parameters
 static boost::shared_ptr<PreintegrationParams> Params() {
@@ -49,34 +38,54 @@ static boost::shared_ptr<PreintegrationParams> Params() {
 }
 
 /* ************************************************************************* */
-TEST(ManifoldPreintegration, UpdateEstimate1) {
+TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
-  ManifoldPreintegration pim(testing::Params());
+  testing::SomeMeasurements measurements;
-  ManifoldPreintegration pimActual(pim);
-  const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
-  Vector9 zeta;
-  zeta.setZero();
-  Matrix9 aH1;
-  Matrix93 aH2, aH3;
-  pim.update(acc, omega, kDt, &aH1, &aH2, &aH3);
-  EXPECT(assert_equal(numericalDerivative31(f, pimActual, acc, omega), aH1, 1e-9));
-  EXPECT(assert_equal(numericalDerivative32(f, pimActual, acc, omega), aH2, 1e-9));
-  EXPECT(assert_equal(numericalDerivative33(f, pimActual, acc, omega), aH3, 1e-9));
-}
 
-/* ************************************************************************* */
+  boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
-TEST(ManifoldPreintegration, UpdateEstimate2) {
+      [=](const Vector3& a, const Vector3& w) {
-  ManifoldPreintegration pim(testing::Params());
+        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
+        testing::integrateMeasurements(measurements, &pim);
+        return pim.deltaRij();
+      };
 
-  const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
+  boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
-  Vector9 zeta;
+      [=](const Vector3& a, const Vector3& w) {
-  zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
+        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
-  Matrix9 aH1;
+        testing::integrateMeasurements(measurements, &pim);
-  Matrix93 aH2, aH3;
+        return pim.deltaPij();
-  pim.update(acc, omega, kDt, &aH1, &aH2, &aH3);
+      };
-  // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
+
-  EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
+  boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
-  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
+      [=](const Vector3& a, const Vector3& w) {
-  EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
+        ManifoldPreintegration pim(testing::Params(), Bias(a, w));
+        testing::integrateMeasurements(measurements, &pim);
+        return pim.deltaVij();
+      };
+
+  // Actual pre-integrated values
+  ManifoldPreintegration pim(testing::Params());
+  testing::integrateMeasurements(measurements, &pim);
+
+  EXPECT(
+      assert_equal(numericalDerivative21(deltaRij, kZero, kZero),
+          Matrix3(Z_3x3)));
+  EXPECT(
+      assert_equal(numericalDerivative22(deltaRij, kZero, kZero),
+          pim.delRdelBiasOmega(), 1e-3));
+
+  EXPECT(
+      assert_equal(numericalDerivative21(deltaPij, kZero, kZero),
+          pim.delPdelBiasAcc()));
+  EXPECT(
+      assert_equal(numericalDerivative22(deltaPij, kZero, kZero),
+          pim.delPdelBiasOmega(), 1e-3));
+
+  EXPECT(
+      assert_equal(numericalDerivative21(deltaVij, kZero, kZero),
+          pim.delVdelBiasAcc()));
+  EXPECT(
+      assert_equal(numericalDerivative22(deltaVij, kZero, kZero),
+          pim.delVdelBiasOmega(), 1e-3));
 }
 
 /* ************************************************************************* */

gtsam/navigation/tests/testTangentPreintegration.cpp

@@ -72,6 +72,29 @@ TEST(TangentPreintegration, UpdateEstimate2) {
   EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
 }
 
+/* ************************************************************************* */
+TEST(ImuFactor, BiasCorrectionJacobians) {
+  testing::SomeMeasurements measurements;
+
+  boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
+      [=](const Vector3& a, const Vector3& w) {
+        TangentPreintegration pim(testing::Params(), Bias(a, w));
+        testing::integrateMeasurements(measurements, &pim);
+        return pim.preintegrated();
+      };
+
+  // Actual pre-integrated values
+  TangentPreintegration pim(testing::Params());
+  testing::integrateMeasurements(measurements, &pim);
+
+  EXPECT(
+      assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
+          pim.preintegrated_H_biasAcc()));
+  EXPECT(
+      assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
+          pim.preintegrated_H_biasOmega(), 1e-3));
+}
+
 /* ************************************************************************* */
 TEST(TangentPreintegration, computeError) {
   TangentPreintegration pim(testing::Params());