From 730199b67819aa32dc390085c1b2bd86dea9e182 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 10:55:02 -0400 Subject: [PATCH 1/3] Fix python wrappers for BetweenFactorVector and PreintegratedCombinedMeasurements and added Vector type to the BetweenFactor generation Created PreintegrationCombinedParams to add constructor for PreintegratedCombinedMeasurements --- gtsam.h | 29 +++++++- gtsam/navigation/CombinedImuFactor.h | 103 +++++++++++++++------------ 2 files changed, 84 insertions(+), 48 deletions(-) diff --git a/gtsam.h b/gtsam.h index e500edbd9..9ab4f1d99 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2405,7 +2405,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2812,7 +2812,34 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include +//typedef PreintegratedCombinedMeasurements::Params PreintegrationCombinedParams; +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); + + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; + class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s) const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 2ad71cb3c..d37d5fe84 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -29,12 +29,6 @@ namespace gtsam { -#ifdef GTSAM_TANGENT_PREINTEGRATION -typedef TangentPreintegration PreintegrationType; -#else -typedef ManifoldPreintegration PreintegrationType; -#endif - /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -54,6 +48,61 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ +struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { + Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration + + /// 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) { + } + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); + } + + void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } + void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } + void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; } + + const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } + const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; } + const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; } + +private: + /// Default constructor makes unitialized params struct + PreintegrationCombinedParams() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + 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); + } + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + + /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. @@ -67,47 +116,7 @@ typedef ManifoldPreintegration PreintegrationType; class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { public: - - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params : PreintegrationParams { - Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk - Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration - - /// See two named constructors below for good values of n_gravity in body frame - Params(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 - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::shared_ptr(new Params(Vector3(0, 0, g))); - } - - // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::shared_ptr(new Params(Vector3(0, 0, -g))); - } - - private: - /// Default constructor makes unitialized params struct - Params() {} - - /** Serialization function */ - friend class boost::serialization::access; - template - 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); - } - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; + typedef PreintegrationCombinedParams Params; protected: /* Covariance matrix of the preintegrated measurements From f58a5f89f22fba829d5c13649d7912ca7ff20a51 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 14:15:19 -0400 Subject: [PATCH 2/3] clean up commented code --- gtsam.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 9ab4f1d99..708753749 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2812,7 +2812,6 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include -//typedef PreintegratedCombinedMeasurements::Params PreintegrationCombinedParams; virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(Vector n_gravity); From ca02a03c08f0076fb0f4ea2d706de7efd094de46 Mon Sep 17 00:00:00 2001 From: Jean-Philippe Tardif Date: Fri, 18 Oct 2019 14:16:07 -0400 Subject: [PATCH 3/3] Move back code,, comments in the code --- gtsam/navigation/CombinedImuFactor.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d37d5fe84..ca9b2ca1a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -29,6 +29,12 @@ namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -48,6 +54,8 @@ namespace gtsam { * Robotics: Science and Systems (RSS), 2015. */ +/// Parameters for pre-integration using PreintegratedCombinedMeasurements: +/// Usage: Create just a single Params and pass a shared pointer to the constructor struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk @@ -95,13 +103,6 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - -#ifdef GTSAM_TANGENT_PREINTEGRATION -typedef TangentPreintegration PreintegrationType; -#else -typedef ManifoldPreintegration PreintegrationType; -#endif - /** * PreintegratedCombinedMeasurements integrates the IMU measurements