From a69c43bf43c0e38ff28deffe5df7bebcc58ba8d0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 10 Oct 2015 13:51:39 -0700 Subject: [PATCH] matchesParamsWith, a few new constructors, and Doxygen streamlining --- gtsam/navigation/CombinedImuFactor.h | 39 ++++++-- gtsam/navigation/ImuFactor.h | 22 ++++- gtsam/navigation/PreintegratedRotation.cpp | 24 +++-- gtsam/navigation/PreintegratedRotation.h | 108 +++++++++++++-------- gtsam/navigation/PreintegrationBase.cpp | 3 +- gtsam/navigation/PreintegrationBase.h | 76 ++++++++++++--- 6 files changed, 192 insertions(+), 80 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 737275759..691fae5b9 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -113,6 +113,9 @@ public: friend class CombinedImuFactor; public: + /// @name Constructors + /// @{ + /** * Default constructor, initializes the class with no measurements * @param bias Current estimate of acceleration and rotation rate biases @@ -123,18 +126,32 @@ public: preintMeasCov_.setZero(); } - Params& p() const { return *boost::static_pointer_cast(p_);} + /// @} - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const PreintegratedCombinedMeasurements& expected, - double tol = 1e-9) const; + /// @name Basic utilities + /// @{ /// Re-initialize PreintegratedCombinedMeasurements void resetIntegration(); + /// const reference to params, shadows definition in base class + Params& p() const { return *boost::static_pointer_cast(p_);} + /// @} + + /// @name Access instance variables + /// @{ + Matrix preintMeasCov() const { return preintMeasCov_; } + /// @} + + /// @name Testable + /// @{ + void print(const std::string& s = "Preintegrated Measurements:") const; + bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; + /// @} + + /// @name Main functionality + /// @{ + /** * Add a single IMU measurement to the preintegration. * @param measuredAcc Measured acceleration (in body frame, as given by the @@ -147,8 +164,10 @@ public: void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT); - /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_; } + /// @} + + /// @name Deprecated + /// @{ /// deprecated constructor /// NOTE(frank): assumes Z-Down convention, only second order integration supported @@ -159,6 +178,8 @@ public: const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); + /// @} + private: /// Serialization function friend class boost::serialization::access; diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 855c14365..d47b5d740 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -81,11 +81,21 @@ public: * @param p Parameters, typically fixed in a single application */ PreintegratedImuMeasurements(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) : + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationBase(p, biasHat) { preintMeasCov_.setZero(); } +/** + * Construct preintegrated directly from members: base class and preintMeasCov + * @param base PreintegrationBase instance + * @param preintMeasCov Covariance matrix used in noise model. + */ + PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) + : PreintegrationBase(base), + preintMeasCov_(preintMeasCov) { + } + /// print void print(const std::string& s = "Preintegrated Measurements:") const; @@ -167,7 +177,7 @@ public: #endif /** Default constructor - only use for serialization */ - ImuFactor(); + ImuFactor() {} /** * Constructor @@ -241,4 +251,10 @@ private: }; // class ImuFactor +template <> +struct traits : public Testable {}; + +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 708d1a3f6..9d623bf38 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -42,14 +42,15 @@ void PreintegratedRotation::resetIntegration() { } void PreintegratedRotation::print(const string& s) const { - cout << s << endl; + cout << s; cout << " deltaTij [" << deltaTij_ << "]" << endl; cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; } bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { - return deltaRij_.equals(other.deltaRij_, tol) + return this->matchesParamsWith(other) + && deltaRij_.equals(other.deltaRij_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } @@ -75,12 +76,16 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! } -void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, - Matrix3* F) { +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega); - const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + // If asked, pass first derivative as well + if (optional_D_incrR_integratedOmega) { + *optional_D_incrR_integratedOmega << D_incrR_integratedOmega; + } // Update deltaTij and rotation deltaTij_ += deltaT; @@ -88,8 +93,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - *D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 002afea76..7fb734a91 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -32,56 +32,74 @@ namespace gtsam { * It includes the definitions of the preintegrated rotation. */ class PreintegratedRotation { -public: - + 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 + 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) { - } + Params() : gyroscopeCovariance(I_3x3) {} virtual void print(const std::string& s) const; - private: + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + ar& BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar& BOOST_SERIALIZATION_NVP(body_P_sensor); } }; -protected: - double deltaTij_; ///< Time interval from i to j - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - + protected: /// Parameters boost::shared_ptr p_; - /// Default constructor for serialization - PreintegratedRotation() { - } + double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + /// Default constructor for serialization + PreintegratedRotation() {} + + public: + /// @name Constructors + /// @{ -public: /// Default constructor, resets integration to zero - explicit PreintegratedRotation(const boost::shared_ptr& p) : - p_(p) { + explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { resetIntegration(); } + /// Explicit initialization of all class members + PreintegratedRotation(const boost::shared_ptr& p, + double deltaTij, const Rot3& deltaRij, + const Matrix3& delRdelBiasOmega) + : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {} + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements void resetIntegration(); + /// check parameters equality: checks whether shared pointer points to same Params object. + bool matchesParamsWith(const PreintegratedRotation& other) const { + return p_ == other.p_; + } + /// @} + /// @name Access instance variables /// @{ + const boost::shared_ptr& params() const { + return p_; + } const double& deltaTij() const { return deltaTij_; } @@ -95,41 +113,47 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const; bool equals(const PreintegratedRotation& other, double tol) const; - /// @} + /// @name Main functionality + /// @{ + /// Take the gyro measurement, correct it using the (constant) bias estimate /// and possibly the sensor pose, and then integrate it forward in time to yield /// an incremental rotation. - Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, - double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const; /// Calculate an incremental rotation given the gyro measurement and a time interval, /// and update both deltaTij_ and deltaRij_. - void integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, - Matrix3* F); + void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none, + OptionalJacobian<3, 3> F = boost::none); /// Return a bias corrected version of the integrated rotation, with optional Jacobian Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = boost::none) const; + OptionalJacobian<3, 3> H = boost::none) const; /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i) const; -private: + /// @} + + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT + ar& BOOST_SERIALIZATION_NVP(p_); + ar& BOOST_SERIALIZATION_NVP(deltaTij_); + ar& BOOST_SERIALIZATION_NVP(deltaRij_); + ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; -} // namespace gtsam +template <> +struct traits : public Testable {}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index bb01971e6..65bc25060 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -64,7 +64,8 @@ void PreintegrationBase::print(const string& s) const { //------------------------------------------------------------------------------ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return fabs(deltaTij_ - other.deltaTij_) < tol + return this->matchesParamsWith(other) + && fabs(deltaTij_ - other.deltaTij_) < tol && deltaXij_.equals(other.deltaXij_, tol) && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 07225dd9a..856ba54cb 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -100,7 +100,14 @@ public: protected: - double deltaTij_; ///< Time interval from i to j + /// Parameters + boost::shared_ptr p_; + + /// Acceleration and gyro bias used for preintegration + imuBias::ConstantBias biasHat_; + + /// Time interval from i to j + double deltaTij_; /** * Preintegrated navigation state, from frame i to frame j @@ -109,12 +116,6 @@ protected: */ NavState deltaXij_; - /// Parameters - boost::shared_ptr p_; - - /// Acceleration and gyro bias used for preintegration - imuBias::ConstantBias biasHat_; - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias @@ -127,20 +128,53 @@ protected: public: + /// @name Constructors + /// @{ + /** * Constructor, initializes the variables in the base class - * @param bias Current estimate of acceleration and rotation rate biases * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases */ PreintegrationBase(const boost::shared_ptr& p, - const imuBias::ConstantBias& biasHat) : + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : p_(p), biasHat_(biasHat) { resetIntegration(); } + /** + * Constructor which takes in all members for generic construction + */ + PreintegrationBase(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat, + double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc, + const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc, + const Matrix3& delVdelBiasOmega) + : p_(p), + biasHat_(biasHat), + deltaTij_(deltaTij), + deltaXij_(deltaXij), + delPdelBiasAcc_(delPdelBiasAcc), + delPdelBiasOmega_(delPdelBiasOmega), + delVdelBiasAcc_(delVdelBiasAcc), + delVdelBiasOmega_(delVdelBiasOmega) {} + /// @} + + /// @name Basic utilities + /// @{ /// Re-initialize PreintegratedMeasurements void resetIntegration(); + /// check parameters equality: checks whether shared pointer points to same Params object. + bool matchesParamsWith(const PreintegrationBase& other) const { + return p_ == other.p_; + } + + /// shared pointer to params + const boost::shared_ptr& params() const { + return p_; + } + + /// const reference to params const Params& p() const { return *boost::static_pointer_cast(p_); } @@ -148,8 +182,10 @@ public: void set_body_P_sensor(const Pose3& body_P_sensor) { p_->body_P_sensor = body_P_sensor; } + /// @} - /// getters + /// @name Instance variables access + /// @{ const NavState& deltaXij() const { return deltaXij_; } @@ -183,17 +219,20 @@ public: const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } - // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } + /// @} - /// print + /// @name Testable + /// @{ void print(const std::string& s) const; - - /// check equality bool equals(const PreintegrationBase& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ /// Subtract estimate and correct for sensor pose /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) @@ -236,11 +275,18 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; + /// @} + + /// @name Deprecated + /// @{ + /// @deprecated predict PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + /// @} + private: /** Serialization function */ friend class boost::serialization::access;