Fixed equals

gtsam/navigation/ImuFactor.cpp

@@ -141,8 +141,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
 //------------------------------------------------------------------------------
 bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
   const This *e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
+  const bool base = Base::equals(*e, tol);
-      && Base::equals(*e, tol);
+  const bool pim = _PIM_.equals(e->_PIM_, tol);
+  return e != nullptr && base && pim;
 }
 
 //------------------------------------------------------------------------------
@@ -161,7 +162,7 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
     const PreintegratedImuMeasurements& 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,
+Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
     pose_j, vel_j, bias), _PIM_(pim) {
   boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
   PreintegratedImuMeasurements::Params>(pim.p());
@@ -185,4 +186,5 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
 #endif
 //------------------------------------------------------------------------------
 
-} // namespace gtsam
+}
+ // namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -71,7 +71,9 @@ protected:
   ///< (first-order propagation from *measurementCovariance*).
 
   /// Default constructor for serialization
-  PreintegratedImuMeasurements() {}
+  PreintegratedImuMeasurements() {
+    preintMeasCov_.setZero();
+  }
 
 public:
 

gtsam/navigation/PreintegratedRotation.cpp

@@ -29,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const {
   cout << s << endl;
   cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
   if (omegaCoriolis)
-    cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
+    cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
-        << endl;
   if (body_P_sensor)
     body_P_sensor->print("body_P_sensor");
 }
 
+bool PreintegratedRotation::Params::equals(
+    const PreintegratedRotation::Params& other, double tol) const {
+  if (body_P_sensor) {
+    if (!other.body_P_sensor
+        || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
+      return false;
+  }
+  if (omegaCoriolis) {
+    if (!other.omegaCoriolis
+        || !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
+      return false;
+  }
+  return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
+}
+
 void PreintegratedRotation::resetIntegration() {
   deltaTij_ = 0.0;
   deltaRij_ = Rot3();
@@ -49,8 +63,7 @@ void PreintegratedRotation::print(const string& s) const {
 
 bool PreintegratedRotation::equals(const PreintegratedRotation& other,
     double tol) const {
-  return this->matchesParamsWith(other)
+  return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol)
-      && deltaRij_.equals(other.deltaRij_, tol)
       && fabs(deltaTij_ - other.deltaTij_) < tol
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
@@ -76,11 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
   return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
 }
 
-void PreintegratedRotation::integrateMeasurement(
+void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
-    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
+    const Vector3& biasHat, double deltaT,
-    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) {
+    OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
+    OptionalJacobian<3, 3> F) {
   Matrix3 D_incrR_integratedOmega;
-  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega);
+  const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
+      D_incrR_integratedOmega);
 
   // If asked, pass first derivative as well
   if (optional_D_incrR_integratedOmega) {
@@ -93,7 +108,8 @@ void PreintegratedRotation::integrateMeasurement(
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+      - D_incrR_integratedOmega * deltaT;
 }
 
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

gtsam/navigation/PreintegratedRotation.h

@@ -43,6 +43,7 @@ class PreintegratedRotation {
     Params() : gyroscopeCovariance(I_3x3) {}
 
     virtual void print(const std::string& s) const;
+    virtual bool equals(const Params& other, double tol=1e-9) const;
 
    private:
     /** Serialization function */
@@ -53,7 +54,6 @@ class PreintegratedRotation {
       ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
       ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
       ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
-      ar& BOOST_SERIALIZATION_NVP(body_P_sensor);
     }
   };
 

gtsam/navigation/PreintegrationBase.cpp

@@ -31,7 +31,7 @@ void PreintegrationBase::Params::print(const string& s) const {
   PreintegratedRotation::Params::print(s);
   cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
       << endl;
-  cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
+  cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
       << endl;
   if (omegaCoriolis && use2ndOrderCoriolis)
     cout << "Using 2nd-order Coriolis" << endl;
@@ -40,6 +40,18 @@ void PreintegrationBase::Params::print(const string& s) const {
   cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
 }
 
+//------------------------------------------------------------------------------
+bool PreintegrationBase::Params::equals(
+    const PreintegratedRotation::Params& other, double tol) const {
+  auto e = dynamic_cast<const PreintegrationBase::Params*>(&other);
+  return e != nullptr && PreintegratedRotation::Params::equals(other, tol)
+      && use2ndOrderCoriolis == e->use2ndOrderCoriolis
+      && equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
+          tol)
+      && equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
+          tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol);
+}
+
 //------------------------------------------------------------------------------
 void PreintegrationBase::resetIntegration() {
   deltaTij_ = 0.0;
@@ -64,8 +76,8 @@ void PreintegrationBase::print(const string& s) const {
 //------------------------------------------------------------------------------
 bool PreintegrationBase::equals(const PreintegrationBase& other,
     double tol) const {
-  return this->matchesParamsWith(other)
+  const bool params_match = p_->equals(*other.p_, tol);
-      && fabs(deltaTij_ - other.deltaTij_) < tol
+  return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
       && deltaXij_.equals(other.deltaXij_, tol)
       && biasHat_.equals(other.biasHat_, tol)
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
@@ -82,13 +94,13 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
     OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
     OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
 
-  // Correct for bias in the sensor frame
+// Correct for bias in the sensor frame
   Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
   Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
 
-  // Compensate for sensor-body displacement if needed: we express the quantities
+// Compensate for sensor-body displacement if needed: we express the quantities
-  // (originally in the IMU frame) into the body frame
+// (originally in the IMU frame) into the body frame
-  // Equations below assume the "body" frame is the CG
+// Equations below assume the "body" frame is the CG
   if (p().body_P_sensor) {
     // Correct omega to rotation rate vector in the body frame
     const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
@@ -98,9 +110,12 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
     j_correctedAcc = bRs * j_correctedAcc;
 
     // Jacobians
-    if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
+    if (D_correctedAcc_measuredAcc)
-    if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
+      *D_correctedAcc_measuredAcc = bRs;
-    if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
+    if (D_correctedAcc_measuredOmega)
+      *D_correctedAcc_measuredOmega = Matrix3::Zero();
+    if (D_correctedOmega_measuredOmega)
+      *D_correctedOmega_measuredOmega = bRs;
 
     // Centrifugal acceleration
     const Vector3 b_arm = p().body_P_sensor->translation().vector();
@@ -120,7 +135,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
     }
   }
 
-  // Do update in one fell swoop
+// Do update in one fell swoop
   return make_pair(j_correctedAcc, j_correctedOmega);
 }
 
@@ -135,22 +150,27 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
   Matrix3 D_correctedAcc_measuredAcc, //
       D_correctedAcc_measuredOmega, //
       D_correctedOmega_measuredOmega;
-  bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
+  bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega
+      && p().body_P_sensor;
   boost::tie(j_correctedAcc, j_correctedOmega) =
       correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
           (needDerivs ? &D_correctedAcc_measuredAcc : 0),
           (needDerivs ? &D_correctedAcc_measuredOmega : 0),
           (needDerivs ? &D_correctedOmega_measuredOmega : 0));
-  // Do update in one fell swoop
+// Do update in one fell swoop
   Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
-  NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
+  NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt,
+      D_updated_current,
       (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
       (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
   if (needDerivs) {
-    *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
+    *D_updated_measuredAcc = D_updated_correctedAcc
-    *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
+        * D_correctedAcc_measuredAcc;
+    *D_updated_measuredOmega = D_updated_correctedOmega
+        * D_correctedOmega_measuredOmega;
     if (!p().body_P_sensor->translation().vector().isZero()) {
-      *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
+      *D_updated_measuredOmega += D_updated_correctedAcc
+          * D_correctedAcc_measuredOmega;
     }
   }
   return updated;
@@ -162,16 +182,16 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
     Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
     Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
 
-  // Save current rotation for updating Jacobians
+// Save current rotation for updating Jacobians
   const Rot3 oldRij = deltaXij_.attitude();
 
-  // Do update
+// Do update
   deltaTij_ += dt;
   deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
       D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
 
-  // Update Jacobians
+// Update Jacobians
-  // TODO(frank): we are repeating some computation here: accessible in F ?
+// TODO(frank): we are repeating some computation here: accessible in F ?
   Vector3 j_correctedAcc, j_correctedOmega;
   boost::tie(j_correctedAcc, j_correctedOmega) =
       correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
@@ -197,7 +217,7 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
 //------------------------------------------------------------------------------
 Vector9 PreintegrationBase::biasCorrectedDelta(
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
-  // Correct deltaRij, derivative is delRdelBiasOmega_
+// Correct deltaRij, derivative is delRdelBiasOmega_
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
   Matrix3 D_correctedRij_bias;
   const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
@@ -208,8 +228,8 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
 
   Vector9 xi;
   Matrix3 D_dR_correctedRij;
-  // TODO(frank): could line below be simplified? It is equivalent to
+// TODO(frank): could line below be simplified? It is equivalent to
-  //   LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
+//   LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
   NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
   NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
       + delPdelBiasOmega_ * biasIncr.gyroscope();
@@ -230,18 +250,18 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 6> H2) const {
-  // correct for bias
+// correct for bias
   Matrix96 D_biasCorrected_bias;
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
       H2 ? &D_biasCorrected_bias : 0);
 
-  // integrate on tangent space
+// integrate on tangent space
   Matrix9 D_delta_state, D_delta_biasCorrected;
   Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
       p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
       H2 ? &D_delta_biasCorrected : 0);
 
-  // Use retract to get back to NavState manifold
+// 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);
   if (H1)
@@ -258,12 +278,12 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
     OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
     OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
 
-  // Note that derivative of constructors below is not identity for velocity, but
+// Note that derivative of constructors below is not identity for velocity, but
-  // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
+// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
   NavState state_i(pose_i, vel_i);
   NavState state_j(pose_j, vel_j);
 
-  /// Predict state at time j
+/// Predict state at time j
   Matrix99 D_predict_state_i;
   Matrix96 D_predict_bias_i;
   NavState predictedState_j = predict(state_i, bias_i,
@@ -273,11 +293,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
   Vector9 error = state_j.localCoordinates(predictedState_j,
       H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
 
-  // Separate out derivatives in terms of 5 arguments
+// Separate out derivatives in terms of 5 arguments
-  // Note that doing so requires special treatment of velocities, as when treated as
+// Note that doing so requires special treatment of velocities, as when treated as
-  // separate variables the retract applied will not be the semi-direct product in NavState
+// separate variables the retract applied will not be the semi-direct product in NavState
-  // Instead, the velocities in nav are updated using a straight addition
+// Instead, the velocities in nav are updated using a straight addition
-  // This is difference is accounted for by the R().transpose calls below
+// This is difference is accounted for by the R().transpose calls below
   if (H1)
     *H1 << D_error_predict * D_predict_state_i.leftCols<6>();
   if (H2)
@@ -300,7 +320,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
     const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
     const Vector3& n_gravity, const Vector3& omegaCoriolis,
     const bool use2ndOrderCoriolis) const {
-  // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
+// 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->n_gravity = n_gravity;
   q->omegaCoriolis = omegaCoriolis;
@@ -311,4 +331,5 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
 #endif
 //------------------------------------------------------------------------------
 
-}/// namespace gtsam
+}
+ /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -83,6 +83,7 @@ public:
     }
 
     void print(const std::string& s) const;
+    bool equals(const PreintegratedRotation::Params& other, double tol) const;
 
   protected:
     /// Default constructor for serialization only: uninitialized!
@@ -132,6 +133,7 @@ protected:
 
   /// Default constructor for serialization
   PreintegrationBase() {
+    resetIntegration();
   }
 
 public:

gtsam/navigation/tests/testImuFactor.cpp

@@ -65,12 +65,12 @@ static const double kAccelerometerSigma = 0.1;
 static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
   auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
-  p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma * I_3x3;
+  p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
+      * I_3x3;
   p->integrationCovariance = 0.0001 * I_3x3;
   return p;
 }
 
-
 // Auxiliary functions to test preintegrated Jacobians
 // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 /* ************************************************************************* */
@@ -123,7 +123,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
 
 /* ************************************************************************* */
 TEST(ImuFactor, Accelerating) {
-  const double a = 0.2, v=50;
+  const double a = 0.2, v = 50;
 
   // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
   // The body itself has Z axis pointing down
@@ -144,17 +144,20 @@ TEST(ImuFactor, Accelerating) {
   EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
 
   // Check G1 and G2 derivatives of pim.update
-  Matrix93 aG1,aG2;
+  Matrix93 aG1, aG2;
-  boost::function<NavState(const Vector3&, const Vector3&)> f =
+  boost::function<NavState(const Vector3&, const Vector3&)> f = boost::bind(
-      boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
+      &PreintegrationBase::updatedDeltaXij, pim, _1, _2, T / 10, boost::none,
-          boost::none, boost::none, boost::none);
+      boost::none, boost::none);
   const Vector3 measuredAcc = runner.actualSpecificForce(T);
   const Vector3 measuredOmega = runner.actualAngularVelocity(T);
-  pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
+  pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1,
-  EXPECT(assert_equal(
+      aG2);
-      numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
+  EXPECT(
-  EXPECT(assert_equal(
+      assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7),
-      numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
+          aG1, 1e-7));
+  EXPECT(
+      assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7),
+          aG2, 1e-7));
 }
 
 /* ************************************************************************* */
@@ -268,7 +271,8 @@ TEST(ImuFactor, ErrorAndJacobians) {
   Vector expectedError(9);
   expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
   EXPECT(
-      assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
+      assert_equal(expectedError,
+          factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
 
   Values values;
   values.insert(X(1), x1);
@@ -284,16 +288,19 @@ TEST(ImuFactor, ErrorAndJacobians) {
 
   // Actual Jacobians
   Matrix H1a, H2a, H3a, H4a, H5a;
-  (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a);
+  (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a,
+      H5a);
 
   // Make sure rotation part is correct when error is interpreted as axis-angle
   // Jacobians are around zero, so the rotation part is the same as:
   Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1);
+      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias),
+      x1);
   EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
 
   Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
-      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2);
+      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
+      x2);
   EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
 
   // Evaluate error with wrong values
@@ -386,7 +393,6 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
   // Create factor
-  bool use2ndOrderCoriolis = true;
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
 
   Values values;
@@ -547,7 +553,8 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
       Vector3(0, 0, M_PI / 10.0 + 0.3));
 
   auto p = defaultParams();
-  p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01));
+  p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
+      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));
@@ -575,32 +582,34 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
 
   // Check updatedDeltaXij derivatives
   Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero();
-  pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none);
+  pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
-  Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
+      boost::none, D_correctedAcc_measuredOmega, boost::none);
+  Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
+      boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
   EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
 
   Matrix93 G1, G2;
   double dt = 0.1;
-  NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
+  NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt,
+      boost::none, G1, G2);
 //  Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose();
 
   Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
       boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
-          dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
+          dt, boost::none, boost::none, boost::none), measuredAcc,
-      1e-6);
+      measuredOmega, 1e-6);
   EXPECT(assert_equal(expectedG1, G1, 1e-5));
 
   Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
       boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
-          dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
+          dt, boost::none, boost::none, boost::none), measuredAcc,
-      1e-6);
+      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)
 //  EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor,
 //      measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
 
-
   // integrate at least twice to get position information
   // otherwise factor cov noise from preint_cov is not positive definite
   pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
@@ -687,8 +696,7 @@ TEST(ImuFactor, PredictArbitrary) {
   const Vector3 v1(0, 0, 0);
 
   const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1,
-                                      Vector3(0.1, 0.2, 0),
+      Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
-                                      Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
 
   const double T = 3.0; // seconds
   ScenarioRunner runner(&scenario, defaultParams(), T / 10);
@@ -836,8 +844,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
   // Now add IMU factors and bias noise models
   Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
   for (int i = 1; i < numFactors; i++) {
-    PreintegratedImuMeasurements pim =
+    PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
-        PreintegratedImuMeasurements(p, priorBias);
+        priorBias);
     for (int j = 0; j < 200; ++j)
       pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
@@ -862,28 +870,31 @@ TEST(ImuFactor, bodyPSensorWithBias) {
 /* ************************************************************************** */
 #include <gtsam/base/serializationTestHelpers.h>
 
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+    "gtsam_noiseModel_Constrained");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
+    "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
+    "gtsam_noiseModel_Gaussian");
 BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
+    "gtsam_noiseModel_Isotropic");
 BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
 BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 
 TEST(ImuFactor, serialization) {
   using namespace gtsam::serializationTestHelpers;
 
-  Vector3 n_gravity(0, 0, -9.81);
+  auto p = defaultParams();
-  Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
+  p->n_gravity = Vector3(0, 0, -9.81);
-  Matrix3 accCov = 1e-7 * I_3x3;
+  p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3());
-  Matrix3 gyroCov = 1e-8 * I_3x3;
+  p->accelerometerCovariance = 1e-7 * I_3x3;
-  Matrix3 integrationCov = 1e-9 * I_3x3;
+  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)
 
-  ImuFactor::PreintegratedMeasurements pim =
+  PreintegratedImuMeasurements pim(p, priorBias);
-      ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
-          integrationCov, true);
 
   // measurements are needed for non-inf noise model, otherwise will throw err when deserialize
 
@@ -895,8 +906,7 @@ TEST(ImuFactor, serialization) {
   Vector3 measuredAcc(0, 0, -9.81);
 
   for (int j = 0; j < 200; ++j)
-    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
+    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
-        body_P_sensor);
 
   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);