Major updates to CombinedImuFactor to make it Testable as well as serializable
							parent
							
								
									cc2456678f
								
							
						
					
					
						commit
						95b4a49f64
					
				|  | @ -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( | ||||
|  |  | |||
|  | @ -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) : | ||||
|   PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( | ||||
|     I_3x3), biasAccOmegaInt(I_6x6) { | ||||
|   PreintegrationCombinedParams(const Vector3& n_gravity) : | ||||
|     PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), | ||||
|     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*/) { | ||||
|     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); | ||||
|     ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); | ||||
|     ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); | ||||
|     ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt); | ||||
|   void serialize(ARCHIVE& ar, const unsigned int /*version*/) { | ||||
|     namespace bs = ::boost::serialization; | ||||
|     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams); | ||||
|     ar & bs::make_nvp("biasAccCovariance", | ||||
|          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> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int /*version*/) { | ||||
|     ar & boost::serialization::make_nvp("NoiseModelFactor6", | ||||
|          boost::serialization::base_object<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(_PIM_); | ||||
|   template <class ARCHIVE> | ||||
|   void serialize(ARCHIVE& ar, const unsigned int /*version*/) { | ||||
|     ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6); | ||||
|     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");
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue