Major updates to CombinedImuFactor to make it Testable as well as serializable

gtsam/navigation/CombinedImuFactor.cpp

@@ -17,9 +17,11 @@
  *  @author Vadim Indelman
  *  @author David Jensen
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  **/
 
 #include <gtsam/navigation/CombinedImuFactor.h>
+#include <boost/serialization/export.hpp>
 
 /* External or standard includes */
 #include <ostream>
@@ -28,6 +30,31 @@ namespace gtsam {
 
 using namespace std;
 
+//------------------------------------------------------------------------------
+// Inner class PreintegrationCombinedParams
+//------------------------------------------------------------------------------
+void PreintegrationCombinedParams::print(const string& s) const {
+  PreintegrationParams::print(s);
+  cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
+       << endl;
+  cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
+       << endl;
+  cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
+       << endl;
+}
+
+//------------------------------------------------------------------------------
+bool PreintegrationCombinedParams::equals(const PreintegrationParams& other,
+                                  double tol) const {
+  auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
+  return e != nullptr && PreintegrationParams::equals(other, tol) &&
+         equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
+                            tol) &&
+         equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
+                            tol) &&
+         equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
+}
+
 //------------------------------------------------------------------------------
 // Inner class PreintegratedCombinedMeasurements
 //------------------------------------------------------------------------------
@@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
   return r;
 }
 
+//------------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
+  f._PIM_.print("combined preintegrated measurements:\n");
+  os << "  noise model sigmas: " << f.noiseModel_->sigmas().transpose();
+  return os;
+}
+
 //------------------------------------------------------------------------------
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 CombinedImuFactor::CombinedImuFactor(

gtsam/navigation/CombinedImuFactor.h

@@ -17,6 +17,7 @@
  *  @author Vadim Indelman
  *  @author David Jensen
  *  @author Frank Dellaert
+ *  @author Varun Agrawal
  **/
 
 #pragma once
@@ -26,6 +27,7 @@
 #include <gtsam/navigation/TangentPreintegration.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/serialization.h>
 
 namespace gtsam {
 
@@ -61,10 +63,19 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
   Matrix3 biasOmegaCovariance;  ///< continuous-time "Covariance" describing gyroscope bias random walk
   Matrix6 biasAccOmegaInt;     ///< covariance of bias used for pre-integration
 
+  /// Default constructor makes unitialized params struct.
+  /// Used for serialization.
+  PreintegrationCombinedParams()
+      : PreintegrationParams(),
+        biasAccCovariance(I_3x3),
+        biasOmegaCovariance(I_3x3),
+        biasAccOmegaInt(I_6x6) {}
+
   /// See two named constructors below for good values of n_gravity in body frame
-PreintegrationCombinedParams(const Vector3& n_gravity) :
+  PreintegrationCombinedParams(const Vector3& n_gravity) :
-  PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
+    PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
-    I_3x3), biasAccOmegaInt(I_6x6) {
+    biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
+
   }
 
   // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
@@ -77,6 +88,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
     return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
   }
 
+  void print(const std::string& s="") const;
+  bool equals(const PreintegrationParams& other, double tol) const;
+
   void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
   void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
   void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
@@ -86,24 +100,25 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
   const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
   
 private:
-  /// Default constructor makes unitialized params struct
-  PreintegrationCombinedParams() {}
 
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
-    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
+    namespace bs = ::boost::serialization;
-    ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
-    ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
+    ar & bs::make_nvp("biasAccCovariance",
-    ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
+         bs::make_array(biasAccCovariance.data(), biasAccCovariance.size()));
+    ar & bs::make_nvp("biasOmegaCovariance",
+         bs::make_array(biasOmegaCovariance.data(), biasOmegaCovariance.size()));
+    ar & bs::make_nvp("biasAccOmegaInt", bs::make_array(biasAccOmegaInt.data(),
+                                                        biasAccOmegaInt.size()));
   }
 
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-
 /**
  * PreintegratedCombinedMeasurements integrates the IMU measurements
  * (rotation rates and accelerations) and the corresponding covariance matrix.
@@ -128,7 +143,6 @@ public:
    */
   Eigen::Matrix<double, 15, 15> preintMeasCov_;
 
-
   friend class CombinedImuFactor;
 
  public:
@@ -136,11 +150,14 @@ public:
   /// @{
 
   /// Default constructor only for serialization and Cython wrapper
-  PreintegratedCombinedMeasurements() {}
+  PreintegratedCombinedMeasurements() {
+    preintMeasCov_.setZero();
+  }
 
   /**
    *  Default constructor, initializes the class with no measurements
-   *  @param bias Current estimate of acceleration and rotation rate biases
+   *  @param p       Parameters, typically fixed in a single application
+   *  @param biasHat Current estimate of acceleration and rotation rate biases
    */
   PreintegratedCombinedMeasurements(
       const boost::shared_ptr<Params>& p,
@@ -149,6 +166,19 @@ public:
     preintMeasCov_.setZero();
   }
 
+  /**
+  *  Construct preintegrated directly from members: base class and preintMeasCov
+  *  @param base               PreintegrationType instance
+  *  @param preintMeasCov      Covariance matrix used in noise model.
+  */
+  PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
+     : PreintegrationType(base),
+       preintMeasCov_(preintMeasCov) {
+  }
+
+  /// Virtual destructor
+  virtual ~PreintegratedCombinedMeasurements() {}
+
   /// @}
 
   /// @name Basic utilities
@@ -158,20 +188,25 @@ public:
   void resetIntegration() override;
 
   /// const reference to params, shadows definition in base class
-  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
+  Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
   /// @}
 
   /// @name Access instance variables
   /// @{
+  /// Return pre-integrated measurement covariance
   Matrix preintMeasCov() const { return preintMeasCov_; }
   /// @}
 
   /// @name Testable
   /// @{
+  /// print
   void print(const std::string& s = "Preintegrated Measurements:") const override;
-  bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
+  /// equals
+  bool equals(const PreintegratedCombinedMeasurements& expected,
+              double tol = 1e-9) const;
   /// @}
 
+
   /// @name Main functionality
   /// @{
 
@@ -205,8 +240,10 @@ public:
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    namespace bs = ::boost::serialization;
     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
-    ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
+    ar& bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(),
+                                                      preintMeasCov_.size()));
   }
 
 public:
@@ -244,9 +281,6 @@ private:
 
   PreintegratedCombinedMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  CombinedImuFactor() {}
-
 public:
 
   /** Shorthand for a smart pointer to a factor */
@@ -256,6 +290,9 @@ public:
   typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
 #endif
 
+  /** Default constructor - only use for serialization */
+  CombinedImuFactor() {}
+
   /**
    * Constructor
    * @param pose_i Previous pose key
@@ -277,12 +314,17 @@ public:
 
   /** implement functions needed for Testable */
 
+  /// @name Testable
+  /// @{
+  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
+                                               const CombinedImuFactor&);
   /// print
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
 
   /// equals
   virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  /// @}
 
   /** Access the preintegrated measurements. */
 
@@ -321,14 +363,12 @@ public:
 #endif
 
 private:
-
   /** Serialization function */
   friend class boost::serialization::access;
-  template<class ARCHIVE>
+  template <class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("NoiseModelFactor6",
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
-         boost::serialization::base_object<Base>(*this));
+    ar& BOOST_SERIALIZATION_NVP(_PIM_);
-    ar & BOOST_SERIALIZATION_NVP(_PIM_);
   }
 
 public:
@@ -336,4 +376,19 @@ public:
 };
 // class CombinedImuFactor
 
-} /// namespace gtsam
+template <>
+struct traits<PreintegrationCombinedParams>
+    : public Testable<PreintegrationCombinedParams> {};
+
+template <>
+struct traits<PreintegratedCombinedMeasurements>
+    : public Testable<PreintegratedCombinedMeasurements> {};
+
+template <>
+struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
+
+}  // namespace gtsam
+
+/// Add Boost serialization export for derived class
+BOOST_CLASS_EXPORT_GUID(gtsam::PreintegrationCombinedParams, "gtsam_PreintegrationCombinedParams");
+// BOOST_CLASS_EXPORT_GUID(gtsam::CombinedImuFactor, "gtsam_CombinedImuFactor");