diff --git a/gtsam.h b/gtsam.h index c5424ef80..03963c773 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2489,10 +2489,30 @@ class NavState { gtsam::Pose3 pose() const; }; +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; + #include -class PreintegrationParams { +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); - // TODO(frank): add setters/getters or make this MATLAB wrapper feature + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; }; #include diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b170ea863..88d9c6437 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -26,6 +26,38 @@ namespace gtsam { +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegratedRotationParams { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + + virtual void print(const std::string& s) const; + virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; + + void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; } + void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } + void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } + + const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; } + boost::optional getOmegaCoriolis() const { return omegaCoriolis; } + boost::optional getBodyPSensor() const { return body_P_sensor; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } +}; + /** * PreintegratedRotation is the base class for all PreintegratedMeasurements * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). @@ -33,29 +65,7 @@ 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 { - Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - - Params() : gyroscopeCovariance(I_3x3) {} - - virtual void print(const std::string& s) const; - virtual bool equals(const Params& other, double tol=1e-9) const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - } - }; + typedef PreintegratedRotationParams Params; protected: /// Parameters diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f68f711f1..f2b2c0fef 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -23,7 +23,7 @@ namespace gtsam { /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor -struct PreintegrationParams: PreintegratedRotation::Params { +struct PreintegrationParams: PreintegratedRotationParams { Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration @@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params { void print(const std::string& s) const; bool equals(const PreintegratedRotation::Params& other, double tol) const; + void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } + void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } + void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } + + const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } + const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } + protected: /// Default constructor for serialization only: uninitialized! PreintegrationParams() {} @@ -60,7 +68,7 @@ protected: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);