Everything compiles and runs with derived classes

gtsam/navigation/CombinedImuFactor.cpp

@@ -31,22 +31,21 @@ using namespace std;
 //------------------------------------------------------------------------------
 // Inner class PreintegratedCombinedMeasurements
 //------------------------------------------------------------------------------
-void PreintegratedCombinedMeasurements::print(
+void PreintegratedCombinedMeasurements::print(const string& s) const {
-    const string& s) const {
+  PreintegrationType::print(s);
-  PreintegrationBase::print(s);
   cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool PreintegratedCombinedMeasurements::equals(
     const PreintegratedCombinedMeasurements& other, double tol) const {
-  return PreintegrationBase::equals(other, tol) &&
+  return PreintegrationType::equals(other, tol)
-         equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
+      && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
 }
 
 //------------------------------------------------------------------------------
 void PreintegratedCombinedMeasurements::resetIntegration() {
-  PreintegrationBase::resetIntegration();
+  PreintegrationType::resetIntegration();
   preintMeasCov_.setZero();
 }
 
@@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
 void PreintegratedCombinedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
   // Update preintegrated measurements.
-  Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-  PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
+  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // Update preintegrated measurements covariance: as in [2] we consider a first
   // order propagation that can be seen as a prediction phase in an EKF
@@ -80,7 +79,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
 
   // Single Jacobians to propagate covariance
   // TODO(frank): should we not also account for bias on position?
-  Matrix3 theta_H_biasOmega = - C.topRows<3>();
+  Matrix3 theta_H_biasOmega = -C.topRows<3>();
   Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
 
   // overall Jacobian wrt preintegrated measurements (df/dx)
@@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
 
   // BLOCK DIAGONAL TERMS
   D_t_t(&G_measCov_Gt) = dt * iCov;
-  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc *
+  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-                         (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
+      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
-                         (vel_H_biasAcc.transpose());
+      * (vel_H_biasAcc.transpose());
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
+  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-                         (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
+      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-                         (theta_H_biasOmega.transpose());
+      * (theta_H_biasOmega.transpose());
   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
   D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
 
   // OFF BLOCK DIAGONAL TERMS
-  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
+  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
-                 theta_H_biasOmega.transpose();
+      * theta_H_biasOmega.transpose();
   D_v_R(&G_measCov_Gt) = temp;
   D_R_v(&G_measCov_Gt) = temp.transpose();
   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
@@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
     const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
     const bool use2ndOrderIntegration) {
   if (!use2ndOrderIntegration)
-    throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
+  throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
   biasHat_ = biasHat;
   boost::shared_ptr<Params> p = Params::MakeSharedD();
   p->gyroscopeCovariance = measuredOmegaCovariance;
@@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
 //------------------------------------------------------------------------------
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------
-CombinedImuFactor::CombinedImuFactor(
+CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
-    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+    Key vel_j, Key bias_i, Key bias_j,
-    const PreintegratedCombinedMeasurements& pim)
+    const PreintegratedCombinedMeasurements& pim) :
-    : 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_i, bias_j),
+        pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
-      _PIM_(pim) {}
+}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   Matrix93 D_r_vel_i, D_r_vel_j;
 
   // error wrt preintegrated measurements
-  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
+  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
-      H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
+      bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
       H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
 
   // if we need the jacobians
@@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor(
     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,
+: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-           pose_j, vel_j, bias_i, bias_j),
+    pose_j, vel_j, bias_i, bias_j),
-      _PIM_(pim) {
+_PIM_(pim) {
   boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
-      boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
+  boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
   p->n_gravity = n_gravity;
   p->omegaCoriolis = omegaCoriolis;
   p->body_P_sensor = body_P_sensor;
@@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor(
 }
 
 void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-                                Pose3& pose_j, Vector3& vel_j,
+    Pose3& pose_j, Vector3& vel_j,
-                                const imuBias::ConstantBias& bias_i,
+    const imuBias::ConstantBias& bias_i,
-                                CombinedPreintegratedMeasurements& pim,
+    CombinedPreintegratedMeasurements& pim,
-                                const Vector3& n_gravity,
+    const Vector3& n_gravity,
-                                const Vector3& omegaCoriolis,
+    const Vector3& omegaCoriolis,
-                                const bool use2ndOrderCoriolis) {
+    const bool use2ndOrderCoriolis) {
   // use deprecated predict
   PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
       omegaCoriolis, use2ndOrderCoriolis);
@@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
 }
 #endif
 
-} /// namespace gtsam
+}
+ /// namespace gtsam
 

gtsam/navigation/CombinedImuFactor.h

@@ -22,12 +22,19 @@
 #pragma once
 
 /* GTSAM includes */
+#include <gtsam/navigation/ManifoldPreintegration.h>
+#include <gtsam/navigation/TangentPreintegration.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/navigation/PreintegrationBase.h>
 #include <gtsam/base/Matrix.h>
 
 namespace gtsam {
 
+#ifdef GTSAM_TANGENT_PREINTEGRATION
+typedef TangentPreintegration PreintegrationType;
+#else
+typedef ManifoldPreintegration PreintegrationType;
+#endif
+
 /*
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@@ -57,7 +64,7 @@ namespace gtsam {
  *
  * @addtogroup SLAM
  */
-class PreintegratedCombinedMeasurements : public PreintegrationBase {
+class PreintegratedCombinedMeasurements : public PreintegrationType {
 
 public:
 
@@ -123,7 +130,7 @@ public:
   PreintegratedCombinedMeasurements(
       const boost::shared_ptr<Params>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
-      : PreintegrationBase(p, biasHat) {
+      : PreintegrationType(p, biasHat) {
     preintMeasCov_.setZero();
   }
 
@@ -133,10 +140,10 @@ public:
   /// @{
 
   /// Re-initialize PreintegratedCombinedMeasurements
-  void resetIntegration();
+  void resetIntegration() override;
 
   /// const reference to params, shadows definition in base class
-  Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
+  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
   /// @}
 
   /// @name Access instance variables
@@ -146,7 +153,7 @@ public:
 
   /// @name Testable
   /// @{
-  void print(const std::string& s = "Preintegrated Measurements:") const;
+  void print(const std::string& s = "Preintegrated Measurements:") const override;
   bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
   /// @}
 
@@ -163,7 +170,7 @@ public:
    * frame)
    */
   void integrateMeasurement(const Vector3& measuredAcc,
-      const Vector3& measuredOmega, double deltaT);
+      const Vector3& measuredOmega, const double dt) override;
 
   /// @}
 
@@ -183,7 +190,7 @@ public:
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
     ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
   }
 };

gtsam/navigation/ImuFactor.cpp

@@ -32,20 +32,20 @@ using namespace std;
 // Inner class PreintegratedImuMeasurements
 //------------------------------------------------------------------------------
 void PreintegratedImuMeasurements::print(const string& s) const {
-  PreintegrationBase::print(s);
+  PreintegrationType::print(s);
   cout << "    preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool PreintegratedImuMeasurements::equals(
     const PreintegratedImuMeasurements& other, double tol) const {
-  return PreintegrationBase::equals(other, tol)
+  return PreintegrationType::equals(other, tol)
       && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
 }
 
 //------------------------------------------------------------------------------
 void PreintegratedImuMeasurements::resetIntegration() {
-  PreintegrationBase::resetIntegration();
+  PreintegrationType::resetIntegration();
   preintMeasCov_.setZero();
 }
 
@@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() {
 void PreintegratedImuMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
   // Update preintegrated measurements (also get Jacobian)
-  Matrix9 A;  // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
   Matrix93 B, C;
-  PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
+  PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
 
   // first order covariance propagation:
   // as in [2] we consider a first order propagation that can be seen as a
@@ -73,30 +73,31 @@ void PreintegratedImuMeasurements::integrateMeasurement(
   preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
 
   // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
-  preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt;
+  preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
 }
 
 //------------------------------------------------------------------------------
-void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs,
+void PreintegratedImuMeasurements::integrateMeasurements(
-                                                         const Matrix& measuredOmegas,
+    const Matrix& measuredAccs, const Matrix& measuredOmegas,
-                                                         const Matrix& dts) {
+    const Matrix& dts) {
-  assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
+  assert(
+      measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
   assert(dts.cols() >= 1);
   assert(measuredAccs.cols() == dts.cols());
   assert(measuredOmegas.cols() == dts.cols());
   size_t n = static_cast<size_t>(dts.cols());
   for (size_t j = 0; j < n; j++) {
-    integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j));
+    integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j));
   }
 }
 
 //------------------------------------------------------------------------------
 #ifdef GTSAM_TANGENT_PREINTEGRATION
-void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12,  //
+void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
-                                             Matrix9* H1, Matrix9* H2) {
+    Matrix9* H1, Matrix9* H2) {
-  PreintegrationBase::mergeWith(pim12, H1, H2);
+  PreintegrationType::mergeWith(pim12, H1, H2);
   preintMeasCov_ =
-      *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
+  *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
 }
 #endif
 //------------------------------------------------------------------------------
@@ -180,12 +181,12 @@ PreintegratedImuMeasurements ImuFactor::Merge(
     const PreintegratedImuMeasurements& pim01,
     const PreintegratedImuMeasurements& pim12) {
   if (!pim01.matchesParamsWith(pim12))
-    throw std::domain_error(
+  throw std::domain_error(
-        "Cannot merge PreintegratedImuMeasurements with different params");
+      "Cannot merge PreintegratedImuMeasurements with different params");
 
   if (pim01.params()->body_P_sensor)
-    throw std::domain_error(
+  throw std::domain_error(
-        "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
+      "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
 
   // the bias for the merged factor will be the bias from 01
   PreintegratedImuMeasurements pim02 = pim01;
@@ -198,25 +199,25 @@ PreintegratedImuMeasurements ImuFactor::Merge(
 
 //------------------------------------------------------------------------------
 ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
-                                       const shared_ptr& f12) {
+    const shared_ptr& f12) {
   // IMU bias keys must be the same.
   if (f01->key5() != f12->key5())
-    throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
+  throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
 
   // expect intermediate pose, velocity keys to matchup.
   if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
-    throw std::domain_error(
+  throw std::domain_error(
-        "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
+      "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
 
   // return new factor
   auto pim02 =
-      Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
+  Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
-  return boost::make_shared<ImuFactor>(f01->key1(),  // P0
+  return boost::make_shared<ImuFactor>(f01->key1(),// P0
-                                       f01->key2(),  // V0
+      f01->key2(),// V0
-                                       f12->key3(),  // P2
+      f12->key3(),// P2
-                                       f12->key4(),  // V2
+      f12->key4(),// V2
-                                       f01->key5(),  // B
+      f01->key5(),// B
-                                       pim02);
+      pim02);
 }
 #endif
 
@@ -251,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
 //------------------------------------------------------------------------------
 // ImuFactor2 methods
 //------------------------------------------------------------------------------
-ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim)
+ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
-    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias),
+    const PreintegratedImuMeasurements& pim) :
-      _PIM_(pim) {}
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
+        bias), _PIM_(pim) {
+}
 
 //------------------------------------------------------------------------------
 NonlinearFactor::shared_ptr ImuFactor2::clone() const {
@@ -269,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
 }
 
 //------------------------------------------------------------------------------
-void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
+void ImuFactor2::print(const string& s,
-  cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
+    const KeyFormatter& keyFormatter) const {
-       << "," << keyFormatter(this->key3()) << ")\n";
+  cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
+      << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
+      << ")\n";
   cout << *this << endl;
 }
 
@@ -284,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
 }
 
 //------------------------------------------------------------------------------
-Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j,
+Vector ImuFactor2::evaluateError(const NavState& state_i,
-                                 const imuBias::ConstantBias& bias_i,  //
+    const NavState& state_j,
-                                 boost::optional<Matrix&> H1,
+    const imuBias::ConstantBias& bias_i, //
-                                 boost::optional<Matrix&> H2,
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-                                 boost::optional<Matrix&> H3) const {
+    boost::optional<Matrix&> H3) const {
   return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
 }
 
 //------------------------------------------------------------------------------
 
 }
- // namespace gtsam
+// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -23,11 +23,18 @@
 
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/navigation/ManifoldPreintegration.h>
+#include <gtsam/navigation/TangentPreintegration.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
 
+#ifdef GTSAM_TANGENT_PREINTEGRATION
+typedef TangentPreintegration PreintegrationType;
+#else
+typedef ManifoldPreintegration PreintegrationType;
+#endif
+
 /*
  * If you are using the factor, please cite:
  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
@@ -61,7 +68,7 @@ namespace gtsam {
  *
  * @addtogroup SLAM
  */
-class PreintegratedImuMeasurements: public PreintegrationBase {
+class PreintegratedImuMeasurements: public PreintegrationType {
 
   friend class ImuFactor;
   friend class ImuFactor2;
@@ -85,29 +92,28 @@ public:
    */
   PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
-      PreintegrationBase(p, biasHat) {
+      PreintegrationType(p, biasHat) {
     preintMeasCov_.setZero();
   }
 
 /**
   *  Construct preintegrated directly from members: base class and preintMeasCov
-  *  @param base               PreintegrationBase instance
+  *  @param base               PreintegrationType instance
   *  @param preintMeasCov      Covariance matrix used in noise model.
   */
-  PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
+  PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
-     : PreintegrationBase(base),
+     : PreintegrationType(base),
        preintMeasCov_(preintMeasCov) {
   }
 
   /// print
-  void print(const std::string& s = "Preintegrated Measurements:") const;
+  void print(const std::string& s = "Preintegrated Measurements:") const override;
 
   /// equals
-  bool equals(const PreintegratedImuMeasurements& expected,
+  bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
-      double tol = 1e-9) const;
 
   /// Re-initialize PreintegratedIMUMeasurements
-  void resetIntegration();
+  void resetIntegration() override;
 
   /**
    * Add a single IMU measurement to the preintegration.
@@ -115,7 +121,8 @@ public:
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between this and the last IMU measurement
    */
-  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt);
+  void integrateMeasurement(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, const double dt) override;
 
   /// Add multiple measurements, in matrix columns
   void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
@@ -152,7 +159,7 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
     namespace bs = ::boost::serialization;
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
     ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
   }
 };

gtsam/navigation/ManifoldPreintegration.cpp

@@ -26,9 +26,9 @@ using namespace std;
 namespace gtsam {
 
 //------------------------------------------------------------------------------
-ManifoldPreintegration::ManifoldPreintegration(const boost::shared_ptr<Params>& p,
+ManifoldPreintegration::ManifoldPreintegration(
-                                       const Bias& biasHat)
+    const boost::shared_ptr<Params>& p, const Bias& biasHat) :
-    : PreintegrationBase(p, biasHat) {
+    PreintegrationBase(p, biasHat) {
   resetIntegration();
 }
 
@@ -46,8 +46,7 @@ void ManifoldPreintegration::resetIntegration() {
 //------------------------------------------------------------------------------
 bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
     double tol) const {
-  return p_->equals(*other.p_, tol)
+  return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
-      && fabs(deltaTij_ - other.deltaTij_) < tol
       && biasHat_.equals(other.biasHat_, tol)
       && deltaXij_.equals(other.deltaXij_, tol)
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
@@ -58,9 +57,9 @@ bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
 }
 
 //------------------------------------------------------------------------------
-void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc,
+void ManifoldPreintegration::update(const Vector3& measuredAcc,
-    const Vector3& measuredOmega, const double dt,
+    const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
-    Matrix9* A, Matrix93* B, Matrix93* C) {
+    Matrix93* C) {
 
   // Correct for bias in the sensor frame
   Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
@@ -83,8 +82,8 @@ void ManifoldPreintegration::integrateMeasurement(const Vector3& measuredAcc,
     // More complicated derivatives in case of non-trivial sensor pose
     *C *= D_correctedOmega_omega;
     if (!p().body_P_sensor->translation().isZero())
-      *C += *B* D_correctedAcc_omega;
+      *C += *B * D_correctedAcc_omega;
-    *B *= D_correctedAcc_acc;  // NOTE(frank): needs to be last
+    *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
   }
 
   // Update Jacobians
@@ -141,4 +140,4 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta(
 
 //------------------------------------------------------------------------------
 
-}  // namespace gtsam
+}// namespace gtsam

gtsam/navigation/ManifoldPreintegration.h

@@ -21,7 +21,8 @@
 
 #pragma once
 
-#include <gtsam/navigation/ManifoldPreintegration.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/navigation/PreintegrationBase.h>
 
 namespace gtsam {
 
@@ -72,14 +73,14 @@ public:
 
   /// @name Instance variables access
   /// @{
-  const NavState& deltaXij() const override { return deltaXij_; }
+  NavState deltaXij() const override { return deltaXij_; }
-  const Rot3& deltaRij()     const override { return deltaXij_.attitude(); }
+  Rot3     deltaRij() const override { return deltaXij_.attitude(); }
-  Vector3 deltaPij()         const override { return deltaXij_.position().vector(); }
+  Vector3  deltaPij() const override { return deltaXij_.position().vector(); }
-  Vector3 deltaVij()         const override { return deltaXij_.velocity(); }
+  Vector3  deltaVij() const override { return deltaXij_.velocity(); }
 
   /// @name Testable
   /// @{
-  bool equals(const ManifoldPreintegration& other, double tol) const override;
+  bool equals(const ManifoldPreintegration& other, double tol) const;
   /// @}
 
   /// @name Main functionality
@@ -89,8 +90,8 @@ public:
   /// It takes measured quantities in the j frame
   /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
   /// NOTE(frank): implementation is different in two versions
-  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
+  void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
-                            Matrix9* A, Matrix93* B, Matrix93* C) override;
+              Matrix9* A, Matrix93* B, Matrix93* C) override;
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far

gtsam/navigation/PreintegrationBase.cpp

@@ -31,7 +31,6 @@ namespace gtsam {
 PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
                                        const Bias& biasHat)
     : p_(p), biasHat_(biasHat), deltaTij_(0.0) {
-  resetIntegration();
 }
 
 //------------------------------------------------------------------------------
@@ -96,6 +95,16 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
   return make_pair(correctedAcc, correctedOmega);
 }
 
+//------------------------------------------------------------------------------
+void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
+    const Vector3& measuredOmega, double dt) {
+  // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
+  // even when not of interest to the caller. Provide scratch space here.
+  Matrix9 A;
+  Matrix93 B, C;
+  update(measuredAcc, measuredOmega, dt, &A, &B, &C);
+}
+
 //------------------------------------------------------------------------------
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,

gtsam/navigation/PreintegrationBase.h

@@ -124,9 +124,9 @@ public:
   const imuBias::ConstantBias& biasHat() const { return biasHat_; }
   double deltaTij() const { return deltaTij_; }
 
-  virtual Vector3 deltaPij() const=0;
+  virtual Vector3  deltaPij() const=0;
-  virtual Vector3 deltaVij() const=0;
+  virtual Vector3  deltaVij() const=0;
-  virtual Rot3 deltaRij() const=0;
+  virtual Rot3     deltaRij() const=0;
   virtual NavState deltaXij() const=0;
 
   // Exposed for MATLAB
@@ -136,8 +136,7 @@ public:
   /// @name Testable
   /// @{
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
-  void print(const std::string& s) const;
+  virtual void print(const std::string& s) const;
-  virtual bool equals(const PreintegrationBase& other, double tol) const = 0;
   /// @}
 
   /// @name Main functionality
@@ -155,9 +154,13 @@ public:
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
   /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
-  virtual void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega,
+  virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
       const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
 
+  /// Version without derivatives
+  virtual void integrateMeasurement(const Vector3& measuredAcc,
+      const Vector3& measuredOmega, const double dt);
+
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
@@ -182,11 +185,6 @@ public:
       OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
           boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
 
-  /** Dummy clone for MATLAB */
-  virtual boost::shared_ptr<PreintegrationBase> clone() const {
-    return boost::shared_ptr<PreintegrationBase>();
-  }
-
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
   /// @name Deprecated
   /// @{

gtsam/navigation/TangentPreintegration.cpp

@@ -25,8 +25,8 @@ namespace gtsam {
 
 //------------------------------------------------------------------------------
 TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
-                                       const Bias& biasHat)
+    const Bias& biasHat) :
-    : TangentPreintegration(p,biasHat) {
+    PreintegrationBase(p, biasHat) {
   resetIntegration();
 }
 
@@ -41,20 +41,21 @@ void TangentPreintegration::resetIntegration() {
 //------------------------------------------------------------------------------
 bool TangentPreintegration::equals(const TangentPreintegration& other,
     double tol) const {
-  return p_->equals(*other.p_, tol)
+  return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
-      && fabs(deltaTij_ - other.deltaTij_) < tol
       && biasHat_.equals(other.biasHat_, tol)
       && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
-      && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
+      && equal_with_abs_tol(preintegrated_H_biasAcc_,
-      && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
+          other.preintegrated_H_biasAcc_, tol)
+      && equal_with_abs_tol(preintegrated_H_biasOmega_,
+          other.preintegrated_H_biasOmega_, tol);
 }
 
 //------------------------------------------------------------------------------
 // See extensive discussion in ImuFactor.lyx
-Vector9 TangentPreintegration::UpdatePreintegrated(
+Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
-    const Vector3& a_body, const Vector3& w_body, double dt,
+    const Vector3& w_body, double dt, const Vector9& preintegrated,
-    const Vector9& preintegrated, OptionalJacobian<9, 9> A,
+    OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
-    OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
+    OptionalJacobian<9, 3> C) {
   const auto theta = preintegrated.segment<3>(0);
   const auto position = preintegrated.segment<3>(3);
   const auto velocity = preintegrated.segment<3>(6);
@@ -65,27 +66,27 @@ Vector9 TangentPreintegration::UpdatePreintegrated(
 
   // Calculate exact mean propagation
   Matrix3 w_tangent_H_theta, invH;
-  const Vector3 w_tangent =  // angular velocity mapped back to tangent space
+  const Vector3 w_tangent = // angular velocity mapped back to tangent space
       local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
   const SO3 R = local.expmap();
   const Vector3 a_nav = R * a_body;
   const double dt22 = 0.5 * dt * dt;
 
   Vector9 preintegratedPlus;
-  preintegratedPlus <<                        // new preintegrated vector:
+  preintegratedPlus << // new preintegrated vector:
-      theta + w_tangent* dt,                  // theta
+      theta + w_tangent * dt, // theta
-      position + velocity* dt + a_nav* dt22,  // position
+  position + velocity * dt + a_nav * dt22, // position
-      velocity + a_nav* dt;                   // velocity
+  velocity + a_nav * dt; // velocity
 
   if (A) {
     // Exact derivative of R*a with respect to theta:
     const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
 
     A->setIdentity();
-    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt;  // theta
+    A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
-    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22;  // position wrpt theta...
+    A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
-    A->block<3, 3>(3, 6) = I_3x3 * dt;            // .. and velocity
+    A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
-    A->block<3, 3>(6, 0) = a_nav_H_theta * dt;    // velocity wrpt theta
+    A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
   }
   if (B) {
     B->block<3, 3>(0, 0) = Z_3x3;
@@ -102,10 +103,9 @@ Vector9 TangentPreintegration::UpdatePreintegrated(
 }
 
 //------------------------------------------------------------------------------
-void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
+void TangentPreintegration::update(const Vector3& measuredAcc,
-                                              const Vector3& measuredOmega,
+    const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
-                                              const double dt, Matrix9* A,
+    Matrix93* C) {
-                                              Matrix93* B, Matrix93* C) {
   // Correct for bias in the sensor frame
   Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
   Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
@@ -124,8 +124,8 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
     // More complicated derivatives in case of non-trivial sensor pose
     *C *= D_correctedOmega_omega;
     if (!p().body_P_sensor->translation().isZero())
-      *C += *B* D_correctedAcc_omega;
+      *C += *B * D_correctedAcc_omega;
-    *B *= D_correctedAcc_acc;  // NOTE(frank): needs to be last
+    *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
   }
 
   // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
@@ -135,24 +135,15 @@ void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
   preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
 }
 
-void TangentPreintegration::integrateMeasurement(const Vector3& measuredAcc,
-                                              const Vector3& measuredOmega,
-                                              double dt) {
-  // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
-  // even when not of interest to the caller. Provide scratch space here.
-  Matrix9 A;
-  Matrix93 B, C;
-  integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
-}
 //------------------------------------------------------------------------------
 Vector9 TangentPreintegration::biasCorrectedDelta(
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
   // We correct for a change between bias_i and the biasHat_ used to integrate
   // This is a simple linear correction with obvious derivatives
   const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
-  const Vector9 biasCorrected =
+  const Vector9 biasCorrected = preintegrated()
-      preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
+      + preintegrated_H_biasAcc_ * biasIncr.accelerometer()
-      preintegrated_H_biasOmega_ * biasIncr.gyroscope();
+      + preintegrated_H_biasOmega_ * biasIncr.gyroscope();
 
   if (H) {
     (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
@@ -174,9 +165,8 @@ Vector9 TangentPreintegration::biasCorrectedDelta(
 
 //------------------------------------------------------------------------------
 Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
-                                    const Vector9& zeta12, double deltaT12,
+    const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
-                                    OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 9> H2) {
-                                    OptionalJacobian<9, 9> H2) {
   const auto t01 = zeta01.segment<3>(0);
   const auto p01 = zeta01.segment<3>(3);
   const auto v01 = zeta01.segment<3>(6);
@@ -195,9 +185,9 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
   Matrix3 t02_H_R02;
   Vector9 zeta02;
   const Matrix3 R = R01.matrix();
-  zeta02 << Rot3::Logmap(R02, t02_H_R02),  // theta
+  zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
-      p01 + v01 * deltaT12 + R * p12,      // position
+  p01 + v01 * deltaT12 + R * p12, // position
-      v01 + R * v12;                       // velocity
+  v01 + R * v12; // velocity
 
   if (H1) {
     H1->setIdentity();
@@ -218,14 +208,16 @@ Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
 }
 
 //------------------------------------------------------------------------------
-void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix9* H1,
+void TangentPreintegration::mergeWith(const TangentPreintegration& pim12,
-                                   Matrix9* H2) {
+    Matrix9* H1, Matrix9* H2) {
   if (!matchesParamsWith(pim12)) {
-    throw std::domain_error("Cannot merge pre-integrated measurements with different params");
+    throw std::domain_error(
+        "Cannot merge pre-integrated measurements with different params");
   }
 
   if (params()->body_P_sensor) {
-    throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
+    throw std::domain_error(
+        "Cannot merge pre-integrated measurements with sensor pose yet");
   }
 
   const double t01 = deltaTij();
@@ -241,13 +233,13 @@ void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, Matrix
 
   preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
 
-  preintegrated_H_biasAcc_ =
+  preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_
-      (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
+      + (*H2) * pim12.preintegrated_H_biasAcc_;
 
-  preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
+  preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_
-                               (*H2) * pim12.preintegrated_H_biasOmega_;
+      + (*H2) * pim12.preintegrated_H_biasOmega_;
 }
 
 //------------------------------------------------------------------------------
 
-}  // namespace gtsam
+}// namespace gtsam

gtsam/navigation/TangentPreintegration.h

@@ -64,9 +64,9 @@ public:
 
   /// @name Instance variables access
   /// @{
-  Vector3 deltaPij()  const override { return preintegrated_.segment<3>(3); }
+  Vector3  deltaPij() const override { return preintegrated_.segment<3>(3); }
-  Vector3 deltaVij()  const override { return preintegrated_.tail<3>(); }
+  Vector3  deltaVij() const override { return preintegrated_.tail<3>(); }
-  Rot3 deltaRij()     const override { return Rot3::Expmap(theta()); }
+  Rot3     deltaRij() const override { return Rot3::Expmap(theta()); }
   NavState deltaXij() const override { return NavState::Retract(preintegrated_); }
 
   const Vector9& preintegrated() const { return preintegrated_; }
@@ -76,7 +76,7 @@ public:
 
   /// @name Testable
   /// @{
-  bool equals(const TangentPreintegration& other, double tol) const override;
+  bool equals(const TangentPreintegration& other, double tol) const;
   /// @}
 
   /// @name Main functionality
@@ -91,15 +91,12 @@ public:
                                      OptionalJacobian<9, 3> B = boost::none,
                                      OptionalJacobian<9, 3> C = boost::none);
 
-  // Version without derivatives
-  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt);
-
   /// Update preintegrated measurements and get derivatives
   /// It takes measured quantities in the j frame
   /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
   /// NOTE(frank): implementation is different in two versions
-  void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
+  void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-                            Matrix9* A, Matrix93* B, Matrix93* C) override;
+      const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
 
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far