added ImuBase class

gtsam/navigation/CombinedImuFactor.cpp

@@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor() :
-    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
+    ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
 
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
@@ -168,12 +168,8 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_
     const Vector3& gravity, const Vector3& omegaCoriolis,
     boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
           Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
-          preintegratedMeasurements_(preintegratedMeasurements),
+          ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
-          gravity_(gravity),
+          preintegratedMeasurements_(preintegratedMeasurements) {}
-          omegaCoriolis_(omegaCoriolis),
-          body_P_sensor_(body_P_sensor),
-          use2ndOrderCoriolis_(use2ndOrderCoriolis){
-}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {

gtsam/navigation/CombinedImuFactor.h

@@ -70,7 +70,7 @@ struct PoseVelocityBias {
  * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
  * and the preintegrated measurements uncertainty.
  */
-class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
+class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuBase{
 public:
 
   /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
@@ -152,11 +152,6 @@ private:
   typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
 
   CombinedPreintegratedMeasurements preintegratedMeasurements_;
-  Vector3 gravity_;
-  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
-
-  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 
 public:
 
@@ -207,10 +202,6 @@ public:
   const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
     return preintegratedMeasurements_; }
 
-  const Vector3& gravity() const { return gravity_; }
-
-  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
-
   /** implement functions needed to derive from Factor */
 
   /// vector of errors

gtsam/navigation/ImuFactor.cpp

@@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
 // ImuFactor methods
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor() :
-    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
+    ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
 
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(
@@ -150,12 +150,8 @@ ImuFactor::ImuFactor(
     boost::optional<const Pose3&> body_P_sensor,
     const bool use2ndOrderCoriolis) :
         Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
-        preintegratedMeasurements_(preintegratedMeasurements),
+        ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
-        gravity_(gravity),
+        preintegratedMeasurements_(preintegratedMeasurements) {}
-        omegaCoriolis_(omegaCoriolis),
-        body_P_sensor_(body_P_sensor),
-        use2ndOrderCoriolis_(use2ndOrderCoriolis){
-}
 
 //------------------------------------------------------------------------------
 gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {

gtsam/navigation/ImuFactor.h

@@ -63,7 +63,7 @@ struct PoseVelocity {
  * Note that this factor does not force "temporal consistency" of the biases (which are usually
  * slowly varying quantities), see also CombinedImuFactor for more details.
  */
-class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
+class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuBase {
 public:
 
   /**
@@ -141,11 +141,6 @@ public:
   typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
 
   PreintegratedMeasurements preintegratedMeasurements_;
-  Vector3 gravity_;
-  Vector3 omegaCoriolis_;
-  boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
-
-  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 
   public:
 
@@ -195,10 +190,6 @@ public:
   const PreintegratedMeasurements& preintegratedMeasurements() const {
     return preintegratedMeasurements_; }
 
-  const Vector3& gravity() const { return gravity_; }
-
-  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
-
   /** implement functions needed to derive from Factor */
 
   /// vector of errors

gtsam/navigation/PreintegrationBase.h

@@ -66,7 +66,7 @@ public:
     deltaRij_(Rot3()), deltaTij_(0.0),
     delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
     delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
-    delRdelBiasOmega_(Z_3x3) { }
+    delRdelBiasOmega_(Z_3x3) {}
 
   /// Needed for testable
   void print(const std::string& s) const {
@@ -199,4 +199,29 @@ private:
   }
 };
 
+class ImuBase {
+
+protected:
+
+  Vector3 gravity_;
+  Vector3 omegaCoriolis_;
+  boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
+  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+
+public:
+
+  ImuBase() :
+    gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
+    body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
+
+  ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
+      boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
+        gravity_(gravity), omegaCoriolis_(omegaCoriolis),
+        body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
+
+  const Vector3& gravity() const { return gravity_; }
+  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
+};
+
 } /// namespace gtsam