Refactor of bias derivatives.

New bias derivatives work for acc but not omega

gtsam/navigation/PreintegrationBase.cpp

@@ -20,9 +20,8 @@
  **/
 
 #include "PreintegrationBase.h"
-#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+#include <gtsam/base/numericalDerivative.h>
 #include <boost/make_shared.hpp>
-#endif
 
 using namespace std;
 
@@ -78,14 +77,14 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
 
 //------------------------------------------------------------------------------
 pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
-    const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+    const Vector3& measuredAcc, const Vector3& measuredOmega,
     OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
     OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
     OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
 
   // Correct for bias in the sensor frame
-  Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-  Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
+  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
 
   // Compensate for sensor-body displacement if needed: we express the quantities
   // (originally in the IMU frame) into the body frame
@@ -95,8 +94,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
     const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
 
     // Convert angular velocity and acceleration from sensor to body frame
-    j_correctedOmega = bRs * j_correctedOmega;
+    correctedOmega = bRs * correctedOmega;
-    j_correctedAcc = bRs * j_correctedAcc;
+    correctedAcc = bRs * correctedAcc;
 
     // Jacobians
     if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
@@ -108,21 +107,21 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
     if (!b_arm.isZero()) {
       // Subtract out the the centripetal acceleration from the measured one
       // to get linear acceleration vector in the body frame:
-      const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
+      const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
       const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
-      j_correctedAcc -= body_Omega_body * b_velocity_bs;
+      correctedAcc -= body_Omega_body * b_velocity_bs;
 
       // Update derivative: centrifugal causes the correlation between acc and omega!!!
       if (D_correctedAcc_measuredOmega) {
-        double wdp = j_correctedOmega.dot(b_arm);
+        double wdp = correctedOmega.dot(b_arm);
         *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
-            + j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+            + correctedOmega * b_arm.transpose()) * bRs.matrix()
-            + 2 * b_arm * j_measuredOmega.transpose();
+            + 2 * b_arm * measuredOmega.transpose();
       }
     }
   }
 
-  return make_pair(j_correctedAcc, j_correctedOmega);
+  return make_pair(correctedAcc, correctedOmega);
 }
 
 //------------------------------------------------------------------------------
@@ -144,15 +143,24 @@ PreintegrationBase::TangentVector PreintegrationBase::UpdateEstimate(
 
   if (A) {
     // First order (small angle) approximation of derivative of invH*w:
-    const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
+//    const Matrix3 invHw_H_theta = skewSymmetric(-0.5 * w_body);
+    // NOTE(frank): Rot3::ExpmapDerivative(w_body) is also an approximation (but less accurate):
+    // If we replace approximation with numerical derivative we get better accuracy:
+      auto f = [w_body](const Vector3& theta) {
+        return Rot3::ExpmapDerivative(theta).inverse() * w_body;
+      };
+      const Matrix3 invHw_H_theta = numericalDerivative11<Vector3, Vector3>(f, zeta.theta());
 
     // Exact derivative of R*a with respect to theta:
     const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * H;
 
     A->setIdentity();
+    // theta:
     A->block<3, 3>(0, 0).noalias() += invHw_H_theta * dt;
+    // position:
     A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;
     A->block<3, 3>(3, 6) = I_3x3 * dt;
+    // velocity:
     A->block<3, 3>(6, 0) = a_nav_H_theta * dt;
   }
   if (B) {
@@ -176,8 +184,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
     OptionalJacobian<9, 3> C) const {
   if (!p().body_P_sensor) {
     // Correct for bias in the sensor frame
-    Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+    const Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-    Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+    const Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
 
     // Do update in one fell swoop
     return UpdateEstimate(correctedAcc, correctedOmega, dt, deltaXij_, A, B, C);
@@ -209,8 +217,8 @@ PreintegrationBase::TangentVector PreintegrationBase::updatedDeltaXij(
 }
 
 //------------------------------------------------------------------------------
-void PreintegrationBase::update(const Vector3& j_measuredAcc,
+void PreintegrationBase::update(const Vector3& measuredAcc,
-                                const Vector3& j_measuredOmega, double dt,
+                                const Vector3& measuredOmega, double dt,
                                 Matrix3* D_incrR_integratedOmega, Matrix9* A,
                                 Matrix93* B, Matrix93* C) {
   // Save current rotation for updating Jacobians
@@ -218,44 +226,41 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
 
   // Do update
   deltaTij_ += dt;
-  deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, A, B, C);
+  deltaXij_ = updatedDeltaXij(measuredAcc, measuredOmega, dt, A, B, C);
 
-  // Update Jacobians
+  // D_plus_abias = D_plus_zeta * D_zeta_abias + D_plus_a * D_a_abias
-  // TODO(frank): we are repeating some computation here: accessible in A ?
+  Matrix93 D_zeta_abias, D_plus_abias;
-  // Possibly: derivatives are just -B and -C ??
+  D_zeta_abias << Z_3x3, delPdelBiasAcc_, delVdelBiasAcc_;
-  Vector3 j_correctedAcc, j_correctedOmega;
+  D_plus_abias = (*A) * D_zeta_abias - (*B);
-  boost::tie(j_correctedAcc, j_correctedOmega) =
+  delPdelBiasAcc_ = D_plus_abias.middleRows<3>(3);
-      correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
+  delVdelBiasAcc_ = D_plus_abias.middleRows<3>(6);
 
-  double dt22 = 0.5 * dt * dt;
+#ifdef USE_NEW_WAY_FOR_OMEGA
-  const Matrix3 dRij = oldRij.matrix();  // expensive
+  // D_plus_wbias = D_plus_zeta * D_zeta_wbias + D_plus_w * D_w_wbias
-  delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
+  Matrix93 D_zeta_wbias, D_plus_wbias;
-  delVdelBiasAcc_ += -dRij * dt;
+  D_zeta_wbias << delRdelBiasOmega_, delPdelBiasOmega_, delVdelBiasOmega_;
-
+  D_plus_wbias = (*A) * D_zeta_wbias - (*C);
-  cout << "B:" << endl;
+  delRdelBiasOmega_ = D_plus_wbias.middleRows<3>(0);
-  cout << -(*B) << endl;
+  delPdelBiasOmega_ = D_plus_wbias.middleRows<3>(3);
-  cout << "update:" << endl;
+  delVdelBiasOmega_ = D_plus_wbias.middleRows<3>(6);
-  cout << - dt22 * dRij << endl;
+#else
-  cout << -dRij * dt << endl;
+  // The old way matches the derivatives of deltaRij()
+  Vector3 correctedAcc, correctedOmega;
+  boost::tie(correctedAcc, correctedOmega) =
+      correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega);
 
   Matrix3 D_acc_R;
-  oldRij.rotate(j_correctedAcc, 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 = j_correctedOmega * dt;
+  const Vector3 integratedOmega = correctedOmega * dt;
-  const Rot3 incrR =
+  const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega);
-      Rot3::Expmap(integratedOmega, D_incrR_integratedOmega);  // expensive !!
   const Matrix3 incrRt = incrR.transpose();
 
-  delRdelBiasOmega_ =
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
-      incrRt * delRdelBiasOmega_ - *D_incrR_integratedOmega * dt;
   delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
   delVdelBiasOmega_ += D_acc_biasOmega * dt;
-  cout << "C:" << endl;
+#endif
-  cout << -(*C) << endl;
-  cout << "update:" << endl;
-  cout <<  - *D_incrR_integratedOmega* dt << endl;
-  cout <<  dt22 * D_acc_biasOmega << endl;
-  cout << D_acc_biasOmega * dt << endl;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationBase.h

@@ -226,7 +226,7 @@ public:
   /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
   /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
   std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
-      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+      const Vector3& measuredAcc, const Vector3& measuredOmega,
       OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
       OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
       OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
@@ -243,14 +243,14 @@ public:
   /// Calculate the updated preintegrated measurement, does not modify
   /// It takes measured quantities in the j frame
   PreintegrationBase::TangentVector updatedDeltaXij(
-      const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, double dt,
+      const Vector3& measuredAcc, const Vector3& measuredOmega, double dt,
       OptionalJacobian<9, 9> A = boost::none,
       OptionalJacobian<9, 3> B = boost::none,
       OptionalJacobian<9, 3> C = boost::none) const;
 
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
-  void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
+  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
               const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* A,
               Matrix93* B, Matrix93* C);
 

gtsam/navigation/tests/testImuFactor.cpp

@@ -34,6 +34,8 @@
 using namespace std;
 using namespace gtsam;
 
+typedef imuBias::ConstantBias Bias;
+
 // Convenience for named keys
 using symbol_shorthand::X;
 using symbol_shorthand::V;
@@ -43,7 +45,8 @@ 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 imuBias::ConstantBias kZeroBiasHat, kZeroBias;
+static const Bias kZeroBiasHat, kZeroBias;
+static const Vector3 Z_3x1 = Vector3::Zero();
 
 /* ************************************************************************* */
 namespace {
@@ -51,7 +54,7 @@ namespace {
 /* ************************************************************************* */
 Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
     const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias) {
+    const Bias& bias) {
   return Rot3::Expmap(
       factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
 }
@@ -75,7 +78,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 /* ************************************************************************* */
 PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const Bias& bias, const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
   PreintegratedImuMeasurements result(defaultParams(), bias);
 
@@ -89,27 +92,19 @@ PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
 }
 
 Vector3 evaluatePreintegratedMeasurementsPosition(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const Bias& bias, const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
   return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
       deltaTs).deltaPij();
 }
 
 Vector3 evaluatePreintegratedMeasurementsVelocity(
-    const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
+    const Bias& bias, const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
   return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
       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());
-}
-
 Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
     const double deltaT) {
   return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
@@ -168,7 +163,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9);
 
   // Check derivatives of computeError
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
+  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
   NavState x1, x2 = actual1.predict(x1, bias);
 
   {
@@ -176,7 +171,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   Matrix96 aH3;
   actual1.computeError(x1, x2, bias, aH1, aH2, aH3);
   boost::function<Vector9(const NavState&, const NavState&,
-                          const imuBias::ConstantBias&)> f =
+                          const Bias&)> f =
       boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3,
                   boost::none, boost::none, boost::none);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
@@ -237,7 +232,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   // biasCorrectedDelta
   Matrix96 actualH;
   pim.biasCorrectedDelta(kZeroBias, actualH);
-  Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
+  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
       boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
           boost::none), kZeroBias);
   EXPECT(assert_equal(expectedH, actualH));
@@ -249,7 +244,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
       boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
           boost::none), state1);
   EXPECT(assert_equal(eH1, aH1));
-  Matrix eH2 = numericalDerivative11<NavState, imuBias::ConstantBias>(
+  Matrix eH2 = numericalDerivative11<NavState, Bias>(
       boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
           boost::none), kZeroBias);
   EXPECT(assert_equal(eH2, aH2));
@@ -328,7 +323,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   using common::x1;
   using common::v1;
   using common::v2;
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
+  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
       Point3(5.5, 1.0, -50.0));
 
@@ -342,14 +337,14 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   auto p = defaultParams();
   p->omegaCoriolis = kNonZeroOmegaCoriolis;
 
-  imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
+  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
   PreintegratedImuMeasurements pim(p, biasHat);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Make sure of biasCorrectedDelta
   Matrix96 actualH;
   pim.biasCorrectedDelta(bias, actualH);
-  Matrix expectedH = numericalDerivative11<Vector9, imuBias::ConstantBias>(
+  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
       boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
           boost::none), bias);
   EXPECT(assert_equal(expectedH, actualH));
@@ -374,7 +369,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   using common::x1;
   using common::v1;
   using common::v2;
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
+  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
       Point3(5.5, 1.0, -50.0));
 
@@ -390,7 +385,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   p->use2ndOrderCoriolis = true;
 
   PreintegratedImuMeasurements pim(p,
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
+      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
@@ -504,38 +499,38 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
       evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
                                         deltaTs);
 
-  // Compute numerical derivatives
+  // Check derivative of rotation
-  Matrix expectedDelPdelBias = numericalDerivative11<Vector,
+  boost::function<Rot3(const Vector3&, const Vector3&)> theta = [=](
-      imuBias::ConstantBias>(
+      const Vector3& a, const Vector3& w) {
+    return evaluatePreintegratedMeasurements(
+               Bias(a, w), measuredAccs, measuredOmegas, deltaTs).deltaRij();
+  };
+  EXPECT(
+      assert_equal(numericalDerivative21(theta, Z_3x1, Z_3x1), Matrix(Z_3x3)));
+  EXPECT(assert_equal(numericalDerivative22(theta, Z_3x1, Z_3x1),
+                      preintegrated.delRdelBiasOmega(), 1e-7));
+
+  // Check derivative of translation
+  Matrix expectedDelPdelBias = numericalDerivative11<Vector, Bias>(
       boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
-          measuredOmegas, deltaTs), kZeroBias);
+                  measuredOmegas, deltaTs),
+      kZeroBias);
   Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
+  EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
+  EXPECT(assert_equal(expectedDelPdelBiasOmega,
+                      preintegrated.delPdelBiasOmega(), 1e-8));
 
-  Matrix expectedDelVdelBias = numericalDerivative11<Vector,
+  // Check derivative of velocity
-      imuBias::ConstantBias>(
+  Matrix expectedDelVdelBias = numericalDerivative11<Vector, Bias>(
       boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
-          measuredOmegas, deltaTs), kZeroBias);
+                  measuredOmegas, deltaTs),
+      kZeroBias);
   Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
-
-  Matrix expectedDelRdelBias =
-      numericalDerivative11<Rot3, imuBias::ConstantBias>(
-          boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
-              measuredAccs, measuredOmegas, deltaTs), kZeroBias);
-  Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
-  Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
-
-  // Compare Jacobians
-  EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
-  EXPECT(
-      assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega(),1e-8));
   EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
-  EXPECT(
+  EXPECT(assert_equal(expectedDelVdelBiasOmega,
-      assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega(),1e-8));
+                      preintegrated.delVdelBiasOmega(), 1e-8));
-  EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
-  EXPECT(
-      assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),1e-7));
 }
 
 /* ************************************************************************* */
@@ -558,7 +553,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
       Point3(0.1, 0.05, 0.01));
   p->omegaCoriolis = kNonZeroOmegaCoriolis;
 
-  imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
+  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
 
   const double T = 3.0; // seconds
   ScenarioRunner runner(&scenario, p, T / 10);
@@ -608,7 +603,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 //      measuredOmega, 1e-6);
 //  EXPECT(assert_equal(expectedG2, G2, 1e-5));
 
-  imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
+  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
 
   // integrate at least twice to get position information
   // otherwise factor cov noise from preint_cov is not positive definite
@@ -636,7 +631,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, PredictPositionAndVelocity) {
-  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
+  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 
   // Measurements
   Vector3 measuredOmega;
@@ -646,7 +641,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
   double deltaT = 0.001;
 
   PreintegratedImuMeasurements pim(defaultParams(),
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
+      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
 
   for (int i = 0; i < 1000; ++i)
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@@ -664,7 +659,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, PredictRotation) {
-  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
+  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 
   // Measurements
   Vector3 measuredOmega;
@@ -674,7 +669,7 @@ TEST(ImuFactor, PredictRotation) {
   double deltaT = 0.001;
 
   PreintegratedImuMeasurements pim(defaultParams(),
-      imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
+      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
 
   for (int i = 0; i < 1000; ++i)
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@@ -684,7 +679,7 @@ TEST(ImuFactor, PredictRotation) {
 
   // Predict
   NavState actual = pim.predict(NavState(), bias);
-  NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Vector3::Zero());
+  NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1);
   EXPECT(assert_equal(expected, actual));
 }
 
@@ -706,7 +701,7 @@ TEST(ImuFactor, PredictArbitrary) {
   //  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
   //////////////////////////////////////////////////////////////////////////////////
 
-  imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
+  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
 
   // Measurements
   Vector3 measuredOmega = runner.actualAngularVelocity(0);
@@ -715,7 +710,7 @@ TEST(ImuFactor, PredictArbitrary) {
   auto p = defaultParams();
   p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
   PreintegratedImuMeasurements pim(p, biasHat);
-  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
+  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
 //  EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
 //                    Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));
 
@@ -742,7 +737,7 @@ TEST(ImuFactor, PredictArbitrary) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, bodyPSensorNoBias) {
-  imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
+  Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
 
   // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
   auto p = defaultParams();
@@ -784,7 +779,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
 
 TEST(ImuFactor, bodyPSensorWithBias) {
   using noiseModel::Diagonal;
-  typedef imuBias::ConstantBias Bias;
+  typedef Bias Bias;
 
   int numFactors = 80;
   Vector6 noiseBetweenBiasSigma;
@@ -890,7 +885,7 @@ TEST(ImuFactor, serialization) {
   p->gyroscopeCovariance = 1e-8 * I_3x3;
   p->integrationCovariance = 1e-9 * I_3x3;
   double deltaT = 0.005;
-  imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
+  Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
 
   PreintegratedImuMeasurements pim(p, priorBias);
 

gtsam/navigation/tests/testPreintegrationBase.cpp

@@ -69,7 +69,7 @@ TEST(PreintegrationBase, UpdateEstimate2) {
   pim.UpdateEstimate(acc, omega, kDt, zeta, 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));
-  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));
+  EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
   EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
 }