matchesParamsWith, a few new constructors, and Doxygen streamlining

gtsam/navigation/CombinedImuFactor.h

@@ -113,6 +113,9 @@ public:
   friend class CombinedImuFactor;
 
  public:
+  /// @name Constructors
+  /// @{
+
   /**
    *  Default constructor, initializes the class with no measurements
    *  @param bias Current estimate of acceleration and rotation rate biases
@@ -123,18 +126,32 @@ public:
     preintMeasCov_.setZero();
   }
 
-  Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
+  /// @}
 
-  /// print
+  /// @name Basic utilities
-  void print(const std::string& s = "Preintegrated Measurements:") const;
+  /// @{
-
-  /// equals
-  bool equals(const PreintegratedCombinedMeasurements& expected,
-              double tol = 1e-9) const;
 
   /// Re-initialize PreintegratedCombinedMeasurements
   void resetIntegration();
 
+  /// const reference to params, shadows definition in base class
+  Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
+  /// @}
+
+  /// @name Access instance variables
+  /// @{
+  Matrix preintMeasCov() const { return preintMeasCov_; }
+  /// @}
+
+  /// @name Testable
+  /// @{
+  void print(const std::string& s = "Preintegrated Measurements:") const;
+  bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
+  /// @}
+
+  /// @name Main functionality
+  /// @{
+
   /**
    * Add a single IMU measurement to the preintegration.
    * @param measuredAcc Measured acceleration (in body frame, as given by the
@@ -147,8 +164,10 @@ public:
   void integrateMeasurement(const Vector3& measuredAcc,
       const Vector3& measuredOmega, double deltaT);
 
-  /// methods to access class variables
+  /// @}
-  Matrix preintMeasCov() const { return preintMeasCov_; }
+
+  /// @name Deprecated
+  /// @{
 
   /// deprecated constructor
   /// NOTE(frank): assumes Z-Down convention, only second order integration supported
@@ -159,6 +178,8 @@ public:
       const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
       const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
 
+  /// @}
+
  private:
   /// Serialization function
   friend class boost::serialization::access;

gtsam/navigation/ImuFactor.h

@@ -81,11 +81,21 @@ public:
    *  @param p    Parameters, typically fixed in a single application
    */
   PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
-      const imuBias::ConstantBias& biasHat) :
+      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       PreintegrationBase(p, biasHat) {
     preintMeasCov_.setZero();
   }
 
+/**
+  *  Construct preintegrated directly from members: base class and preintMeasCov
+  *  @param base               PreintegrationBase instance
+  *  @param preintMeasCov      Covariance matrix used in noise model.
+  */
+  PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
+     : PreintegrationBase(base),
+       preintMeasCov_(preintMeasCov) {
+  }
+
   /// print
   void print(const std::string& s = "Preintegrated Measurements:") const;
 
@@ -167,7 +177,7 @@ public:
 #endif
 
   /** Default constructor - only use for serialization */
-  ImuFactor();
+  ImuFactor() {}
 
   /**
    * Constructor
@@ -241,4 +251,10 @@ private:
 };
 // class ImuFactor
 
+template <>
+struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
+
+template <>
+struct traits<ImuFactor> : public Testable<ImuFactor> {};
+
 } /// namespace gtsam

gtsam/navigation/PreintegratedRotation.cpp

@@ -42,14 +42,15 @@ void PreintegratedRotation::resetIntegration() {
 }
 
 void PreintegratedRotation::print(const string& s) const {
-  cout << s << endl;
+  cout << s;
   cout << "    deltaTij [" << deltaTij_ << "]" << endl;
   cout << "    deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
 }
 
 bool PreintegratedRotation::equals(const PreintegratedRotation& other,
     double tol) const {
-  return deltaRij_.equals(other.deltaRij_, tol)
+  return this->matchesParamsWith(other)
+      && deltaRij_.equals(other.deltaRij_, tol)
       && fabs(deltaTij_ - other.deltaTij_) < tol
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
 }
@@ -75,12 +76,16 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
   return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
 }
 
-void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
+void PreintegratedRotation::integrateMeasurement(
-    const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
+    const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-    Matrix3* 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,
+  // If asked, pass first derivative as well
-      D_incrR_integratedOmega);
+  if (optional_D_incrR_integratedOmega) {
+    *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
+  }
 
   // Update deltaTij and rotation
   deltaTij_ += deltaT;
@@ -88,8 +93,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
+  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
-      - *D_incrR_integratedOmega * deltaT;
 }
 
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

gtsam/navigation/PreintegratedRotation.h

@@ -33,7 +33,6 @@ namespace gtsam {
  */
 class PreintegratedRotation {
  public:
-
   /// Parameters for pre-integration:
   /// Usage: Create just a single Params and pass a shared pointer to the constructor
   struct Params {
@@ -41,9 +40,7 @@ public:
     boost::optional<Vector3> omegaCoriolis;  ///< Coriolis constant
     boost::optional<Pose3> body_P_sensor;    ///< The pose of the sensor in the body frame
 
-    Params() :
+    Params() : gyroscopeCovariance(I_3x3) {}
-        gyroscopeCovariance(I_3x3) {
-    }
 
     virtual void print(const std::string& s) const;
 
@@ -59,29 +56,50 @@ public:
   };
 
  protected:
+  /// Parameters
+  boost::shared_ptr<Params> p_;
+
   double deltaTij_;           ///< Time interval from i to j
   Rot3 deltaRij_;             ///< Preintegrated relative orientation (in frame i)
   Matrix3 delRdelBiasOmega_;  ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
 
-  /// Parameters
-  boost::shared_ptr<Params> p_;
-
   /// Default constructor for serialization
-  PreintegratedRotation() {
+  PreintegratedRotation() {}
-  }
 
  public:
+  /// @name Constructors
+  /// @{
+
   /// Default constructor, resets integration to zero
-  explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
+  explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
-      p_(p) {
     resetIntegration();
   }
 
+  /// Explicit initialization of all class members
+  PreintegratedRotation(const boost::shared_ptr<Params>& p,
+                        double deltaTij, const Rot3& deltaRij,
+                        const Matrix3& delRdelBiasOmega)
+      : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
+
+  /// @}
+
+  /// @name Basic utilities
+  /// @{
+
   /// Re-initialize PreintegratedMeasurements
   void resetIntegration();
 
+  /// check parameters equality: checks whether shared pointer points to same Params object.
+  bool matchesParamsWith(const PreintegratedRotation& other) const {
+    return p_ == other.p_;
+  }
+  /// @}
+
   /// @name Access instance variables
   /// @{
+  const boost::shared_ptr<Params>& params() const {
+    return p_;
+  }
   const double& deltaTij() const {
     return deltaTij_;
   }
@@ -95,23 +113,24 @@ public:
 
   /// @name Testable
   /// @{
-
   void print(const std::string& s) const;
   bool equals(const PreintegratedRotation& other, double tol) const;
-
   /// @}
 
+  /// @name Main functionality
+  /// @{
+
   /// Take the gyro measurement, correct it using the (constant) bias estimate
   /// and possibly the sensor pose, and then integrate it forward in time to yield
   /// an incremental rotation.
-  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
+  Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-      double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
+                           OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
 
   /// Calculate an incremental rotation given the gyro measurement and a time interval,
   /// and update both deltaTij_ and deltaRij_.
-  void integrateMeasurement(const Vector3& measuredOmega,
+  void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
-      const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
+                            OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
-      Matrix3* F);
+                            OptionalJacobian<3, 3> F = boost::none);
 
   /// Return a bias corrected version of the integrated rotation, with optional Jacobian
   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
@@ -120,6 +139,8 @@ public:
   /// Integrate coriolis correction in body frame rot_i
   Vector3 integrateCoriolis(const Rot3& rot_i) const;
 
+  /// @}
+
  private:
   /** Serialization function */
   friend class boost::serialization::access;
@@ -132,4 +153,7 @@ private:
   }
 };
 
-} // namespace gtsam
+template <>
+struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
+
+}  /// namespace gtsam

gtsam/navigation/PreintegrationBase.cpp

@@ -64,7 +64,8 @@ void PreintegrationBase::print(const string& s) const {
 //------------------------------------------------------------------------------
 bool PreintegrationBase::equals(const PreintegrationBase& other,
     double tol) const {
-  return fabs(deltaTij_ - other.deltaTij_) < tol
+  return this->matchesParamsWith(other)
+      && fabs(deltaTij_ - other.deltaTij_) < tol
       && deltaXij_.equals(other.deltaXij_, tol)
       && biasHat_.equals(other.biasHat_, tol)
       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)

gtsam/navigation/PreintegrationBase.h

@@ -100,7 +100,14 @@ public:
 
 protected:
 
-  double deltaTij_;   ///< Time interval from i to j
+  /// Parameters
+  boost::shared_ptr<Params> p_;
+
+  /// Acceleration and gyro bias used for preintegration
+  imuBias::ConstantBias biasHat_;
+
+  /// Time interval from i to j
+  double deltaTij_;
 
   /**
    * Preintegrated navigation state, from frame i to frame j
@@ -109,12 +116,6 @@ protected:
    */
   NavState deltaXij_;
 
-  /// Parameters
-  boost::shared_ptr<Params> p_;
-
-  /// Acceleration and gyro bias used for preintegration
-  imuBias::ConstantBias biasHat_;
-
   Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
   Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
   Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
@@ -127,20 +128,53 @@ protected:
 
 public:
 
+  /// @name Constructors
+  /// @{
+
   /**
    *  Constructor, initializes the variables in the base class
-   *  @param bias Current estimate of acceleration and rotation rate biases
    *  @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,
-      const imuBias::ConstantBias& biasHat) :
+      const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
       p_(p), biasHat_(biasHat) {
     resetIntegration();
   }
 
+  /**
+   *  Constructor which takes in all members for generic construction
+   */
+  PreintegrationBase(const boost::shared_ptr<Params>& p, const imuBias::ConstantBias& biasHat,
+                     double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc,
+                     const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc,
+                     const Matrix3& delVdelBiasOmega)
+      : p_(p),
+        biasHat_(biasHat),
+        deltaTij_(deltaTij),
+        deltaXij_(deltaXij),
+        delPdelBiasAcc_(delPdelBiasAcc),
+        delPdelBiasOmega_(delPdelBiasOmega),
+        delVdelBiasAcc_(delVdelBiasAcc),
+        delVdelBiasOmega_(delVdelBiasOmega) {}
+  /// @}
+
+  /// @name Basic utilities
+  /// @{
   /// Re-initialize PreintegratedMeasurements
   void resetIntegration();
 
+  /// check parameters equality: checks whether shared pointer points to same Params object.
+  bool matchesParamsWith(const PreintegrationBase& other) const {
+    return p_ == other.p_;
+  }
+
+  /// shared pointer to params
+  const boost::shared_ptr<Params>& params() const {
+    return p_;
+  }
+
+  /// const reference to params
   const Params& p() const {
     return *boost::static_pointer_cast<Params>(p_);
   }
@@ -148,8 +182,10 @@ public:
   void set_body_P_sensor(const Pose3& body_P_sensor) {
     p_->body_P_sensor = body_P_sensor;
   }
+  /// @}
 
-  /// getters
+  /// @name Instance variables access
+  /// @{
   const NavState& deltaXij() const {
     return deltaXij_;
   }
@@ -183,17 +219,20 @@ public:
   const Matrix3& delVdelBiasOmega() const {
     return delVdelBiasOmega_;
   }
-
   // Exposed for MATLAB
   Vector6 biasHatVector() const {
     return biasHat_.vector();
   }
+  /// @}
 
-  /// print
+  /// @name Testable
+  /// @{
   void print(const std::string& s) const;
-
-  /// check equality
   bool equals(const PreintegrationBase& other, double tol) const;
+  /// @}
+
+  /// @name Main functionality
+  /// @{
 
   /// Subtract estimate and correct for sensor pose
   /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
@@ -236,11 +275,18 @@ public:
       OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
           boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
 
+  /// @}
+
+  /// @name Deprecated
+  /// @{
+
   /// @deprecated predict
   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
       const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
 
+  /// @}
+
 private:
   /** Serialization function */
   friend class boost::serialization::access;