diff --git a/gtsam.h b/gtsam.h index e500edbd9..708753749 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,33 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include +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..ca9b2ca1a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -54,6 +54,56 @@ typedef ManifoldPreintegration PreintegrationType; * 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 + 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 +}; + + /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. @@ -67,47 +117,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