Isolated PreintegrationParams in separate class

gtsam/navigation/CombinedImuFactor.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -63,14 +63,14 @@ public:
 
   /// Parameters for pre-integration:
   /// Usage: Create just a single Params and pass a shared pointer to the constructor
-  struct Params : PreintegrationBase::Params {
+  struct Params : PreintegrationParams {
     Matrix3 biasAccCovariance;    ///< continuous-time "Covariance" describing accelerometer bias random walk
     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 n_gravity in body frame
     Params(const Vector3& n_gravity) :
-        PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
+        PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
             I_3x3), biasAccOmegaInit(I_6x6) {
     }
 

gtsam/navigation/ImuFactor.h

@@ -82,7 +82,7 @@ public:
    *  @param bias Current estimate of acceleration and rotation rate biases
    *  @param p    Parameters, typically fixed in a single application
    */
-  PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
+  PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       PreintegrationBase(p, biasHat) {
     preintMeasCov_.setZero();
@@ -230,7 +230,7 @@ public:
   /// @deprecated typename
   typedef PreintegratedImuMeasurements PreintegratedMeasurements;
 
-  /// @deprecated constructor, in the new one gravity, coriolis settings are in Params
+  /// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams
   ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
       const PreintegratedMeasurements& preintegratedMeasurements,
       const Vector3& n_gravity, const Vector3& omegaCoriolis,
@@ -238,7 +238,7 @@ public:
       const bool use2ndOrderCoriolis = false);
 
   /// @deprecated use PreintegrationBase::predict,
-  /// in the new one gravity, coriolis settings are in Params
+  /// in the new one gravity, coriolis settings are in PreintegrationParams
   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& n_gravity,

gtsam/navigation/PreintegrationBase.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -20,38 +20,14 @@
  **/
 
 #include "PreintegrationBase.h"
+#ifdef ALLOW_DEPRECATED_IN_GTSAM4
 #include <boost/make_shared.hpp>
+#endif
 
 using namespace std;
 
 namespace gtsam {
 
-//------------------------------------------------------------------------------
-void PreintegrationBase::Params::print(const string& s) const {
-  PreintegratedRotation::Params::print(s);
-  cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
-      << endl;
-  cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
-      << endl;
-  if (omegaCoriolis && use2ndOrderCoriolis)
-    cout << "Using 2nd-order Coriolis" << endl;
-  if (body_P_sensor)
-    body_P_sensor->print("    ");
-  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;

gtsam/navigation/PreintegrationBase.h

@@ -21,10 +21,9 @@
 
 #pragma once
 
-#include <gtsam/navigation/PreintegratedRotation.h>
+#include <gtsam/navigation/PreintegrationParams.h>
 #include <gtsam/navigation/NavState.h>
 #include <gtsam/navigation/ImuBias.h>
-#include <boost/make_shared.hpp>
 
 namespace gtsam {
 
@@ -55,54 +54,6 @@ struct PoseVelocityBias {
  */
 class PreintegrationBase {
 
-public:
-
-  /// Parameters for pre-integration:
-  /// Usage: Create just a single Params and pass a shared pointer to the constructor
-  struct Params: PreintegratedRotation::Params {
-    Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
-    Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
-    bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
-    Vector3 n_gravity; ///< Gravity vector in nav 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& n_gravity) :
-        accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
-            false), n_gravity(n_gravity) {
-    }
-
-    // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
-    static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
-      return boost::make_shared<Params>(Vector3(0, 0, g));
-    }
-
-    // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
-    static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
-      return boost::make_shared<Params>(Vector3(0, 0, -g));
-    }
-
-    void print(const std::string& s) const;
-    bool equals(const PreintegratedRotation::Params& other, double tol) const;
-
-  protected:
-    /// Default constructor for serialization only: uninitialized!
-    Params() {}
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      namespace bs = ::boost::serialization;
-      ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
-           boost::serialization::base_object<PreintegratedRotation::Params>(*this));
-      ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
-      ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
-      ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
-      ar & BOOST_SERIALIZATION_NVP(n_gravity);
-    }
-  };
-
 protected:
 
   /// Parameters. Declared mutable only for deprecated predict method.
@@ -110,7 +61,7 @@ protected:
 #ifdef ALLOW_DEPRECATED_IN_GTSAM4
   mutable
 #endif
-  boost::shared_ptr<Params> p_;
+  boost::shared_ptr<PreintegrationParams> p_;
 
   /// Acceleration and gyro bias used for preintegration
   imuBias::ConstantBias biasHat_;
@@ -146,7 +97,7 @@ public:
    *  @param p    Parameters, typically fixed in a single application
    *  @param bias Current estimate of acceleration and rotation rate biases
    */
-  PreintegrationBase(const boost::shared_ptr<Params>& p,
+  PreintegrationBase(const boost::shared_ptr<PreintegrationParams>& p,
       const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       p_(p), biasHat_(biasHat) {
     resetIntegration();
@@ -155,7 +106,7 @@ public:
   /**
    *  Constructor which takes in all members for generic construction
    */
-  PreintegrationBase(const boost::shared_ptr<Params>& p, const imuBias::ConstantBias& biasHat,
+  PreintegrationBase(const boost::shared_ptr<PreintegrationParams>& p, const imuBias::ConstantBias& biasHat,
                      double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc,
                      const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc,
                      const Matrix3& delVdelBiasOmega)
@@ -174,19 +125,19 @@ public:
   /// Re-initialize PreintegratedMeasurements
   void resetIntegration();
 
-  /// check parameters equality: checks whether shared pointer points to same Params object.
+  /// check parameters equality: checks whether shared pointer points to same PreintegrationParams object.
   bool matchesParamsWith(const PreintegrationBase& other) const {
     return p_ == other.p_;
   }
 
   /// shared pointer to params
-  const boost::shared_ptr<Params>& params() const {
+  const boost::shared_ptr<PreintegrationParams>& params() const {
     return p_;
   }
 
   /// const reference to params
-  const Params& p() const {
+  const PreintegrationParams& p() const {
-    return *boost::static_pointer_cast<Params>(p_);
+    return *boost::static_pointer_cast<PreintegrationParams>(p_);
   }
 
   void set_body_P_sensor(const Pose3& body_P_sensor) {

gtsam/navigation/PreintegrationParams.cpp

@@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegrationParams.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include "PreintegrationParams.h"
+
+using namespace std;
+
+namespace gtsam {
+
+//------------------------------------------------------------------------------
+void PreintegrationParams::print(const string& s) const {
+  PreintegratedRotation::Params::print(s);
+  cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
+       << endl;
+  cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
+       << endl;
+  if (omegaCoriolis && use2ndOrderCoriolis)
+    cout << "Using 2nd-order Coriolis" << endl;
+  if (body_P_sensor) body_P_sensor->print("    ");
+  cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
+}
+
+//------------------------------------------------------------------------------
+bool PreintegrationParams::equals(const PreintegratedRotation::Params& other,
+                                  double tol) const {
+  auto e = dynamic_cast<const PreintegrationParams*>(&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);
+}
+
+}  // namespace gtsam

gtsam/navigation/PreintegrationParams.h

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  PreintegrationParams.h
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/navigation/PreintegratedRotation.h>
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+/// Parameters for pre-integration:
+/// Usage: Create just a single Params and pass a shared pointer to the constructor
+struct PreintegrationParams: PreintegratedRotation::Params {
+  Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
+  Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
+  bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
+  Vector3 n_gravity; ///< Gravity vector in nav frame
+
+  /// The Params constructor insists on getting the navigation frame gravity vector
+  /// For convenience, two commonly used conventions are provided by named constructors below
+  PreintegrationParams(const Vector3& n_gravity)
+      : accelerometerCovariance(I_3x3),
+        integrationCovariance(I_3x3),
+        use2ndOrderCoriolis(false),
+        n_gravity(n_gravity) {}
+
+  // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
+  static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
+    return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g));
+  }
+
+  // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+  static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
+    return boost::make_shared<PreintegrationParams>(Vector3(0, 0, -g));
+  }
+
+  void print(const std::string& s) const;
+  bool equals(const PreintegratedRotation::Params& other, double tol) const;
+
+protected:
+  /// Default constructor for serialization only: uninitialized!
+  PreintegrationParams() {}
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
+    ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
+         boost::serialization::base_object<PreintegratedRotation::Params>(*this));
+    ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
+    ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
+    ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
+    ar & BOOST_SERIALIZATION_NVP(n_gravity);
+  }
+};
+
+} // namespace gtsam

gtsam/navigation/ScenarioRunner.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/navigation/ScenarioRunner.h>
 #include <gtsam/base/timing.h>
 
+#include <boost/assign.hpp>
 #include <cmath>
 
 namespace gtsam {

gtsam/navigation/ScenarioRunner.h

@@ -28,7 +28,7 @@ namespace gtsam {
  */
 class ScenarioRunner {
  public:
-  typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
+  typedef boost::shared_ptr<PreintegrationParams> SharedParams;
   ScenarioRunner(const Scenario* scenario, const SharedParams& p,
                  double imuSampleTime = 1.0 / 100.0,
                  const imuBias::ConstantBias& bias = imuBias::ConstantBias())

gtsam/navigation/tests/testImuFactor.cpp

@@ -62,8 +62,8 @@ static const double kGyroSigma = 0.02;
 static const double kAccelerometerSigma = 0.1;
 
 // Create default parameters with Z-down and above noise paramaters
-static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
+static boost::shared_ptr<PreintegrationParams> defaultParams() {
-  auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity);
+  auto p = PreintegrationParams::MakeSharedD(kGravity);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
       * I_3x3;

gtsam/navigation/tests/testScenarioRunner.cpp

@@ -32,8 +32,8 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
 static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
 
 // Create default parameters with Z-down and above noise parameters
-static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
+static boost::shared_ptr<PreintegrationParams> defaultParams() {
-  auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
+  auto p = PreintegrationParams::MakeSharedD(10);
   p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
   p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
   p->integrationCovariance = 0.0000001 * I_3x3;