Gravity should be specified in NAV coordinates! Default Nav frame is assumed to be *Z down* for the old-style constructors.

gtsam/navigation/CombinedImuFactor.cpp

@@ -148,7 +148,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
   if (!use2ndOrderIntegration)
     throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
   biasHat_ = biasHat;
-  boost::shared_ptr<Params> p = Params::MakeSharedFRD();
+  boost::shared_ptr<Params> p = Params::MakeSharedD();
   p->gyroscopeCovariance = measuredOmegaCovariance;
   p->accelerometerCovariance = measuredAccCovariance;
   p->integrationCovariance = integrationErrorCovariance;
@@ -260,7 +260,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor(
     Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
-    const CombinedPreintegratedMeasurements& pim, const Vector3& b_gravity,
+    const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
     const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
     const bool use2ndOrderCoriolis)
     : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
@@ -268,7 +268,7 @@ CombinedImuFactor::CombinedImuFactor(
       _PIM_(pim) {
   boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
       boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
-  p->b_gravity = b_gravity;
+  p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;
   p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@@ -279,11 +279,11 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
                                 Pose3& pose_j, Vector3& vel_j,
                                 const imuBias::ConstantBias& bias_i,
                                 CombinedPreintegratedMeasurements& pim,
-                                const Vector3& b_gravity,
+                                const Vector3& n_gravity,
                                 const Vector3& omegaCoriolis,
                                 const bool use2ndOrderCoriolis) {
   // use deprecated predict
-  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
       omegaCoriolis, use2ndOrderCoriolis);
   pose_j = pvb.pose;
   vel_j = pvb.velocity;

gtsam/navigation/CombinedImuFactor.h

@@ -66,19 +66,19 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
     Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
     Matrix6 biasAccOmegaInit;     ///< covariance of bias used for pre-integration
 
-    /// See two named constructors below for good values of b_gravity in body frame
+    /// See two named constructors below for good values of n_gravity in body frame
-    Params(const Vector3& b_gravity) :
+    Params(const Vector3& n_gravity) :
-        PreintegrationBase::Params(b_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
+        PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
             I_3x3), biasAccOmegaInit(I_6x6) {
     }
 
-    // Default Params for Front-Right-Down convention: b_gravity points along positive Z-axis
+    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
-    static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
+    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
       return boost::make_shared<Params>(Vector3(0, 0, g));
     }
 
-    // Default Params for Front-Left-Up convention: b_gravity points along negative Z-axis
+    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
-    static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
+    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
       return boost::make_shared<Params>(Vector3(0, 0, -g));
     }
 
@@ -151,7 +151,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
   Matrix preintMeasCov() const { return preintMeasCov_; }
 
   /// deprecated constructor
-  /// NOTE(frank): assumes FRD convention, only second order integration supported
+  /// NOTE(frank): assumes Z-Down convention, only second order integration supported
   PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
       const Matrix3& measuredAccCovariance,
       const Matrix3& measuredOmegaCovariance,
@@ -263,7 +263,7 @@ public:
   /// @deprecated constructor
   CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
                     Key bias_j, const CombinedPreintegratedMeasurements& pim,
-                    const Vector3& b_gravity, const Vector3& omegaCoriolis,
+                    const Vector3& n_gravity, const Vector3& omegaCoriolis,
                     const boost::optional<Pose3>& body_P_sensor = boost::none,
                     const bool use2ndOrderCoriolis = false);
 
@@ -271,7 +271,7 @@ public:
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
                       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
                       CombinedPreintegratedMeasurements& pim,
-                      const Vector3& b_gravity, const Vector3& omegaCoriolis,
+                      const Vector3& n_gravity, const Vector3& omegaCoriolis,
                       const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/ImuFactor.cpp

@@ -116,7 +116,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
   if (!use2ndOrderIntegration)
     throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
   biasHat_ = biasHat;
-  boost::shared_ptr<Params> p = Params::MakeSharedFRD();
+  boost::shared_ptr<Params> p = Params::MakeSharedD();
   p->gyroscopeCovariance = measuredOmegaCovariance;
   p->accelerometerCovariance = measuredAccCovariance;
   p->integrationCovariance = integrationErrorCovariance;
@@ -171,14 +171,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
 
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-    const PreintegratedMeasurements& pim, const Vector3& b_gravity,
+    const PreintegratedMeasurements& pim, const Vector3& n_gravity,
     const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
     const bool use2ndOrderCoriolis) :
     Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
         pose_j, vel_j, bias), _PIM_(pim) {
   boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
       PreintegratedMeasurements::Params>(pim.p());
-  p->b_gravity = b_gravity;
+  p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;
   p->use2ndOrderCoriolis = use2ndOrderCoriolis;
@@ -188,10 +188,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
 //------------------------------------------------------------------------------
 void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
     Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-    PreintegratedMeasurements& pim, const Vector3& b_gravity,
+    PreintegratedMeasurements& pim, const Vector3& n_gravity,
     const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
   // use deprecated predict
-  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, b_gravity,
+  PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
       omegaCoriolis, use2ndOrderCoriolis);
   pose_j = pvb.pose;
   vel_j = pvb.velocity;

gtsam/navigation/ImuFactor.h

@@ -108,7 +108,7 @@ public:
   Matrix preintMeasCov() const { return preintMeasCov_; }
 
   /// @deprecated constructor
-  /// NOTE(frank): assumes FRD convention, only second order integration supported
+  /// NOTE(frank): assumes Z-Down convention, only second order integration supported
   PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
       const Matrix3& measuredAccCovariance,
       const Matrix3& measuredOmegaCovariance,
@@ -210,14 +210,14 @@ public:
   /// @deprecated constructor
   ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
       const PreintegratedMeasurements& preintegratedMeasurements,
-      const Vector3& b_gravity, const Vector3& omegaCoriolis,
+      const Vector3& n_gravity, const Vector3& omegaCoriolis,
       const boost::optional<Pose3>& body_P_sensor = boost::none,
       const bool use2ndOrderCoriolis = false);
 
   /// @deprecated use PreintegrationBase::predict
   static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
       Vector3& vel_j, const imuBias::ConstantBias& bias_i,
-      PreintegratedMeasurements& pim, const Vector3& b_gravity,
+      PreintegratedMeasurements& pim, const Vector3& n_gravity,
       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/NavState.cpp

@@ -248,7 +248,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
 }
 
 //------------------------------------------------------------------------------
-Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
+Vector9 NavState::correctPIM(const Vector9& pim, double dt,
+    const Vector3& n_gravity,
     const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
   const Rot3& nRb = R_;
@@ -256,11 +257,12 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
   const double dt2 = dt * dt;
 
   Vector9 delta;
-  Matrix3 D_dP_Ri, D_dP_nv;
+  Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
   dR(delta) = dR(pim);
   dP(delta) = dP(pim)
-      + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri : 0, H2 ? &D_dP_nv : 0);
+      + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
-  dV(delta) = dV(pim);
+      + (0.5 * dt2) * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
+  dV(delta) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
 
   if (omegaCoriolis) {
     delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
@@ -272,8 +274,9 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
     if (H1) {
       if (!omegaCoriolis)
         H1->setZero(); // if coriolis H1 is already initialized
-      D_t_R(H1) += dt * D_dP_Ri;
+      D_t_R(H1) += dt * D_dP_Ri1 + (0.5 * dt2) * D_dP_Ri2;
       D_t_v(H1) += dt * D_dP_nv * Ri;
+      D_v_R(H1) += dt * D_dV_Ri;
     }
     if (H2) {
       H2->setIdentity();
@@ -283,24 +286,5 @@ Vector9 NavState::integrateTangent(const Vector9& pim, double dt,
   return delta;
 }
 //------------------------------------------------------------------------------
-NavState NavState::predict(const Vector9& pim, double dt,
-    const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis,
-    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
-
-  Matrix9 D_delta_state, D_delta_pim;
-  Vector9 delta = integrateTangent(pim, dt, omegaCoriolis, use2ndOrderCoriolis,
-      H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0);
-
-  Matrix9 D_predicted_state, D_predicted_delta;
-  NavState predicted = retract(delta, H1 ? &D_predicted_state : 0,
-      H1 || H2 ? &D_predicted_delta : 0);
-  // TODO(frank): the derivatives of retract are very sparse: inline below:
-  if (H1)
-    *H1 = D_predicted_state + D_predicted_delta * D_delta_state;
-  if (H2)
-    *H2 = D_predicted_delta * D_delta_pim;
-  return predicted;
-}
-//------------------------------------------------------------------------------
 
 }/// namespace gtsam

gtsam/navigation/NavState.h

@@ -210,21 +210,13 @@ public:
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
       OptionalJacobian<9, 9> H = boost::none) const;
 
-  /// Integrate a tangent vector forwards on tangent space, taking into account
+  /// Correct preintegrated tangent vector with our velocity and rotated gravity,
-  /// Coriolis forces if omegaCoriolis is given.
+  /// taking into account Coriolis forces if omegaCoriolis is given.
-  Vector9 integrateTangent(const Vector9& pim, double dt,
+  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
       const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
           false, OptionalJacobian<9, 9> H1 = boost::none,
       OptionalJacobian<9, 9> H2 = boost::none) const;
 
-  /// Integrate a tangent vector forwards on tangent space, taking into account
-  /// Coriolis forces if omegaCoriolis is given. Calls retract after to yield a NavState
-  NavState predict(const Vector9& pim, double dt,
-      const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
-          false, OptionalJacobian<9, 9> H1 = boost::none,
-      OptionalJacobian<9, 9> H2 = boost::none) const;
-  /// @}
-
 private:
   /// @{
   /// serialization

gtsam/navigation/PreintegrationBase.cpp

@@ -157,15 +157,10 @@ NavState PreintegrationBase::predict(const NavState& state_i,
 
   // integrate on tangent space
   Matrix9 D_delta_state, D_delta_biasCorrected;
-  Vector9 xi = state_i.integrateTangent(biasCorrected, deltaTij_,
+  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
       p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
       H2 ? &D_delta_biasCorrected : 0);
 
-  // Correct for gravity
-  double dt = deltaTij_, dt2 = dt * dt;
-  NavState::dP(xi) += 0.5 * p().b_gravity * dt2;
-  NavState::dV(xi) += p().b_gravity * dt;
-
   // Use retract to get back to NavState manifold
   Matrix9 D_predict_state, D_predict_delta;
   NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
@@ -312,11 +307,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
 //------------------------------------------------------------------------------
 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
     const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
-    const Vector3& b_gravity, const Vector3& omegaCoriolis,
+    const Vector3& n_gravity, const Vector3& omegaCoriolis,
     const bool use2ndOrderCoriolis) {
   // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
   boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
-  q->b_gravity = b_gravity;
+  q->n_gravity = n_gravity;
   q->omegaCoriolis = omegaCoriolis;
   q->use2ndOrderCoriolis = use2ndOrderCoriolis;
   p_ = q;

gtsam/navigation/PreintegrationBase.h

@@ -63,22 +63,22 @@ public:
     /// (to compensate errors in Euler integration)
     ///  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
     bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
-    Vector3 b_gravity; ///< Gravity constant in body frame
+    Vector3 n_gravity; ///< Gravity constant in body frame
 
-    /// The Params constructor insists on the user passing in gravity in the *body* frame.
+    /// The Params constructor insists on getting the navigation frame gravity vector
     /// For convenience, two commonly used conventions are provided by named constructors below
-    Params(const Vector3& b_gravity) :
+    Params(const Vector3& n_gravity) :
         accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
-            false), b_gravity(b_gravity) {
+            false), n_gravity(n_gravity) {
     }
 
-    // Default Params for Front-Right-Down convention: gravity points along positive Z-axis
+    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
-    static boost::shared_ptr<Params> MakeSharedFRD(double g = 9.81) {
+    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
       return boost::make_shared<Params>(Vector3(0, 0, g));
     }
 
-    // Default Params for Front-Left-Up convention: gravity points along negative Z-axis
+    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
-    static boost::shared_ptr<Params> MakeSharedFLU(double g = 9.81) {
+    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
       return boost::make_shared<Params>(Vector3(0, 0, -g));
     }
 
@@ -94,7 +94,7 @@ public:
       ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
       ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
       ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
-      ar & BOOST_SERIALIZATION_NVP(b_gravity);
+      ar & BOOST_SERIALIZATION_NVP(n_gravity);
     }
   };
 
@@ -201,7 +201,7 @@ public:
 
   /// @deprecated predict
   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias_i, const Vector3& b_gravity,
+      const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/tests/testImuFactor.cpp

@@ -36,7 +36,7 @@ using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
-static const Vector3 kGravity(0, 0, 9.81);
+static const Vector3 kGravityAlongNavZDown(0, 0, 9.81);
 static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
 static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
 
@@ -151,8 +151,7 @@ TEST(ImuFactor, StraightLine) {
 
   imuBias::ConstantBias biasHat, bias;
   boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
-      PreintegratedImuMeasurements::Params::MakeSharedFRD();
+      PreintegratedImuMeasurements::Params::MakeSharedU(g); // Up convention!
-  p->b_gravity = Vector3(0, 0, g); // FRD convention
 
   PreintegratedImuMeasurements pim(p, biasHat);
   for (size_t i = 0; i < 10; i++)
@@ -171,7 +170,6 @@ TEST(ImuFactor, StraightLine) {
   // Check prediction, note we move along x in body, along y in nav
   NavState expected(nRb, Point3(10, 20 + a * exact, 0),
       Velocity3(0, a * dt, 0));
-  GTSAM_PRINT(pim);
   EXPECT(assert_equal(expected, pim.predict(state1, bias)));
 }
 
@@ -197,12 +195,10 @@ TEST(ImuFactor, PreintegratedMeasurements) {
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  EXPECT(
+  EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij())));
-      assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
+  EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij())));
-  EXPECT(
+  EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij())));
-      assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
+  DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9);
-  EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
-  DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
 
   // Integrate again
   Vector3 expectedDeltaP2;
@@ -216,39 +212,38 @@ TEST(ImuFactor, PreintegratedMeasurements) {
   PreintegratedImuMeasurements actual2 = actual1;
   actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-  EXPECT(
+  EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij())));
-      assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
+  EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij())));
-  EXPECT(
+  EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij())));
-      assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
+  DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9);
-  EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
-  DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
 }
 
 /* ************************************************************************* */
 // Common linearization point and measurements for tests
 namespace common {
-imuBias::ConstantBias bias; // Bias
+static const imuBias::ConstantBias biasHat, bias; // Bias
-Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
+static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
-    Point3(5.0, 1.0, -50.0));
+    Point3(5.0, 1.0, 0));
-Vector3 v1(Vector3(0.5, 0.0, 0.0));
+static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
-
+static const NavState state1(x1, v1);
-NavState state1(x1, v1);
-Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0),
-    Point3(5.5, 1.0, -50.0));
-Vector3 v2(Vector3(0.5, 0.0, 0.0));
-NavState state2(x2, v2);
 
 // Measurements
-Vector3 measuredOmega(M_PI / 100, 0, 0);
+static const double w = M_PI / 100;
-Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector();
+static const Vector3 measuredOmega(w, 0, 0);
-double deltaT = 1.0;
+static const Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown);
+static const double deltaT = 1.0;
+
+static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0),
+    Point3(5.5, 1.0, 0));
+static const Vector3 v2(Vector3(0.5, 0.0, 0.0));
+static const NavState state2(x2, v2);
 } // namespace common
 
 /* ************************************************************************* */
 TEST(ImuFactor, PreintegrationBaseMethods) {
   using namespace common;
   boost::shared_ptr<PreintegratedImuMeasurements::Params> p =
-      PreintegratedImuMeasurements::Params::MakeSharedFRD();
+      PreintegratedImuMeasurements::Params::MakeSharedD();
   p->gyroscopeCovariance = kMeasuredOmegaCovariance;
   p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
   p->accelerometerCovariance = kMeasuredAccCovariance;
@@ -285,22 +280,21 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
 /* ************************************************************************* */
 TEST(ImuFactor, ErrorAndJacobians) {
   using namespace common;
-  PreintegratedImuMeasurements pim(bias, kMeasuredAccCovariance,
+  PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance,
       kMeasuredOmegaCovariance, kIntegrationErrorCovariance);
 
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-  EXPECT(assert_equal(state2, pim.predict(state1, bias), 1e-6));
+  EXPECT(assert_equal(state2, pim.predict(state1, bias)));
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
 
   // Expected error
-  Vector errorExpected(9);
+  Vector expectedError(9);
-  errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
   EXPECT(
-      assert_equal(errorExpected, factor.evaluateError(x1, v1, x2, v2, bias),
+      assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, bias)));
-          1e-6));
 
   Values values;
   values.insert(X(1), x1);
@@ -308,7 +302,7 @@ TEST(ImuFactor, ErrorAndJacobians) {
   values.insert(X(2), x2);
   values.insert(V(2), v2);
   values.insert(B(1), bias);
-  EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
+  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
 
   // Make sure linearization is correct
   double diffDelta = 1e-5;
@@ -331,17 +325,17 @@ TEST(ImuFactor, ErrorAndJacobians) {
   // Evaluate error with wrong values
   Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
   values.update(V(2), v2_wrong);
-  errorExpected << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901;
+  expectedError << 0, 0, 0, 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901;
   EXPECT(
-      assert_equal(errorExpected,
+      assert_equal(expectedError,
-          factor.evaluateError(x1, v1, x2, v2_wrong, bias), 1e-6));
+          factor.evaluateError(x1, v1, x2, v2_wrong, bias)));
-  EXPECT(assert_equal(errorExpected, factor.unwhitenedError(values), 1e-6));
+  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));
 
   // Make sure the whitening is done correctly
   Matrix cov = pim.preintMeasCov();
   Matrix R = RtR(cov.inverse());
-  Vector whitened = R * errorExpected;
+  Vector whitened = R * expectedError;
-  EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6));
+  EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values),1e-5));
 
   // Make sure linearization is correct
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-5);
@@ -359,7 +353,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   // Measurements
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
-  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
       + Vector3(0.2, 0.0, 0.0);
   double deltaT = 1.0;
 
@@ -370,7 +364,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) {
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kNonZeroOmegaCoriolis);
 
   Values values;
@@ -397,7 +391,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   // Measurements
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
-  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
       + Vector3(0.2, 0.0, 0.0);
   double deltaT = 1.0;
 
@@ -410,7 +404,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   // Create factor
   Pose3 bodyPsensor = Pose3();
   bool use2ndOrderCoriolis = true;
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
 
   Values values;
@@ -808,7 +802,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   // Measurements
   Vector3 measuredOmega;
   measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
-  Vector3 measuredAcc = x1.rotation().unrotate(-Point3(kGravity)).vector()
+  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
       + Vector3(0.2, 0.0, 0.0);
   double deltaT = 1.0;
 
@@ -823,7 +817,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kNonZeroOmegaCoriolis);
 
   Values values;
@@ -858,13 +852,13 @@ TEST(ImuFactor, PredictPositionAndVelocity) {
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
 
   // Predict
   Pose3 x1;
   Vector3 v1(0, 0.0, 0.0);
-  PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravity,
+  PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
   Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
   Vector3 expectedVelocity;
@@ -893,14 +887,14 @@ TEST(ImuFactor, PredictRotation) {
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
 
   // Predict
   Pose3 x1, x2;
   Vector3 v1 = Vector3(0, 0.0, 0.0);
   Vector3 v2;
-  ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis);
+  ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis);
   Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
   Vector3 expectedVelocity;
   expectedVelocity << 0, 0, 0;
@@ -926,14 +920,14 @@ TEST(ImuFactor, PredictArbitrary) {
     pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravity,
+  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
       kZeroOmegaCoriolis);
 
   // Predict
   Pose3 x1, x2;
   Vector3 v1 = Vector3(0, 0.0, 0.0);
   Vector3 v2;
-  ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravity, kZeroOmegaCoriolis);
+  ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis);
 
   // Regression test for Imu Refactor
   Rot3 expectedR( //

gtsam/navigation/tests/testNavState.cpp

@@ -149,55 +149,41 @@ boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
     &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
 
 TEST(NavState, Coriolis) {
-  Matrix9 actualH;
+  Matrix9 aH;
 
   // first-order
-  kState1.coriolis(dt, kOmegaCoriolis, false, actualH);
+  kState1.coriolis(dt, kOmegaCoriolis, false, aH);
-  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), actualH));
+  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
   // second-order
-  kState1.coriolis(dt, kOmegaCoriolis, true, actualH);
+  kState1.coriolis(dt, kOmegaCoriolis, true, aH);
-  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), actualH));
+  EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
 }
 
 TEST(NavState, Coriolis2) {
-  Matrix9 actualH;
+  Matrix9 aH;
   NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
       Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
 
   // first-order
-  state2.coriolis(dt, kOmegaCoriolis, false, actualH);
+  state2.coriolis(dt, kOmegaCoriolis, false, aH);
-  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), actualH));
+  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
   // second-order
-  state2.coriolis(dt, kOmegaCoriolis, true, actualH);
+  state2.coriolis(dt, kOmegaCoriolis, true, aH);
-  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), actualH));
+  EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
 }
 
 /* ************************************************************************* */
-TEST(NavState, PredictXi) {
+TEST(NavState, correctPIM) {
   Vector9 xi;
   xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
   double dt = 0.5;
-  Matrix9 actualH1, actualH2;
+  Matrix9 aH1, aH2;
-  boost::function<Vector9(const NavState&, const Vector9&)> integrateTangent =
+  boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
-      boost::bind(&NavState::integrateTangent, _1, _2, dt, kGravity, kOmegaCoriolis,
+      boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
           false, boost::none, boost::none);
-  kState1.integrateTangent(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
+  kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
-  EXPECT(assert_equal(numericalDerivative21(integrateTangent, kState1, xi), actualH1));
+  EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
-  EXPECT(assert_equal(numericalDerivative22(integrateTangent, kState1, xi), actualH2));
+  EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
-}
-
-/* ************************************************************************* */
-TEST(NavState, Predict) {
-  Vector9 xi;
-  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
-  double dt = 0.5;
-  Matrix9 actualH1, actualH2;
-  boost::function<NavState(const NavState&, const Vector9&)> predict =
-      boost::bind(&NavState::predict, _1, _2, dt, kGravity, kOmegaCoriolis,
-          false, boost::none, boost::none);
-  kState1.predict(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2);
-  EXPECT(assert_equal(numericalDerivative21(predict, kState1, xi), actualH1));
-  EXPECT(assert_equal(numericalDerivative22(predict, kState1, xi), actualH2));
 }
 
 /* ************************************************************************* */