added ImuFactorBase. In the process of moving stuff to base class

gtsam/navigation/CombinedImuFactor.cpp

@@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
 // CombinedImuFactor methods
 //------------------------------------------------------------------------------
 CombinedImuFactor::CombinedImuFactor() :
-    ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
+    ImuFactorBase(), 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,7 +168,7 @@ 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),
-          ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
+          ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
           preintegratedMeasurements_(preintegratedMeasurements) {}
 
 //------------------------------------------------------------------------------
@@ -186,12 +186,9 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
       << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << ","
       << keyFormatter(this->key6()) << ")\n";
+  ImuFactorBase::print("");
   preintegratedMeasurements_.print("  preintegrated measurements:");
-  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
-  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
   this->noiseModel_->print("  noise model: ");
-  if(this->body_P_sensor_)
-    this->body_P_sensor_->print("  sensor pose in body frame: ");
 }
 
 //------------------------------------------------------------------------------
@@ -199,9 +196,7 @@ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) cons
   const This *e =  dynamic_cast<const This*> (&expected);
   return e != NULL && Base::equals(*e, tol)
   && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
-  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && ImuFactorBase::equals(*e, tol);
-  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
 }
 
 //------------------------------------------------------------------------------
@@ -393,7 +388,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
 //------------------------------------------------------------------------------
 PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
     const imuBias::ConstantBias& bias_i,
-    const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+    const PreintegrationBase& preintegratedMeasurements,
     const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
 
   const double& deltaTij = preintegratedMeasurements.deltaTij_;
@@ -433,8 +428,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3&
   const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
 
   Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-
+  return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
-  return PoseVelocityBias(pose_j, vel_j, bias_i);
 }
 
 } /// namespace gtsam

gtsam/navigation/CombinedImuFactor.h

@@ -24,6 +24,7 @@
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/navigation/ImuFactorBase.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
@@ -70,7 +71,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>, public ImuBase{
+class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{
 public:
 
   /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
@@ -217,7 +218,7 @@ public:
   /// predicted states from IMU
   static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
       const imuBias::ConstantBias& bias_i,
-      const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+      const PreintegrationBase& preintegratedMeasurements,
       const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
 private:

gtsam/navigation/ImuFactor.cpp

@@ -41,7 +41,7 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
   measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
   measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
   measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
-  preintMeasCov_.setZero(9,9);
+  preintMeasCov_.setZero();
 }
 
 //------------------------------------------------------------------------------
@@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
 // ImuFactor methods
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor() :
-    ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
+    ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
 
 //------------------------------------------------------------------------------
 ImuFactor::ImuFactor(
@@ -150,7 +150,7 @@ 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),
-        ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
+        ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
         preintegratedMeasurements_(preintegratedMeasurements) {}
 
 //------------------------------------------------------------------------------
@@ -167,12 +167,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
       << keyFormatter(this->key3()) << ","
       << keyFormatter(this->key4()) << ","
       << keyFormatter(this->key5()) << ")\n";
+  ImuFactorBase::print("");
   preintegratedMeasurements_.print("  preintegrated measurements:");
-  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
-  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
   this->noiseModel_->print("  noise model: ");
-  if(this->body_P_sensor_)
-    this->body_P_sensor_->print("  sensor pose in body frame: ");
 }
 
 //------------------------------------------------------------------------------
@@ -180,9 +177,7 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This *e =  dynamic_cast<const This*> (&expected);
   return e != NULL && Base::equals(*e, tol)
   && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
-  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && ImuFactorBase::equals(*e, tol);
-  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
 }
 
 //------------------------------------------------------------------------------
@@ -333,14 +328,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
 }
 
 //------------------------------------------------------------------------------
-PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
-    const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
+    const imuBias::ConstantBias& bias_i,
-    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
+    const PreintegrationBase& preintegratedMeasurements,
-{
+    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
 
   const double& deltaTij = preintegratedMeasurements.deltaTij_;
-  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
+  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
+  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
 
   const Rot3 Rot_i = pose_i.rotation();
   const Vector3 pos_i = pose_i.translation().vector();
@@ -375,7 +370,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
   const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
 
   Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-  return PoseVelocity(pose_j, vel_j);
+  return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
 }
 
 } /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -24,6 +24,7 @@
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/navigation/PreintegrationBase.h>
+#include <gtsam/navigation/ImuFactorBase.h>
 #include <gtsam/base/debug.h>
 
 namespace gtsam {
@@ -47,11 +48,14 @@ namespace gtsam {
 /**
  * Struct to hold return variables by the Predict Function
  */
-struct PoseVelocity {
+struct PoseVelocityBias {
   Pose3 pose;
   Vector3 velocity;
-  PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
+  imuBias::ConstantBias bias;
-    pose(_pose), velocity(_velocity) {
+
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+      const imuBias::ConstantBias _bias) :
+        pose(_pose), velocity(_velocity), bias(_bias) {
   }
 };
 
@@ -63,7 +67,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>, public ImuBase {
+class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
 public:
 
   /**
@@ -202,8 +206,8 @@ public:
       boost::optional<Matrix&> H5 = boost::none) const;
 
   /// predicted states from IMU
-  static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
+  static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
-      const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
+      const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements,
       const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
   private:

gtsam/navigation/ImuFactorBase.h

@@ -0,0 +1,74 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  PreintegrationBase.h
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+/* GTSAM includes */
+#include <gtsam/navigation/ImuBias.h>
+
+namespace gtsam {
+
+class ImuFactorBase {
+
+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:
+
+  ImuFactorBase() :
+    gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
+    body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
+
+  ImuFactorBase(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_; }
+
+  /// Needed for testable
+  //------------------------------------------------------------------------------
+  void print(const std::string& s) const {
+    std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
+    std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
+    std::cout << "  use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl;
+    if(this->body_P_sensor_)
+      this->body_P_sensor_->print("  sensor pose in body frame: ");
+  }
+
+  /// Needed for testable
+  //------------------------------------------------------------------------------
+  bool equals(const ImuFactorBase& expected, double tol) const {
+    return equal_with_abs_tol(gravity_, expected.gravity_, tol)
+    && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
+    && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
+    && ((!body_P_sensor_ && !expected.body_P_sensor_) ||
+        (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
+  }
+
+};
+
+} /// namespace gtsam

gtsam/navigation/PreintegrationBase.h

@@ -81,11 +81,11 @@ public:
   /// Needed for testable
   bool equals(const PreintegrationBase& expected, double tol) const {
     return biasHat_.equals(expected.biasHat_, tol)
-        && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+    && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
-        && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+    && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
-        && deltaRij_.equals(expected.deltaRij_, tol)
+    && deltaRij_.equals(expected.deltaRij_, tol)
-        && fabs(deltaTij_ - expected.deltaTij_) < tol
+    && fabs(deltaTij_ - expected.deltaTij_) < tol
-        && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
+    && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
     && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
     && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
     && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)