From 6b56b609f223d812ac42b76067efc4d16efee70a Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Dec 2014 18:28:15 -0500 Subject: [PATCH 01/65] added base class for preintegration in Imu factors --- gtsam/navigation/CombinedImuFactor.cpp | 37 +--- gtsam/navigation/CombinedImuFactor.h | 66 +------ gtsam/navigation/ImuFactor.cpp | 37 +--- gtsam/navigation/ImuFactor.h | 263 ++++++++++--------------- gtsam/navigation/PreintegrationBase.h | 160 +++++++++++++++ 5 files changed, 285 insertions(+), 278 deletions(-) create mode 100644 gtsam/navigation/PreintegrationBase.h diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2ce631ea..51151102b 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -36,11 +36,7 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, use2ndOrderIntegration) { measurementCovariance_.setZero(); measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; @@ -54,42 +50,21 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); + PreintegrationBase::print(s); cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; + PreintegrationBase::resetIntegration(); PreintMeasCov_.setZero(); } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5e2bfeadb..6536a49a8 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,7 +23,7 @@ /* GTSAM includes */ #include -#include +#include #include namespace gtsam { @@ -78,33 +78,20 @@ public: * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received * from the IMU) so as to avoid costly integration at time of factor construction. */ - class CombinedPreintegratedMeasurements { + class CombinedPreintegratedMeasurements: public PreintegrationBase { friend class CombinedImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases - bool use2ndOrderIntegration_; ///< Controls the order of integration - public: /** @@ -145,56 +132,17 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} Matrix PreintMeasCov() const { return PreintMeasCov_;} - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - private: - /** Serialization function */ + + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9a9cc9d62..446686c39 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -35,11 +35,7 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, use2ndOrderIntegration) { measurementCovariance_.setZero(); measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; @@ -50,42 +46,21 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); + PreintegrationBase::print(s); cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; + PreintegrationBase::resetIntegration(); PreintMeasCov_.setZero(); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2af634926..1d4b027fa 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,7 +23,7 @@ /* GTSAM includes */ #include -#include +#include #include namespace gtsam { @@ -73,31 +73,19 @@ public: * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received * from the IMU) so as to avoid costly integration at time of factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements: public PreintegrationBase { friend class ImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of + ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). - bool use2ndOrderIntegration_; ///< Controls the order of integration - - public: + public: /** * Default constructor, initializes the class with no measurements @@ -133,154 +121,115 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} Matrix preintMeasCov() const { return PreintMeasCov_;} - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; - private: - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - - /** Default constructor - only use for serialization */ - ImuFactor(); - - /** - * Constructor - * @param pose_i Previous pose key - * @param vel_i Previous velocity key - * @param pose_j Current pose key - * @param vel_j Current velocity key - * @param bias Previous bias key - * @param preintegratedMeasurements Preintegrated IMU measurements - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect - */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); - - virtual ~ImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; - - /** implement functions needed for Testable */ - - /// 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. */ - - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; - - /// predicted states from IMU - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - - private: - - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); + ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); } - }; // class ImuFactor + }; - typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; + private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + + public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + ImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// 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. */ + + const PreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const; + + /// predicted states from IMU + static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // class ImuFactor + +typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h new file mode 100644 index 000000000..f83da595d --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +/* GTSAM includes */ +#include + +namespace gtsam { + + +/** + * PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor.h) and + * CombinedPreintegratedMeasurements (in CombinedImuFactor.h). It includes the definitions of the + * preintegrated variables and the methods to access, print, and compare them. + */ +class PreintegrationBase { + + friend class ImuFactor; + friend class CombinedImuFactor; + +protected: + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + +public: + + /** + * Default constructor, initializes the variables in the base class + * @param bias Current estimate of acceleration and rotation rate biases + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : + biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), + deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + deltaRij_(Rot3()), deltaTij_(0.0), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3) { } + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + biasHat_.print(" biasHat"); + std::cout << " deltaTij " << deltaTij_ << std::endl; + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + deltaRij_.print(" deltaRij "); + } + + /// Needed for testable + bool equals(const PreintegrationBase& expected, double tol) const { + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; + } + + /// methods to access class variables + Matrix deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const{return deltaTij_;} + Vector deltaPij() const {return deltaPij_;} + Vector deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} + Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} + Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} + Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} + Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} + Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } + + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam From c4b62929bfce968b778212fb25d118a45c705638 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Dec 2014 18:42:44 -0500 Subject: [PATCH 02/65] fixed potential bug when IMU-to-body trasformation is not the identity --- gtsam/navigation/CombinedImuFactor.cpp | 5 ++--- gtsam/navigation/ImuFactor.cpp | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 51151102b..256a9aae6 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -101,9 +101,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( }else{ delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; @@ -177,7 +176,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( if(!use2ndOrderIntegration_){ deltaPij_ += deltaVij_ * deltaT; }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; } deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; deltaRij_ = deltaRij_ * Rincr; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 446686c39..288e53140 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -87,7 +87,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians @@ -98,7 +97,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( }else{ delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; } delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; @@ -157,7 +156,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( if(!use2ndOrderIntegration_){ deltaPij_ += deltaVij_ * deltaT; }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; } deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; deltaRij_ = deltaRij_ * Rincr; From 218af7c88963c5bd03b70a1f8c83fbb034cf916c Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Dec 2014 18:58:20 -0500 Subject: [PATCH 03/65] included methods in the base class to reduce redundancy between ImuFactor and CombinedImuFactor --- gtsam/navigation/CombinedImuFactor.cpp | 35 +++------------------ gtsam/navigation/ImuFactor.cpp | 35 +++------------------ gtsam/navigation/PreintegrationBase.h | 42 ++++++++++++++++++++++++++ 3 files changed, 50 insertions(+), 62 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 256a9aae6..7e011b697 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -76,18 +76,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement @@ -95,17 +85,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we @@ -173,14 +153,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 288e53140..9fc7559f9 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -72,18 +72,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // NOTE: order is important here because each update uses old values (i.e., we have to update // jacobians and covariances before updating preintegrated measurements). - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement @@ -91,17 +81,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); // Update preintegrated measurements covariance // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework @@ -153,14 +133,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f83da595d..7708981bd 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -105,6 +105,48 @@ public: delRdelBiasOmega_ = Z_3x3; } + /// Update preintegrated measurements + void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& Rincr, double deltaT){ + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; + } + deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; + } + + /// Update Jacobians to be used during preintegration + void updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ + if(!use2ndOrderIntegration_){ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + }else{ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() + * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; + } + delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; + delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; + delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + } + + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, const Vector3& measuredOmega, + Vector3& correctedAcc, Vector3& correctedOmega, boost::optional body_P_sensor){ + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + } + /// methods to access class variables Matrix deltaRij() const {return deltaRij_.matrix();} double deltaTij() const{return deltaTij_;} From 7dd359ce5ddada579ee4af05083d024380aa03a6 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Dec 2014 19:09:04 -0500 Subject: [PATCH 04/65] fixed naming convention --- gtsam/navigation/CombinedImuFactor.cpp | 12 ++++++------ gtsam/navigation/CombinedImuFactor.h | 6 +++--- gtsam/navigation/ImuFactor.cpp | 14 +++++++------- gtsam/navigation/ImuFactor.h | 6 +++--- 4 files changed, 19 insertions(+), 19 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 7e011b697..0c5b89d9f 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -45,27 +45,27 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ PreintegrationBase::print(s); cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; + cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ PreintegrationBase::resetIntegration(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -149,7 +149,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6536a49a8..856929c41 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -87,7 +87,7 @@ public: Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases @@ -132,7 +132,7 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} + Matrix PreintMeasCov() const { return preintMeasCov_;} private: @@ -142,7 +142,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9fc7559f9..d29620e97 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -41,27 +41,27 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - PreintMeasCov_.setZero(9,9); + preintMeasCov_.setZero(9,9); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; + cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ PreintegrationBase::resetIntegration(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -119,7 +119,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // the deltaT allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // This in only kept for documentation. @@ -129,7 +129,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -149,7 +149,7 @@ ImuFactor::ImuFactor( const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 1d4b027fa..78291450e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -82,7 +82,7 @@ public: Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). public: @@ -121,7 +121,7 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} + Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -131,7 +131,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; From b9e96e4c6fdb9c406a1d32176692939f76f2f910 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Dec 2014 19:35:21 -0500 Subject: [PATCH 05/65] added ImuBase class --- gtsam/navigation/CombinedImuFactor.cpp | 10 +++------- gtsam/navigation/CombinedImuFactor.h | 11 +---------- gtsam/navigation/ImuFactor.cpp | 10 +++------- gtsam/navigation/ImuFactor.h | 11 +---------- gtsam/navigation/PreintegrationBase.h | 27 +++++++++++++++++++++++++- 5 files changed, 34 insertions(+), 35 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 0c5b89d9f..45f067fcf 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, @@ -168,12 +168,8 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ -} + ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 856929c41..fb481c8d6 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -70,7 +70,7 @@ struct PoseVelocityBias { * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty * and the preintegrated measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6 { +class CombinedImuFactor: public NoiseModelFactor6, public ImuBase{ public: /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) @@ -152,11 +152,6 @@ private: typedef NoiseModelFactor6 Base; CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect public: @@ -207,10 +202,6 @@ public: const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - /** implement functions needed to derive from Factor */ /// vector of errors diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index d29620e97..9f7021c3a 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} + ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -150,12 +150,8 @@ ImuFactor::ImuFactor( boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ -} + ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 78291450e..429db33ec 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -63,7 +63,7 @@ struct PoseVelocity { * Note that this factor does not force "temporal consistency" of the biases (which are usually * slowly varying quantities), see also CombinedImuFactor for more details. */ -class ImuFactor: public NoiseModelFactor5 { +class ImuFactor: public NoiseModelFactor5, public ImuBase { public: /** @@ -141,11 +141,6 @@ public: typedef NoiseModelFactor5 Base; PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect public: @@ -195,10 +190,6 @@ public: const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - /** implement functions needed to derive from Factor */ /// vector of errors diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 7708981bd..edc391498 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -66,7 +66,7 @@ public: deltaRij_(Rot3()), deltaTij_(0.0), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3) { } + delRdelBiasOmega_(Z_3x3) {} /// Needed for testable void print(const std::string& s) const { @@ -199,4 +199,29 @@ private: } }; +class ImuBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + ImuBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + +}; + } /// namespace gtsam From 1992cb27baf47b31d3c50aef4d9ff27f19b20e74 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 11:51:31 -0500 Subject: [PATCH 06/65] added ImuFactorBase. In the process of moving stuff to base class --- gtsam/navigation/CombinedImuFactor.cpp | 18 +++---- gtsam/navigation/CombinedImuFactor.h | 5 +- gtsam/navigation/ImuFactor.cpp | 29 +++++----- gtsam/navigation/ImuFactor.h | 16 +++--- gtsam/navigation/ImuFactorBase.h | 74 ++++++++++++++++++++++++++ gtsam/navigation/PreintegrationBase.h | 10 ++-- 6 files changed, 110 insertions(+), 42 deletions(-) create mode 100644 gtsam/navigation/ImuFactorBase.h diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 45f067fcf..3cb4c8588 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, @@ -168,7 +168,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ @@ -186,12 +186,9 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; + ImuFactorBase::print(""); preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ @@ -199,9 +196,7 @@ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) cons const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -393,7 +388,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ //------------------------------------------------------------------------------ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ const double& deltaTij = preintegratedMeasurements.deltaTij_; @@ -433,8 +428,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant } } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index fb481c8d6..23e1a0c5d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include namespace gtsam { @@ -70,7 +71,7 @@ struct PoseVelocityBias { * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty * and the preintegrated measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuBase{ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) @@ -217,7 +218,7 @@ public: /// predicted states from IMU static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9f7021c3a..824b850e8 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -41,7 +41,7 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - preintMeasCov_.setZero(9,9); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -150,7 +150,7 @@ ImuFactor::ImuFactor( boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ @@ -167,12 +167,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; + ImuFactorBase::print(""); preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ @@ -180,9 +177,7 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -333,14 +328,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const } //------------------------------------------------------------------------------ -PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) -{ +PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const PreintegrationBase& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); const Rot3 Rot_i = pose_i.rotation(); const Vector3 pos_i = pose_i.translation().vector(); @@ -375,7 +370,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 429db33ec..5307e94a3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include namespace gtsam { @@ -47,11 +48,14 @@ namespace gtsam { /** * Struct to hold return variables by the Predict Function */ -struct PoseVelocity { +struct PoseVelocityBias { Pose3 pose; Vector3 velocity; - PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { } }; @@ -63,7 +67,7 @@ struct PoseVelocity { * Note that this factor does not force "temporal consistency" of the biases (which are usually * slowly varying quantities), see also CombinedImuFactor for more details. */ -class ImuFactor: public NoiseModelFactor5, public ImuBase { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** @@ -202,8 +206,8 @@ public: boost::optional H5 = boost::none) const; /// predicted states from IMU - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h new file mode 100644 index 000000000..f3e715f92 --- /dev/null +++ b/gtsam/navigation/ImuFactorBase.h @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +/* GTSAM includes */ +#include + +namespace gtsam { + +class ImuFactorBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + ImuFactorBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /// Needed for testable + //------------------------------------------------------------------------------ + void print(const std::string& s) const { + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /// Needed for testable + //------------------------------------------------------------------------------ + bool equals(const ImuFactorBase& expected, double tol) const { + return equal_with_abs_tol(gravity_, expected.gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) || + (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); + } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index edc391498..dc824a9d8 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -81,11 +81,11 @@ public: /// Needed for testable bool equals(const PreintegrationBase& expected, double tol) const { return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) From 7a9a8dd9d6e4224afbd767b1fb1290bdc36447aa Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 12:09:13 -0500 Subject: [PATCH 07/65] moved prediction to base class --- gtsam/navigation/CombinedImuFactor.cpp | 46 ------------------ gtsam/navigation/CombinedImuFactor.h | 20 ++------ gtsam/navigation/ImuFactor.cpp | 46 ------------------ gtsam/navigation/ImuFactor.h | 20 ++------ gtsam/navigation/ImuFactorBase.h | 61 ++++++++++++++++++++++++ gtsam/navigation/PreintegrationBase.h | 1 + gtsam/navigation/tests/testImuFactor.cpp | 6 +-- 7 files changed, 72 insertions(+), 128 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 3cb4c8588..1c25a0d7e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -385,50 +385,4 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ return r; } -//------------------------------------------------------------------------------ -PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant -} - } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 23e1a0c5d..eac1e5702 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -45,20 +45,6 @@ namespace gtsam { * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. */ -/** - * Struct to hold all state variables of CombinedImuFactor returned by Predict function - */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - /** * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias * at previous time step), and current state (pose, velocity, bias at current time step). According to the @@ -133,7 +119,7 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix PreintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -219,7 +205,9 @@ public: static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ + return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); + } private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 824b850e8..430f9f4a4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -327,50 +327,4 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const return r; } -//------------------------------------------------------------------------------ -PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant -} - } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 5307e94a3..ff632eb8b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -45,20 +45,6 @@ namespace gtsam { * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. */ -/** - * Struct to hold return variables by the Predict Function - */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - /** * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), * current state (pose and velocity at current time step), and the bias estimate. According to the @@ -207,8 +193,10 @@ public: /// predicted states from IMU static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ + return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); + } private: diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index f3e715f92..c47f5f62b 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -26,6 +26,20 @@ namespace gtsam { +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } +}; + class ImuFactorBase { protected: @@ -69,6 +83,53 @@ public: (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); } + /// Predict state at time j + //------------------------------------------------------------------------------ + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const PreintegrationBase& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ + + const double& deltaTij = preintegratedMeasurements.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ + + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant + } + }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index dc824a9d8..55a793004 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -34,6 +34,7 @@ namespace gtsam { */ class PreintegrationBase { + friend class ImuFactorBase; friend class ImuFactor; friend class CombinedImuFactor; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index cf81c32ae..add5161f2 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -561,13 +561,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); - - } /* ************************************************************************* */ @@ -595,7 +593,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); From 7b43d5c9432125284f0bca51dd14296c9655c7f4 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 12:10:40 -0500 Subject: [PATCH 08/65] fixed naming convention --- gtsam/navigation/CombinedImuFactor.h | 4 ++-- gtsam/navigation/ImuFactor.h | 4 ++-- gtsam/navigation/ImuFactorBase.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index eac1e5702..d1b27d338 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -202,11 +202,11 @@ public: boost::optional H6 = boost::none) const; /// predicted states from IMU - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); + return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); } private: diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index ff632eb8b..98cc5e642 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -192,10 +192,10 @@ public: boost::optional H5 = boost::none) const; /// predicted states from IMU - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::Predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); + return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); } private: diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index c47f5f62b..a3a89d31c 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -85,7 +85,7 @@ public: /// Predict state at time j //------------------------------------------------------------------------------ - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ From 26ca8a5bb4a49c1f6b97735e57665136fa4a2951 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 12:13:08 -0500 Subject: [PATCH 09/65] fixed matlab wrapper --- gtsam.h | 92 +++++++++++++++++++++++++++------------------------------ 1 file changed, 44 insertions(+), 48 deletions(-) diff --git a/gtsam.h b/gtsam.h index 96d51117a..d7dfe8214 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2393,8 +2393,8 @@ class ConstantBias { }///\namespace imuBias #include -class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2419,7 +2419,6 @@ class ImuFactorPreintegratedMeasurements { Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); @@ -2433,54 +2432,12 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; }; -#include -class AHRSFactorPreintegratedMeasurements { - // Standard Constructor - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); - - // get Data - Matrix measurementCovariance() const; - Matrix deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - #include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); - }; class CombinedImuFactorPreintegratedMeasurements { // Standard Constructor CombinedImuFactorPreintegratedMeasurements( @@ -2521,7 +2478,7 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix delVdelBiasAcc() const; Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() const; + Matrix preintMeasCov() const; }; virtual class CombinedImuFactor : gtsam::NonlinearFactor { @@ -2530,11 +2487,50 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); }; +#include +class AHRSFactorPreintegratedMeasurements { + // Standard Constructor + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); + + // get Data + Matrix measurementCovariance() const; + Matrix deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + #include //virtual class AttitudeFactor : gtsam::NonlinearFactor { // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); From 9f7fbdc53038f2c6e2b8beaac87d8b34db9f67b0 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 12:14:37 -0500 Subject: [PATCH 10/65] fixed unit tests --- gtsam/navigation/tests/testCombinedImuFactor.cpp | 8 ++++---- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aab38f258..656fc1c6f 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -184,7 +184,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); @@ -284,13 +284,13 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); @@ -319,7 +319,7 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index add5161f2..f9faa8b99 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -561,7 +561,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); @@ -593,7 +593,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); From 95baccb3b44f854d8cca72e898ba7decd97be434 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 12:44:14 -0500 Subject: [PATCH 11/65] moved error and jacobian computation for ImuFactor to base class --- gtsam/navigation/ImuFactor.cpp | 144 +----------------------------- gtsam/navigation/ImuFactorBase.h | 146 +++++++++++++++++++++++++++++++ 2 files changed, 149 insertions(+), 141 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 430f9f4a4..ddd066489 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -182,149 +182,11 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { //------------------------------------------------------------------------------ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, + const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, - boost::optional H5) const -{ - - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3; - } - - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; + boost::optional H5) const{ + return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index a3a89d31c..66da084a1 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -23,6 +23,7 @@ /* GTSAM includes */ #include +#include namespace gtsam { @@ -83,6 +84,151 @@ public: (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); } + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + //------------------------------------------------------------------------------ + Vector computeErrorAndJacobians(const PreintegrationBase& preintegratedMeasurements_, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, boost::optional H5) const{ + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; + } + + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3; + } + + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3; + } + + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; + } + /// Predict state at time j //------------------------------------------------------------------------------ static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, From 30810e29173c1439bb6fc9a67e9516c66e240211 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 16:18:33 -0500 Subject: [PATCH 12/65] moved error and jacobian computation to base class --- gtsam/navigation/CombinedImuFactor.cpp | 227 ++++++------------------- gtsam/navigation/ImuFactor.cpp | 1 + gtsam/navigation/ImuFactorBase.h | 25 ++- 3 files changed, 68 insertions(+), 185 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c25a0d7e..e9df7a060 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -206,182 +206,65 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ boost::optional H3, boost::optional H4, boost::optional H5, boost::optional H6) const { - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3, - //dBiasAcc/dPi - Z_3x3, Z_3x3, - //dBiasOmega/dPi - Z_3x3, Z_3x3; - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3, - //dBiasAcc/dVi - Z_3x3, - //dBiasOmega/dVi - Z_3x3; - } - - if(H3) { - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3, - //dBiasAcc/dPosej - Z_3x3, Z_3x3, - //dBiasOmega/dPosej - Z_3x3, Z_3x3; - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3, - //dBiasAcc/dVj - Z_3x3, - //dBiasOmega/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -I_3x3, Z_3x3, - //dBiasOmega/dBias_i - Z_3x3, -I_3x3; - } - - if(H6) { - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Z_3x3, Z_3x3, - // dfV/dBias_j - Z_3x3, Z_3x3, - // dfR/dBias_j - Z_3x3, Z_3x3, - //dBiasAcc/dBias_j - I_3x3, Z_3x3, - //dBiasOmega/dBias_j - Z_3x3, I_3x3; - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - + // Bias evolution model: random walk const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 + // error wrt preintegrated position, velocity, rotation + Vector r_pvR(9); + // if we need the jacobians + if(H1 || H2 || H3 || H4 || H5 || H6){ + Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR; // pvR = mnemonic: position (p), velocity (v), rotation (R) + + // include errors wrt preintegrated measurements + r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); + + if(H1) { + H1->resize(15,6); + H1->block<9,6>(0,0) = H1_pvR; + // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] + H1->block<6,6>(0,9) = Matrix::Zero(6,6); + } + if(H2) { + H2->resize(15,3); + H2->block<9,3>(0,0) = H2_pvR; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H2->block<6,3>(0,9) = Matrix::Zero(6,3); + } + if(H3) { + H3->resize(15,6); + H3->block<9,6>(0,0) = H3_pvR; + // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] + H3->block<6,6>(0,9) = Matrix::Zero(6,6); + } + if(H4) { + H4->resize(15,3); + H4->block<9,3>(0,0) = H4_pvR; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H4->block<6,3>(0,9) = Matrix::Zero(6,3); + } + if(H5) { + H5->resize(15,6); + H5->block<9,6>(0,0) = H5_pvR; + // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] + H5->block<6,6>(0,9) = - Matrix::Identity(6,6); + } + if(H6) { + H6->resize(15,6); + H6->block<9,6>(0,0) = Matrix::Zero(6,6); + // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] + H6->block<6,6>(0,9) = Matrix::Identity(6,6); + } + Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 + return r; + } + // else, only compute the error vector: + // Evaluate residual error, including the model for bias evolution + r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + boost::none, boost::none, boost::none, boost::none, boost::none); + Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 return r; } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index ddd066489..29c85de21 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -186,6 +186,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const{ + return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 66da084a1..36fa3616d 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -138,18 +138,18 @@ public: (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; } if(H2) { @@ -191,7 +191,6 @@ public: const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - H5->resize(9,6); (*H5) << // dfP/dBias From c1d63b77ff89fa7bac3369074d9e785e7976b0b6 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 16:35:40 -0500 Subject: [PATCH 13/65] added comments, made more elegant error evaluation for CombinedImuFactor --- gtsam/navigation/CombinedImuFactor.cpp | 31 +++++++++++++------------- gtsam/navigation/ImuFactorBase.h | 20 +++++++++-------- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index e9df7a060..592c87bcc 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -206,21 +206,17 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ boost::optional H3, boost::optional H4, boost::optional H5, boost::optional H6) const { - // Bias evolution model: random walk - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - // error wrt preintegrated position, velocity, rotation - Vector r_pvR(9); - // if we need the jacobians if(H1 || H2 || H3 || H4 || H5 || H6){ - Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR; // pvR = mnemonic: position (p), velocity (v), rotation (R) + Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) - // include errors wrt preintegrated measurements - r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + // error wrt preintegrated measurements + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + if(H1) { H1->resize(15,6); H1->block<9,6>(0,0) = H1_pvR; @@ -249,22 +245,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ H5->resize(15,6); H5->block<9,6>(0,0) = H5_pvR; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block<6,6>(0,9) = - Matrix::Identity(6,6); + H5->block<6,6>(0,9) = Hbias_i; } if(H6) { H6->resize(15,6); H6->block<9,6>(0,0) = Matrix::Zero(6,6); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block<6,6>(0,9) = Matrix::Identity(6,6); + H6->block<6,6>(0,9) = Hbias_j; } - Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 + Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; } // else, only compute the error vector: - // Evaluate residual error, including the model for bias evolution - r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + // error wrt preintegrated measurements + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, boost::none, boost::none, boost::none, boost::none, boost::none); - Vector r(15); r << r_pvR, fbiasAcc, fbiasOmega; // vector of size 15 + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + // overall error + Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; } diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 36fa3616d..30c0f8c0e 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -52,15 +52,24 @@ protected: public: + /** Default constructor - only use for serialization */ ImuFactorBase() : gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + /** + * Default constructor, stores basic quantities required by the Imu factors + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + /// Methods to access class variables const Vector3& gravity() const { return gravity_; } const Vector3& omegaCoriolis() const { return omegaCoriolis_; } @@ -91,6 +100,7 @@ public: boost::optional H3, boost::optional H4, boost::optional H5) const{ const double& deltaTij = preintegratedMeasurements_.deltaTij_; + // We need the mistmatch w.r.t. the biases used for preintegration const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); @@ -100,7 +110,7 @@ public: const Vector3 pos_i = pose_i.translation().vector(); const Vector3 pos_j = pose_j.translation().vector(); - // We compute factor's Jacobians + // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) @@ -114,11 +124,8 @@ public: Rot3::Expmap( theta_biascorrected_corioliscorrected ); const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { @@ -134,7 +141,6 @@ public: dfPdPi = - Rot_i.matrix(); dfVdPi = Z_3x3; } - (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ @@ -151,7 +157,6 @@ public: // dfR/dPi Z_3x3; } - if(H2) { H2->resize(9,3); (*H2) << @@ -164,7 +169,6 @@ public: // dfR/dVi Z_3x3; } - if(H3) { H3->resize(9,6); (*H3) << @@ -175,7 +179,6 @@ public: // dfR/dPosej Jrinv_fRhat * ( I_3x3 ), Z_3x3; } - if(H4) { H4->resize(9,3); (*H4) << @@ -186,7 +189,6 @@ public: // dfR/dVj Z_3x3; } - if(H5) { const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); From 83d84bcc29d656cccc045a49b488bad32a2c0d22 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 4 Dec 2014 16:56:55 -0500 Subject: [PATCH 14/65] removed last redundancy between error computation and predict --- gtsam/navigation/ImuFactorBase.h | 31 +++++++++++-------------------- 1 file changed, 11 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 30c0f8c0e..c06331eeb 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -105,10 +105,9 @@ public: const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); + const Rot3& Rot_i = pose_i.rotation(); + const Rot3& Rot_j = pose_j.rotation(); + const Vector3& pos_j = pose_j.translation().vector(); // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ @@ -208,22 +207,14 @@ public: // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; + PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements_, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_); - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; + const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); + const Vector3 fv = vel_j - predictedState_j.velocity; + + // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) const Vector3 fR = Rot3::Logmap(fRhat); Vector r(9); r << fp, fv, fR; @@ -241,8 +232,8 @@ public: const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); + const Rot3& Rot_i = pose_i.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ From cc03a13f5f82b6d13d4c44e1a8354344e408b7c8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 09:28:40 +0100 Subject: [PATCH 15/65] check.navigation target --- .cproject | 114 +++++++++++++++++++++++++----------------------------- 1 file changed, 52 insertions(+), 62 deletions(-) diff --git a/.cproject b/.cproject index bcf690995..6e30db267 100644 --- a/.cproject +++ b/.cproject @@ -592,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -600,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -648,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -656,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -664,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -680,7 +675,6 @@ make - tests/testBayesTree true false @@ -1128,7 +1122,6 @@ make - testErrors.run true false @@ -1358,46 +1351,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1480,6 +1433,7 @@ make + testSimulated2DOriented.run true false @@ -1519,6 +1473,7 @@ make + testSimulated2D.run true false @@ -1526,6 +1481,7 @@ make + testSimulated3D.run true false @@ -1539,6 +1495,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1796,7 +1792,6 @@ cpack - -G DEB true false @@ -1804,7 +1799,6 @@ cpack - -G RPM true false @@ -1812,7 +1806,6 @@ cpack - -G TGZ true false @@ -1820,7 +1813,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1994,6 +1986,14 @@ true true + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + make -j2 @@ -2675,7 +2675,6 @@ make - testGraph.run true false @@ -2683,7 +2682,6 @@ make - testJunctionTree.run true false @@ -2691,7 +2689,6 @@ make - testSymbolicBayesNetB.run true false @@ -2801,14 +2798,6 @@ true true - - make - -j4 - testBasisDecompositions.run - true - true - true - make -j4 @@ -3235,6 +3224,7 @@ make + tests/testGaussianISAM2 true false From 37e6b796ec8eb87fb70c6c079fd62aebe7a2f899 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 09:29:00 +0100 Subject: [PATCH 16/65] Slightly edited and re-formatted comments --- gtsam/navigation/CombinedImuFactor.h | 55 ++++++++++++++++----------- gtsam/navigation/ImuFactor.h | 32 +++++++++------- gtsam/navigation/PreintegrationBase.h | 10 +++-- 3 files changed, 58 insertions(+), 39 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d1b27d338..98a248024 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -34,36 +34,47 @@ namespace gtsam { * @addtogroup SLAM * * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias - * at previous time step), and current state (pose, velocity, bias at current time step). According to the - * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are - * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: - * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). - * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices - * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. - * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. - * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty - * and the preintegrated measurements uncertainty. + * CombinedImuFactor is a 6-ways factor involving previous state (pose and + * velocity of the vehicle, as well as bias at previous time step), and current + * state (pose, velocity, bias at current time step). Following the pre- + * integration scheme proposed in [2], the CombinedImuFactor includes many IMU + * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * class. There are 3 main differences wrpt the ImuFactor class: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous + * and current time step).Therefore, the factor internally imposes the biases + * to be slowly varying; in particular, the matrices "biasAccCovariance" and + * "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias + * estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * the correlation between the bias uncertainty and the preintegrated + * measurements uncertainty. */ class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + /** + * CombinedPreintegratedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. */ class CombinedPreintegratedMeasurements: public PreintegrationBase { diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 98cc5e642..89344f1a1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -39,19 +39,24 @@ namespace gtsam { * Int. Conf. on Robotics and Automation (ICRA), 2014. * ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), - * current state (pose and velocity at current time step), and the bias estimate. According to the - * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are - * "summarized" using the PreintegratedMeasurements class. - * Note that this factor does not force "temporal consistency" of the biases (which are usually - * slowly varying quantities), see also CombinedImuFactor for more details. + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of + * the vehicle at previous time step), current state (pose and velocity at + * current time step), and the bias estimate. Following the preintegration + * scheme proposed in [2], the ImuFactor includes many IMU measurements, which + * are "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not model "temporal consistency" of the biases + * (which are usually slowly varying quantities), which is up to the caller. + * See also CombinedImuFactor for a class that does this for you. */ class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: @@ -59,9 +64,10 @@ public: /** * PreintegratedMeasurements accumulates (integrates) the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. */ class PreintegratedMeasurements: public PreintegrationBase { diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 55a793004..3ce3336df 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -28,9 +28,10 @@ namespace gtsam { /** - * PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor.h) and - * CombinedPreintegratedMeasurements (in CombinedImuFactor.h). It includes the definitions of the - * preintegrated variables and the methods to access, print, and compare them. + * PreintegrationBase is the base class for PreintegratedMeasurements + * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). + * It includes the definitions of the preintegrated variables and the methods + * to access, print, and compare them. */ class PreintegrationBase { @@ -119,7 +120,8 @@ public: } /// Update Jacobians to be used during preintegration - void updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ + void updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ if(!use2ndOrderIntegration_){ delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; From 507979c5262fee3970d8e7082f4bc4e3295aed10 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 12:12:54 +0100 Subject: [PATCH 17/65] PreintegratedRotation works for AHRS --- gtsam/navigation/AHRSFactor.cpp | 75 +++++--------- gtsam/navigation/AHRSFactor.h | 41 +++----- gtsam/navigation/PreintegratedRotation.h | 126 +++++++++++++++++++++++ 3 files changed, 165 insertions(+), 77 deletions(-) create mode 100644 gtsam/navigation/PreintegratedRotation.h diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..090b0c980 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -18,9 +18,9 @@ **/ #include +#include -/* External or standard includes */ -#include +using namespace std; namespace gtsam { @@ -29,47 +29,38 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias), deltaTij_(0.0) { + biasHat_(bias) { measurementCovariance_ << measuredOmegaCovariance; - delRdelBiasOmega_.setZero(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()), deltaTij_(0.0) { + biasHat_(Vector3()) { measurementCovariance_.setZero(); - delRdelBiasOmega_.setZero(); - delRdelBiasOmega_.setZero(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" - << std::endl; - std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; +void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegratedRotation::print(s); + cout << "biasHat [" << biasHat_.transpose() << "]" << endl; + cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return equal_with_abs_tol(biasHat_, other.biasHat_, tol) + return PreintegratedRotation::equals(other, tol) + && equal_with_abs_tol(biasHat_, other.biasHat_, tol) && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol) - && deltaRij_.equals(other.deltaRij_, tol) - && std::fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); + other.measurementCovariance_, tol); } //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_.setZero(); + PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } @@ -95,23 +86,23 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); + const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !! const Matrix3 incrRt = incrR.transpose(); // Right Jacobian computed at theta_incr const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); - // Update Jacobians - // --------------------------------------------------------------------------- - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + // Update Jacobian + update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); // Update preintegrated measurements covariance - // --------------------------------------------------------------------------- - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - Rot3 Rot_j = deltaRij_ * incrR; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) + // Update rotation and deltaTij. TODO Frank moved from end of this function !!! + updateIntegratedRotationAndDeltaT(incrR, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Update preintegrated measurements covariance: as in [2] we consider a first @@ -129,28 +120,13 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT; - // Update preintegrated measurements - // --------------------------------------------------------------------------- - deltaRij_ = deltaRij_ * incrR; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - if (H) { - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - } - return theta_biascorrected; + return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( @@ -192,13 +168,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { } //------------------------------------------------------------------------------ -void AHRSFactor::print(const std::string& s, +void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) body_P_sensor_->print(" sensor pose in body frame: "); diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e62a24cca..8b065dd7e 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,6 +20,7 @@ #pragma once /* GTSAM includes */ +#include #include #include @@ -35,7 +36,7 @@ public: * Can be built incrementally so as to avoid costly integration at time of * factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements : public PreintegratedRotation { friend class AHRSFactor; @@ -43,9 +44,6 @@ public: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -61,31 +59,22 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); + const Matrix3& measurementCovariance() const { + return measurementCovariance_; + } + Vector3 biasHat() const { + return biasHat_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; /// equals bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - double deltaTij() const { - return deltaTij_; - } - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } - /// TODO: Document void resetIntegration(); @@ -106,7 +95,7 @@ public: /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i, const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij_; + return rot_i.transpose() * omegaCoriolis * deltaTij(); } // This function is only used for test purposes @@ -120,11 +109,9 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h new file mode 100644 index 000000000..1b21978fb --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegratedRotation.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * PreintegratedRotation is the base class for all PreintegratedMeasurements + * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). + * It includes the definitions of the preintegrated rotation. + */ +class PreintegratedRotation { + + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + /// Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; + +public: + + /** + * Default constructor, initializes the variables in the base class + */ + PreintegratedRotation() : + deltaRij_(Rot3()), deltaTij_(0.0), + delRdelBiasOmega_(Z_3x3) {} + + /// methods to access class variables + Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive + Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive + const double& deltaTij() const{return deltaTij_;} + const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + deltaRij_.print(" deltaRij "); + } + + /// Needed for testable + bool equals(const PreintegratedRotation& expected, double tol) const { + return deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, double deltaT){ + deltaRij_ = deltaRij_ * incrR; + deltaTij_ += deltaT; + } + + /** + * Update Jacobians to be used during preintegration + * TODO: explain arguments + */ + void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR, + double deltaT) { + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + } + + /// Return a bias corrected version of the integrated rotation - expensive + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { + return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + } + + /// Get so<3> version of bias corrected rotation, with optional Jacobian + Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, + boost::optional H) const { + // First, we correct deltaRij using the biasOmegaIncr, rotated + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // This was done via an expmap, now we go *back* to so<3> + const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + if (H) { + // We then do a very expensive Jacobian calculation. TODO Right Duy ? + const Matrix3 Jrinv_theta_bc = // + Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = // + Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); + (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + } + return theta_biascorrected; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam From ac8e4d253629a4f6126536aa379bfa55defcaec3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 12:17:08 +0100 Subject: [PATCH 18/65] Made PreintegratedRotation a base class of PreintegrationBase --- gtsam/navigation/CombinedImuFactor.cpp | 16 ++--- gtsam/navigation/ImuFactor.cpp | 15 ++-- gtsam/navigation/ImuFactorBase.h | 23 +++--- gtsam/navigation/PreintegrationBase.h | 96 ++++++++++++-------------- 4 files changed, 70 insertions(+), 80 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 592c87bcc..b3e8d4b11 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -91,11 +91,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Single Jacobians to propagate covariance @@ -105,10 +107,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Matrix3 H_vel_pos = Z_3x3; Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix3 H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; + Matrix3 H_vel_biasacc = - deltaRij() * deltaT; Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_vel = Z_3x3; @@ -150,10 +152,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 29c85de21..640e02641 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -86,11 +86,14 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements covariance // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + // Even though Luca says has to happen after ? Don't understand why. + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); + + const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); Matrix H_pos_pos = I_3x3; @@ -99,7 +102,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Matrix H_vel_pos = Z_3x3; Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); @@ -130,10 +133,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index c06331eeb..7dbe6c9ec 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -99,7 +99,7 @@ public: const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const{ - const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const double& deltaTij = preintegratedMeasurements_.deltaTij(); // We need the mistmatch w.r.t. the biases used for preintegration const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); @@ -111,10 +111,11 @@ public: // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Vector3 theta_biascorrected = + preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term @@ -122,6 +123,7 @@ public: const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( theta_biascorrected_corioliscorrected ); + // TODO: these are not always needed const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); @@ -189,9 +191,8 @@ public: Z_3x3; } if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); H5->resize(9,6); (*H5) << // dfP/dBias @@ -228,7 +229,7 @@ public: const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - const double& deltaTij = preintegratedMeasurements.deltaTij_; + const double& deltaTij = preintegratedMeasurements.deltaTij(); const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); @@ -255,8 +256,10 @@ public: vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.biascorrectedDeltaRij(biasOmegaIncr); + // TODO Frank says comment below does not reflect what was in code // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3ce3336df..173228c61 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -21,19 +21,18 @@ #pragma once -/* GTSAM includes */ +#include #include namespace gtsam { - /** * PreintegrationBase is the base class for PreintegratedMeasurements * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase { +class PreintegrationBase : public PreintegratedRotation { friend class ImuFactorBase; friend class ImuFactor; @@ -45,14 +44,11 @@ protected: Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias public: @@ -65,82 +61,90 @@ public: PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3) {} + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {} + + /// methods to access class variables + const Vector3& deltaPij() const {return deltaPij_;} + const Vector3& deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} // TODO expensive + const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} + const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} + const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} + const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} /// Needed for testable void print(const std::string& s) const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; + PreintegratedRotation::print(s); std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); + biasHat_.print(" biasHat"); } /// Needed for testable bool equals(const PreintegrationBase& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) + return PreintegratedRotation::equals(expected, tol) + && biasHat_.equals(expected.biasHat_, tol) && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol); } /// Re-initialize PreintegratedMeasurements void resetIntegration(){ + PreintegratedRotation::resetIntegration(); deltaPij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; delPdelBiasAcc_ = Z_3x3; delPdelBiasOmega_ = Z_3x3; delVdelBiasAcc_ = Z_3x3; delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; } /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& Rincr, double deltaT){ + void updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, double deltaT) { + Matrix3 dRij = deltaRij(); // expensive + Vector3 temp = dRij * correctedAcc * deltaT; if(!use2ndOrderIntegration_){ deltaPij_ += deltaVij_ * deltaT; }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT; + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; + deltaVij_ += temp; + // TODO: we update rotation *after* the others. Is that correct? + updateIntegratedRotationAndDeltaT(incrR,deltaT); } /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& Jr_theta_incr, const Rot3& Rincr, double deltaT){ - if(!use2ndOrderIntegration_){ + const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){ + Matrix3 dRij = deltaRij(); // expensive + Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + // TODO: we update rotation *after* the others. Is that correct? + update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); } - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, const Vector3& measuredOmega, - Vector3& correctedAcc, Vector3& correctedOmega, boost::optional body_P_sensor){ + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedOmega = biasHat_.correctGyroscope(measuredOmega); - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame if(body_P_sensor){ Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame @@ -150,18 +154,6 @@ public: } } - /// methods to access class variables - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, @@ -189,16 +181,14 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; From 8bfe4d75fb7ad2893a370c14e6d3ba935773dc79 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 12:36:14 +0100 Subject: [PATCH 19/65] Possibly controversial (sorry @lucacarlone ) name change to make it easier to see copy/paste patterns. --- gtsam/navigation/AHRSFactor.cpp | 12 +++---- gtsam/navigation/AHRSFactor.h | 6 ++-- gtsam/navigation/CombinedImuFactor.cpp | 12 +++---- gtsam/navigation/CombinedImuFactor.h | 6 ++-- gtsam/navigation/ImuFactor.cpp | 10 +++--- gtsam/navigation/ImuFactor.h | 6 ++-- gtsam/navigation/ImuFactorBase.h | 50 +++++++++++++------------- gtsam/navigation/PreintegrationBase.h | 8 ++--- 8 files changed, 53 insertions(+), 57 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 090b0c980..7d6fdcc11 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -148,7 +148,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Matrix3::Zero()) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -156,7 +156,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const Vector3& omegaCoriolis, boost::optional body_P_sensor) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } @@ -172,7 +172,7 @@ void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) @@ -183,7 +183,7 @@ void AHRSFactor::print(const string& s, bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -197,11 +197,11 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, // Do bias correction, if (H3) will contain 3*3 derivative used below const Vector3 theta_biascorrected = // - preintegratedMeasurements_.predict(bias, H3); + _PIM_.predict(bias, H3); // Coriolis term const Vector3 coriolis = // - preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); + _PIM_.integrateCoriolis(rot_i, omegaCoriolis_); const Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 8b065dd7e..e3eb3d51a 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -119,7 +119,7 @@ private: typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; Vector3 gravity_; Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -165,7 +165,7 @@ public: /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -195,7 +195,7 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index b3e8d4b11..edd57d17f 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -158,7 +158,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, @@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - preintegratedMeasurements_(preintegratedMeasurements) {} + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -185,7 +185,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; ImuFactorBase::print(""); - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } @@ -193,7 +193,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && _PIM_.equals(e->_PIM_, tol) && ImuFactorBase::equals(*e, tol); } @@ -209,7 +209,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); // error wrt bias evolution model (random walk) @@ -256,7 +256,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } // else, only compute the error vector: // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, boost::none, boost::none, boost::none, boost::none, boost::none); // error wrt bias evolution model (random walk) Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 98a248024..03a0dd824 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -149,7 +149,7 @@ private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements preintegratedMeasurements_; + CombinedPreintegratedMeasurements _PIM_; public: @@ -198,7 +198,7 @@ public: /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -228,7 +228,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 640e02641..1b055bef2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -139,7 +139,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -150,7 +150,7 @@ ImuFactor::ImuFactor( const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - preintegratedMeasurements_(preintegratedMeasurements) {} + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -167,7 +167,7 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; ImuFactorBase::print(""); - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } @@ -175,7 +175,7 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && _PIM_.equals(e->_PIM_, tol) && ImuFactorBase::equals(*e, tol); } @@ -186,7 +186,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const boost::optional H3, boost::optional H4, boost::optional H5) const{ - return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); + return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 89344f1a1..01a245f8b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -136,7 +136,7 @@ public: typedef ImuFactor This; typedef NoiseModelFactor5 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; public: @@ -184,7 +184,7 @@ public: /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -212,7 +212,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 7dbe6c9ec..2d637d117 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -95,14 +95,14 @@ public: /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j //------------------------------------------------------------------------------ - Vector computeErrorAndJacobians(const PreintegrationBase& preintegratedMeasurements_, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const{ - const double& deltaTij = preintegratedMeasurements_.deltaTij(); + const double& deltaTij = _PIM.deltaTij(); // We need the mistmatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); // we give some shorter name to rotations and translations const Rot3& Rot_i = pose_i.rotation(); @@ -115,7 +115,7 @@ public: // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect Vector3 theta_biascorrected = - preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5); + _PIM.biascorrectedThetaRij(biasOmegaIncr, H5); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term @@ -144,13 +144,13 @@ public: } (*H1) << // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + Rot_i.matrix() * skewSymmetric(_PIM.deltaPij() + + _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr), // dfP/dPi dfPdPi, // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + Rot_i.matrix() * skewSymmetric(_PIM.deltaVij() + + _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr), // dfV/dPi dfVdPi, // dfR/dRi @@ -196,11 +196,11 @@ public: H5->resize(9,6); (*H5) << // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + - Rot_i.matrix() * _PIM.delPdelBiasAcc(), + - Rot_i.matrix() * _PIM.delPdelBiasOmega(), // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + - Rot_i.matrix() * _PIM.delVdelBiasAcc(), + - Rot_i.matrix() * _PIM.delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); @@ -208,7 +208,7 @@ public: // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements_, + PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM, gravity_, omegaCoriolis_, use2ndOrderCoriolis_); const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); @@ -226,28 +226,28 @@ public: //------------------------------------------------------------------------------ static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, + const PreintegrationBase& _PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - const double& deltaTij = preintegratedMeasurements.deltaTij(); - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + const double& deltaTij = _PIM.deltaTij(); + const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij() + + _PIM.delPdelBiasAcc() * biasAccIncr + + _PIM.delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij*deltaTij; - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij() + + _PIM.delVdelBiasAcc() * biasAccIncr + + _PIM.delVdelBiasOmega() * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + gravity * deltaTij); @@ -256,7 +256,7 @@ public: vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.biascorrectedDeltaRij(biasOmegaIncr); + const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr); // TODO Frank says comment below does not reflect what was in code // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 173228c61..fd5597aff 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -34,11 +34,6 @@ namespace gtsam { */ class PreintegrationBase : public PreintegratedRotation { - friend class ImuFactorBase; - friend class ImuFactor; - friend class CombinedImuFactor; - -protected: imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration bool use2ndOrderIntegration_; ///< Controls the order of integration @@ -67,7 +62,8 @@ public: /// methods to access class variables const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} // TODO expensive + const imuBias::ConstantBias& biasHat() const { return biasHat_;} + Vector biasHatVector() const { return biasHat_.vector();} // expensive const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} From aa93475b3d76b30cc6470c6530a22eda3f161037 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 13:24:03 +0100 Subject: [PATCH 20/65] Moved two very large methods from ImuFactorBase to PreintegrationBase --- gtsam/navigation/CombinedImuFactor.cpp | 8 +- gtsam/navigation/CombinedImuFactor.h | 8 - gtsam/navigation/ImuFactor.cpp | 16 +- gtsam/navigation/ImuFactor.h | 7 - gtsam/navigation/ImuFactorBase.h | 192 ----------------- gtsam/navigation/PreintegrationBase.h | 193 ++++++++++++++++++ .../tests/testCombinedImuFactor.cpp | 4 +- gtsam/navigation/tests/testImuFactor.cpp | 4 +- 8 files changed, 212 insertions(+), 220 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index edd57d17f..31ea65ab1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -209,7 +209,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); // error wrt bias evolution model (random walk) @@ -256,7 +258,9 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } // else, only compute the error vector: // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // boost::none, boost::none, boost::none, boost::none, boost::none); // error wrt bias evolution model (random walk) Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 03a0dd824..3e84c3e67 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -212,14 +212,6 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); - } - private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1b055bef2..07781a2b6 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -148,7 +148,8 @@ ImuFactor::ImuFactor( const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), + pose_i, vel_i, pose_j, vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {} @@ -180,13 +181,14 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { } //------------------------------------------------------------------------------ -Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5) const{ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4, boost::optional H5) const { - return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 01a245f8b..999fda80f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -197,13 +197,6 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const PreintegrationBase& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false){ - return ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements, gravity, omegaCoriolis, use2ndOrderCoriolis); - } - private: /** Serialization function */ diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 2d637d117..1b7919a82 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -27,20 +27,6 @@ namespace gtsam { -/** - * Struct to hold all state variables of returned by Predict function - */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - class ImuFactorBase { protected: @@ -93,184 +79,6 @@ public: (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); } - /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j - //------------------------------------------------------------------------------ - Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, boost::optional H5) const{ - - const double& deltaTij = _PIM.deltaTij(); - // We need the mistmatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3& Rot_i = pose_i.rotation(); - const Rot3& Rot_j = pose_j.rotation(); - const Vector3& pos_j = pose_j.translation().vector(); - - // Jacobian computation - /* ---------------------------------------------------------------------------------------------------- */ - // Get Get so<3> version of bias corrected rotation - // If H5 is asked for, we will need the Jacobian, which we store in H5 - // H5 will then be corrected below to take into account the Coriolis effect - Vector3 theta_biascorrected = - _PIM.biascorrectedThetaRij(biasOmegaIncr, H5); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - // TODO: these are not always needed - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(_PIM.deltaPij() - + _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(_PIM.deltaVij() - + _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; - } - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3; - } - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; - } - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3; - } - if(H5) { - // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * _PIM.delPdelBiasAcc(), - - Rot_i.matrix() * _PIM.delPdelBiasOmega(), - // dfV/dBias - - Rot_i.matrix() * _PIM.delVdelBiasAcc(), - - Rot_i.matrix() * _PIM.delVdelBiasOmega(), - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_); - - const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); - - const Vector3 fv = vel_j - predictedState_j.velocity; - - // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - /// Predict state at time j - //------------------------------------------------------------------------------ - static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const PreintegrationBase& _PIM, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = _PIM.deltaTij(); - const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij() - + _PIM.delPdelBiasAcc() * biasAccIncr - + _PIM.delPdelBiasOmega() * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij() - + _PIM.delVdelBiasAcc() * biasAccIncr - + _PIM.delVdelBiasOmega() * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr); - // TODO Frank says comment below does not reflect what was in code - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } - }; } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index fd5597aff..6cf6770c4 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -26,6 +26,20 @@ namespace gtsam { +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } +}; + /** * PreintegrationBase is the base class for PreintegratedMeasurements * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). @@ -150,6 +164,185 @@ public: } } + /// Predict state at time j + //------------------------------------------------------------------------------ + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const { + + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() + + delPdelBiasAcc() * biasAccIncr + + delPdelBiasOmega() * biasOmegaIncr) + + vel_i * deltaTij() + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij()*deltaTij(); + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + + delVdelBiasAcc() * biasAccIncr + + delVdelBiasOmega() * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij() // Coriolis term + + gravity * deltaTij()); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // TODO Frank says comment below does not reflect what was in code + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant + } + + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + //------------------------------------------------------------------------------ + Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5) const { + + // We need the mistmatch w.r.t. the biases used for preintegration + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Rot_i = pose_i.rotation(); + const Rot3& Rot_j = pose_j.rotation(); + const Vector3& pos_j = pose_j.translation().vector(); + + // Jacobian computation + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Vector3 theta_biascorrected = + biascorrectedThetaRij(biasOmegaIncr, H5); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + // TODO: these are not always needed + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis * deltaTij()); + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij()*deltaTij(); + dfVdPi = skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij(); + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(deltaPij() + + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(deltaVij() + + delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij() + + skewSymmetric(omegaCoriolis) * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis) * deltaTij(), // Coriolis term + // dfR/dVi + Z_3x3; + } + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3; + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3; + } + if(H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * delPdelBiasAcc(), + - Rot_i.matrix() * delPdelBiasOmega(), + // dfV/dBias + - Rot_i.matrix() * delVdelBiasAcc(), + - Rot_i.matrix() * delVdelBiasOmega(), + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis); + + const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); + + const Vector3 fv = vel_j - predictedState_j.velocity; + + // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 656fc1c6f..b55396041 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -290,7 +290,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); @@ -319,7 +319,7 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f9faa8b99..57becd86c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -561,7 +561,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); @@ -593,7 +593,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = factor.predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); From 4e6534eff751693ff5def375dc785e75dc604245 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 13:52:51 +0100 Subject: [PATCH 21/65] Tried to harmonize AHRS and IMU handling of coriolis term (but they are inconsistent) --- gtsam/navigation/AHRSFactor.cpp | 16 +++--- gtsam/navigation/AHRSFactor.h | 6 --- gtsam/navigation/PreintegratedRotation.h | 6 +++ gtsam/navigation/PreintegrationBase.h | 69 ++++++++++++------------ 4 files changed, 49 insertions(+), 48 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 7d6fdcc11..2859429db 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -182,8 +182,7 @@ void AHRSFactor::print(const string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -191,24 +190,23 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, +Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = // - _PIM_.predict(bias, H3); + const Vector3 theta_biascorrected = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = // - _PIM_.integrateCoriolis(rot_i, omegaCoriolis_); + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Matrix3 coriolisHat = skewSymmetric(coriolis); const Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); // Get error between actual and prediction - const Rot3 actualRij = rot_i.between(rot_j); + const Rot3 actualRij = Ri.between(Rj); const Rot3 fRhat = deltaRij_corrected.between(actualRij); Vector3 fR = Rot3::Logmap(fRhat); @@ -219,7 +217,7 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; (*H1) << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e3eb3d51a..d6a6c8dd9 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -92,12 +92,6 @@ public: Vector3 predict(const Vector3& bias, boost::optional H = boost::none) const; - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij(); - } - // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 1b21978fb..e84096859 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -112,6 +112,12 @@ public: return theta_biascorrected; } + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i, + const Vector3& omegaCoriolis) const { + return rot_i.transpose() * omegaCoriolis * deltaTij(); + } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 6cf6770c4..110d48118 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -176,24 +176,26 @@ public: const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); + const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij() - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + - coriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij() // Coriolis term + - 2 * coriolisHat * vel_i * deltaTij() // Coriolis term + gravity * deltaTij()); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij(); // 2nd order term for velocity + pos_j += - 0.5 * coriolisHat * coriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - coriolisHat * coriolisHat * pos_i * deltaTij(); // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); @@ -201,11 +203,11 @@ public: // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Vector3 theta_corrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + const Rot3 deltaRij_corrected = + Rot3::Expmap( theta_corrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_corrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant @@ -221,13 +223,13 @@ public: boost::optional H3, boost::optional H4, boost::optional H5) const { - // We need the mistmatch w.r.t. the biases used for preintegration + // We need the mismatch w.r.t. the biases used for preintegration const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); // we give some shorter name to rotations and translations - const Rot3& Rot_i = pose_i.rotation(); - const Rot3& Rot_j = pose_j.rotation(); + const Rot3& Ri = pose_i.rotation(); + const Rot3& Rj = pose_j.rotation(); const Vector3& pos_j = pose_j.translation().vector(); // Jacobian computation @@ -235,19 +237,20 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Vector3 theta_biascorrected = - biascorrectedThetaRij(biasOmegaIncr, H5); + Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); + // Prediction + const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); // TODO: these are not always needed - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis * deltaTij()); + const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Ri.inverse().matrix() * omegaCoriolis * deltaTij()); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { @@ -256,26 +259,26 @@ public: Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij()*deltaTij(); - dfVdPi = skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * Rot_i.matrix() * deltaTij(); + dfPdPi = - Ri.matrix() + 0.5 * coriolisHat * coriolisHat * Ri.matrix() * deltaTij()*deltaTij(); + dfVdPi = coriolisHat * coriolisHat * Ri.matrix() * deltaTij(); } else{ - dfPdPi = - Rot_i.matrix(); + dfPdPi = - Ri.matrix(); dfVdPi = Z_3x3; } (*H1) << // dfP/dRi - Rot_i.matrix() * skewSymmetric(deltaPij() + Ri.matrix() * skewSymmetric(deltaPij() + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), // dfP/dPi dfPdPi, // dfV/dRi - Rot_i.matrix() * skewSymmetric(deltaVij() + Ri.matrix() * skewSymmetric(deltaVij() + delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr), // dfV/dPi dfVdPi, // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + Jrinv_fRhat * (- Rj.between(Ri).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi Z_3x3; } @@ -284,10 +287,10 @@ public: (*H2) << // dfP/dVi - I_3x3 * deltaTij() - + skewSymmetric(omegaCoriolis) * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + + coriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - I_3x3 - + 2 * skewSymmetric(omegaCoriolis) * deltaTij(), // Coriolis term + + 2 * coriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } @@ -295,7 +298,7 @@ public: H3->resize(9,6); (*H3) << // dfP/dPosej - Z_3x3, Rot_j.matrix(), + Z_3x3, Rj.matrix(), // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej @@ -317,11 +320,11 @@ public: H5->resize(9,6); (*H5) << // dfP/dBias - - Rot_i.matrix() * delPdelBiasAcc(), - - Rot_i.matrix() * delPdelBiasOmega(), + - Ri.matrix() * delPdelBiasAcc(), + - Ri.matrix() * delPdelBiasOmega(), // dfV/dBias - - Rot_i.matrix() * delVdelBiasAcc(), - - Rot_i.matrix() * delVdelBiasOmega(), + - Ri.matrix() * delVdelBiasAcc(), + - Ri.matrix() * delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); From 943a18b124074b50c9aa5f7fac1bcbe633ffa39f Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sat, 6 Dec 2014 17:42:07 -0500 Subject: [PATCH 22/65] Fixed confusion in naming. --- gtsam/navigation/PreintegrationBase.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 110d48118..119d86310 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -241,7 +241,8 @@ public: // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction @@ -250,7 +251,7 @@ public: // TODO: these are not always needed const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Ri.inverse().matrix() * omegaCoriolis * deltaTij()); + const Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { @@ -259,8 +260,8 @@ public: Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis){ - dfPdPi = - Ri.matrix() + 0.5 * coriolisHat * coriolisHat * Ri.matrix() * deltaTij()*deltaTij(); - dfVdPi = coriolisHat * coriolisHat * Ri.matrix() * deltaTij(); + dfPdPi = - Ri.matrix() + 0.5 * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); + dfVdPi = omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); } else{ dfPdPi = - Ri.matrix(); @@ -287,10 +288,10 @@ public: (*H2) << // dfP/dVi - I_3x3 * deltaTij() - + coriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + + omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - I_3x3 - + 2 * coriolisHat * deltaTij(), // Coriolis term + + 2 * omegaCoriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } From d46224e8a14c608374201bbce0fd6bd9b522b894 Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Dec 2014 19:04:41 -0500 Subject: [PATCH 23/65] added const and changed name in updatePreintegratedMeasurements to "omegaCoriolisHat" to comply with notation in "predict" --- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/PreintegrationBase.h | 20 +++++++++----------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index e84096859..0922ea63d 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -74,7 +74,7 @@ public: } /// Update preintegrated measurements - void updateIntegratedRotationAndDeltaT(const Rot3& incrR, double deltaT){ + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT){ deltaRij_ = deltaRij_ * incrR; deltaTij_ += deltaT; } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 119d86310..9869ce133 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -116,7 +116,7 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, double deltaT) { + const Rot3& incrR, const double deltaT) { Matrix3 dRij = deltaRij(); // expensive Vector3 temp = dRij * correctedAcc * deltaT; if(!use2ndOrderIntegration_){ @@ -176,7 +176,7 @@ public: const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); - const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); + const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ @@ -184,23 +184,22 @@ public: + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij() - - coriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + - omegaCoriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr) - - 2 * coriolisHat * vel_i * deltaTij() // Coriolis term + - 2 * omegaCoriolisHat * vel_i * deltaTij() // Coriolis term + gravity * deltaTij()); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * coriolisHat * coriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - coriolisHat * coriolisHat * pos_i * deltaTij(); // 2nd order term for velocity + pos_j += - 0.5 * omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij(); // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // TODO Frank says comment below does not reflect what was in code - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_corrected = theta_biascorrected - @@ -240,9 +239,8 @@ public: Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - const Matrix3 coriolisHat = skewSymmetric(coriolis); const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction @@ -251,7 +249,7 @@ public: // TODO: these are not always needed const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); if(H1) { From 792d2656d09b21a63fb18c1ca4b168bbb74a721b Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Dec 2014 19:13:02 -0500 Subject: [PATCH 24/65] using cross product instead of skewSymm matrix in predict --- gtsam/navigation/PreintegrationBase.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9869ce133..0468b77e2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -176,26 +176,24 @@ public: const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); - const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); - // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij() - - omegaCoriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr) - - 2 * omegaCoriolisHat * vel_i * deltaTij() // Coriolis term + - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + gravity * deltaTij()); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position - vel_j += - omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij(); // 2nd order term for velocity + pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); From 6d571ca6b99f12256bcfe4e344ef54503c7d4ed2 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 7 Dec 2014 13:14:45 -0500 Subject: [PATCH 25/65] truth revealing unit test :-) re-established good functioning of IMU factor (TODO: fix CombinedImuFactor F & G) --- gtsam/navigation/ImuFactor.cpp | 29 +++++--- gtsam/navigation/ImuFactor.h | 4 +- gtsam/navigation/PreintegrationBase.h | 22 ------ gtsam/navigation/tests/testImuFactor.cpp | 86 ++++++++++++++++++++++++ 4 files changed, 107 insertions(+), 34 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 07781a2b6..9f76c30f4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -67,7 +67,8 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { + boost::optional body_P_sensor, + boost::optional Fout, boost::optional Gout) { // NOTE: order is important here because each update uses old values (i.e., we have to update // jacobians and covariances before updating preintegrated measurements). @@ -87,6 +88,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) + const Matrix3 R_i = deltaRij(); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); // Update preintegrated measurements. TODO Frank moved from end of this function !!! @@ -102,7 +104,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Matrix H_vel_pos = Z_3x3; Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); @@ -124,15 +126,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only kept for documentation. - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // Fout and Gout are used for testing purposes and are not needed by the factor + if(Fout){ + Fout->resize(9,9); + (*Fout) << F; + } + if(Gout){ + // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT + // This in only kept for testing. + Gout->resize(9,9); + (*Gout) << I_3x3 * deltaT, Z_3x3, Z_3x3, + Z_3x3, R_i * deltaT, Z_3x3, + Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; + //preintMeasCov = F * preintMeasCov * F.transpose() + Gout * (1/deltaT) * measurementCovariance * Gout.transpose(); + } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 999fda80f..e0a6b861a 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -111,9 +111,11 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time interval between two consecutive IMU measurements * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + * @param Fout, Gout Jacobians used internally (only needed for testing) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional Fout = boost::none, boost::optional Gout = boost::none); /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 0468b77e2..df5553b38 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -343,28 +343,6 @@ public: return r; } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 57becd86c..fbc4bee05 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -49,6 +49,23 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } +Vector PreIntegrateIMUObservations_delta_vel(const Vector3& msr_acc_t, + const double msr_dt, const Vector3& delta_angles, const Vector3& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; +} + +Vector PreIntegrateIMUObservations_delta_angles(const Vector3& msr_gyro_t, + const double msr_dt, const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles) * Rot3::Expmap( msr_gyro_t * msr_dt ); + Vector result = Rot3::Logmap(R_t_to_t0); + return result; +} + ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -423,6 +440,75 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Vector3 theta_i = preintegrated.thetaRij(); // before adding new measurement + const Vector3 deltaVij = preintegrated.deltaVij();// before adding new measurement + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + // Compute expected F + Matrix H_vel_angles_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, + newMeasuredAcc, newDeltaT, _1, deltaVij), theta_i); + Matrix H_angles_angles_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, + newMeasuredOmega, newDeltaT, _1), theta_i); + Matrix Fexpected(9,9); + Fexpected << I_3x3, I_3x3 * newDeltaT, Z_3x3, + Z_3x3, I_3x3, H_vel_angles_expected, + Z_3x3, Z_3x3, H_angles_angles_expected; + + // verify F + EXPECT(assert_equal(Fexpected, Factual)); + + // Compute expected G + Matrix H_vel_noiseAcc_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, + _1, newDeltaT, theta_i, deltaVij), newMeasuredAcc); + Matrix H_angles_noiseOmega_expected = + numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, + _1, newDeltaT, theta_i), newMeasuredOmega); + Matrix Gexpected(9,9); + // [integrationError measuredAcc measuredOmega] + Gexpected << I_3x3 * newDeltaT, Z_3x3, Z_3x3, + Z_3x3, H_vel_noiseAcc_expected, Z_3x3, + Z_3x3, Z_3x3, H_angles_noiseOmega_expected; + // verify G + EXPECT(assert_equal(Gexpected, Gactual,1e-7)); +} + //#include ///* ************************************************************************* */ //TEST( ImuFactor, LinearizeTiming) From aee20d669d3ad977ea3158debf29710ff4cc5145 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 7 Dec 2014 13:58:13 -0500 Subject: [PATCH 26/65] removed useless comments, added other comments --- gtsam/navigation/ImuFactor.cpp | 50 ++++++++++++++-------------------- 1 file changed, 20 insertions(+), 30 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9f76c30f4..08b80ce4a 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -68,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor, - boost::optional Fout, boost::optional Gout) { + boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values (i.e., we have to update // jacobians and covariances before updating preintegrated measurements). @@ -91,34 +91,21 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Matrix3 R_i = deltaRij(); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - // Update preintegrated measurements. TODO Frank moved from end of this function !!! - // Even though Luca says has to happen after ? Don't understand why. + // Update preintegrated measurements updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; + // pos vel angle + F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos + Z_3x3, I_3x3, H_vel_angles, // vel + Z_3x3, Z_3x3, H_angles_angles;// angle // first order uncertainty propagation: // the deltaT allows to pass from continuous time noise to discrete time noise @@ -126,19 +113,22 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - // Fout and Gout are used for testing purposes and are not needed by the factor - if(Fout){ - Fout->resize(9,9); - (*Fout) << F; + // F_test and Gout are used for testing purposes and are not needed by the factor + if(F_test){ + // This in only for testing + F_test->resize(9,9); + (*F_test) << F; } - if(Gout){ + if(G_test){ // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only kept for testing. - Gout->resize(9,9); - (*Gout) << I_3x3 * deltaT, Z_3x3, Z_3x3, - Z_3x3, R_i * deltaT, Z_3x3, - Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - //preintMeasCov = F * preintMeasCov * F.transpose() + Gout * (1/deltaT) * measurementCovariance * Gout.transpose(); + // This in only for testing + + G_test->resize(9,9); + // intNoise accNoise omegaNoise + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle + //preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } } From ab54ca1697eab189214f2aacf869cdefa40191a0 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 7 Dec 2014 15:03:48 -0500 Subject: [PATCH 27/65] addressed some of Frank's comments --- gtsam/navigation/ImuFactor.cpp | 10 +++++----- gtsam/navigation/PreintegrationBase.h | 15 +++++++++------ 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 08b80ce4a..4cd5977fb 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -122,13 +122,13 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( if(G_test){ // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // This in only for testing - G_test->resize(9,9); // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle - //preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle + // Propagation with no approximation: + // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index df5553b38..be3f9afa5 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -125,7 +125,6 @@ public: deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } deltaVij_ += temp; - // TODO: we update rotation *after* the others. Is that correct? updateIntegratedRotationAndDeltaT(incrR,deltaT); } @@ -143,7 +142,6 @@ public: } delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasOmega_ += temp; - // TODO: we update rotation *after* the others. Is that correct? update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); } @@ -244,11 +242,16 @@ public: // Prediction const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); - // TODO: these are not always needed + // Residual rotation error const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + // Accessory matrix, used to build the jacobians + Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat; + if(H1 || H3 || H5){ + Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); + Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + } if(H1) { H1->resize(9,6); From 2730dab4c626f09aef46cb1b51a5edc352f30f16 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 7 Dec 2014 15:04:10 -0500 Subject: [PATCH 28/65] made test more serious and easy to understand --- gtsam/navigation/tests/testImuFactor.cpp | 98 ++++++++++++++---------- 1 file changed, 58 insertions(+), 40 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index fbc4bee05..943f1cced 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -49,20 +49,25 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } -Vector PreIntegrateIMUObservations_delta_vel(const Vector3& msr_acc_t, - const double msr_dt, const Vector3& delta_angles, const Vector3& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; -} +// Correspond to updatePreintegratedMeasurements, but has a different syntax to test numerical derivatives +Vector updatePreintegratedMeasurementsTest( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Vector3& logDeltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration_) { -Vector PreIntegrateIMUObservations_delta_angles(const Vector3& msr_gyro_t, - const double msr_dt, const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles) * Rot3::Expmap( msr_gyro_t * msr_dt ); - Vector result = Rot3::Logmap(R_t_to_t0); + Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration_){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); + Vector result(9); result << deltaPij_new, deltaVij_new, logDeltaRij_new; return result; } @@ -471,42 +476,55 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Vector3 theta_i = preintegrated.thetaRij(); // before adding new measurement - const Vector3 deltaVij = preintegrated.deltaVij();// before adding new measurement + const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, Factual, Gactual); - // Compute expected F - Matrix H_vel_angles_expected = - numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, - newMeasuredAcc, newDeltaT, _1, deltaVij), theta_i); - Matrix H_angles_angles_expected = - numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, - newMeasuredOmega, newDeltaT, _1), theta_i); - Matrix Fexpected(9,9); - Fexpected << I_3x3, I_3x3 * newDeltaT, Z_3x3, - Z_3x3, I_3x3, H_vel_angles_expected, - Z_3x3, Z_3x3, H_angles_angles_expected; + bool use2ndOrderIntegration = false; - // verify F + // Compute expected F wrt positions + Matrix df_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + _1, deltaVij_old, logDeltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // Compute expected F wrt velocities + Matrix df_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, _1, logDeltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // Compute expected F wrt angles + Matrix df_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); + Matrix Fexpected(9,9); + + Fexpected << df_dpos, df_dvel, df_dangle; EXPECT(assert_equal(Fexpected, Factual)); - // Compute expected G - Matrix H_vel_noiseAcc_expected = - numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, - _1, newDeltaT, theta_i, deltaVij), newMeasuredAcc); - Matrix H_angles_noiseOmega_expected = - numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, - _1, newDeltaT, theta_i), newMeasuredOmega); + // Compute expected G wrt integration noise + Matrix df_dintNoise(9,3); + df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; + + // Compute expected F wrt acc noise + Matrix df_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + // Compute expected F wrt gyro noise + Matrix df_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); Matrix Gexpected(9,9); - // [integrationError measuredAcc measuredOmega] - Gexpected << I_3x3 * newDeltaT, Z_3x3, Z_3x3, - Z_3x3, H_vel_noiseAcc_expected, Z_3x3, - Z_3x3, Z_3x3, H_angles_noiseOmega_expected; - // verify G - EXPECT(assert_equal(Gexpected, Gactual,1e-7)); + + Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; + EXPECT(assert_equal(Gexpected, Gactual)); } //#include From d5d759488869d7e744af42f7ab72a97edbf44072 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 12:29:26 -0500 Subject: [PATCH 29/65] deleted hard-coded function from IMU unit test --- gtsam/navigation/tests/testImuFactor.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 943f1cced..b68d4ea5d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -346,20 +346,14 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Measurements Vector3 deltatheta; deltatheta << 0, 0, 0; - // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); - const Matrix3 actualDelFdeltheta = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + Matrix3 actualDelFdeltheta = Rot3::rightJacobianExpMapSO3inverse(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - } /* ************************************************************************* */ @@ -376,7 +370,6 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -388,7 +381,7 @@ TEST( ImuFactor, fistOrderExponential ) hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - // Compare Jacobians + // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); } From 422db08c69cf5f9b9e95b468dbef8d01c2cd402d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 12:30:53 -0500 Subject: [PATCH 30/65] included Jacobian for logmap and expmap, with unit tests (Note: only implemented for Rot3M, this will not work in quaternion mode) --- gtsam/geometry/Rot3.h | 22 ++++++++++++--- gtsam/geometry/Rot3M.cpp | 14 ++++++++-- gtsam/geometry/tests/testRot3.cpp | 46 +++++++++++++++++++------------ 3 files changed, 58 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ef2d19750..cdb0bf305 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -294,15 +294,21 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); + static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { + if(H){ + H->resize(3,3); + *H = Rot3::rightJacobianExpMapSO3(v); + } + if(zero(v)) + return Rot3(); + else + return rodriguez(v); } /** * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R, boost::optional H = boost::none); /// Left-trivialized derivative of the exponential map static Matrix3 dexpL(const Vector3& v); @@ -313,11 +319,19 @@ namespace gtsam { /** * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * expmap(thetahat + thetatilde) \approx expmap(thetahat) * expmap(Jr * thetatilde) + * where Jr = rightJacobianExpMapSO3(thetahat); + * This maps a perturbation in the tangent space (thetatilde) to + * a perturbation on the manifold (expmap(Jr * thetatilde)) */ static Matrix3 rightJacobianExpMapSO3(const Vector3& x); /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * Rtilde) \approx logmap( Rhat ) + Jrinv * logmap( Rtilde ) + * where Jrinv = rightJacobianExpMapSO3inverse(logmap( Rtilde )); + * This maps a perturbation on the manifold (Rtilde) + * to a perturbation in the tangent space (Jrinv * logmap( Rtilde )) */ static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d0c7e95f3..46f5b8109 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -200,7 +200,7 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { static const double PI = boost::math::constants::pi(); @@ -208,6 +208,8 @@ Vector3 Rot3::Logmap(const Rot3& R) { // Get trace(R) double tr = rot.trace(); + Vector3 thetaR; + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { @@ -218,7 +220,7 @@ Vector3 Rot3::Logmap(const Rot3& R) { return (PI / sqrt(2.0+2.0*rot(1,1))) * Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * + thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) * Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; @@ -231,11 +233,17 @@ Vector3 Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector3( + thetaR = magnitude*Vector3( rot(2,1)-rot(1,2), rot(0,2)-rot(2,0), rot(1,0)-rot(0,1)); } + + if(H){ + H->resize(3,3); + *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + } + return thetaR; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 88accb90f..824878d13 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -215,33 +215,45 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +/* ************************************************************************* */ +Vector3 thetahat(0.1, 0, 0.1); +TEST( Rot3, rightJacobianExpMapSO3 ) +{ + Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3 ) +TEST( Rot3, jacobianExpmap ) { - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; - - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - CHECK(assert_equal(expectedJacobian, actualJacobian)); + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + const Rot3 R = Rot3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ TEST( Rot3, rightJacobianExpMapSO3inverse ) { - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); +/* ************************************************************************* */ +TEST( Rot3, jacobianLogmap ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + Rot3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ From f991c1a3989c407a5b4bb4d04eb6677fcb7ad5f6 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 13:15:51 -0500 Subject: [PATCH 31/65] getting rid of rightJacobianSO3 (not completed yet) --- gtsam/navigation/ImuFactor.cpp | 13 +++++++----- gtsam/navigation/PreintegratedRotation.h | 15 +++++++------- gtsam/navigation/PreintegrationBase.h | 25 ++++++++++++++---------- 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 4cd5977fb..f25268350 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -77,8 +77,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -87,15 +87,18 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements covariance // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ +// Matrix3 Jr_theta_i; +// const Vector3 theta_i = thetaRij(Jr_theta_i); // super-expensive parametrization of so(3) +// const Matrix3 R_i = deltaRij(); + const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 R_i = deltaRij(); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 R_i = deltaRij(); // Update preintegrated measurements updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); - const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + Matrix3 Jrinv_theta_j; thetaRij(Jrinv_theta_j); // computation of rightJacobianInverse Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 0922ea63d..b93360fab 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -49,7 +49,7 @@ public: /// methods to access class variables Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive - Vector3 thetaRij() const {return Rot3::Logmap(deltaRij_);} // super-expensive + Vector3 thetaRij(boost::optional H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive const double& deltaTij() const{return deltaTij_;} const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} @@ -99,17 +99,18 @@ public: boost::optional H) const { // First, we correct deltaRij using the biasOmegaIncr, rotated const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // This was done via an expmap, now we go *back* to so<3> - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + if (H) { - // We then do a very expensive Jacobian calculation. TODO Right Duy ? - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + Matrix3 Jrinv_theta_bc; + // This was done via an expmap, now we go *back* to so<3> + const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + return theta_biascorrected; } - return theta_biascorrected; + //else + return Rot3::Logmap(deltaRij_biascorrected); } /// Integrate coriolis correction in body frame rot_i diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index be3f9afa5..f91a889f2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -239,18 +239,24 @@ public: const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); Vector3 theta_corrected = theta_biascorrected - coriolis; - // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); - - // Residual rotation error - const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); + Rot3 deltaRij_corrected, fRhat; + Vector3 fR; // Accessory matrix, used to build the jacobians Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat; - if(H1 || H3 || H5){ - Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); + + // This is done to save computation: we ask for the jacobians only when they are needed + if(H1 || H2 || H3 || H4 || H5){ + deltaRij_corrected = Rot3::Expmap( theta_corrected, Jr_theta_bcc); + // Residual rotation error + fRhat = deltaRij_corrected.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRhat, Jrinv_fRhat); Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + }else{ + deltaRij_corrected = Rot3::Expmap( theta_corrected); + // Residual rotation error + fRhat = deltaRij_corrected.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRhat); } if(H1) { @@ -339,8 +345,7 @@ public: const Vector3 fv = vel_j - predictedState_j.velocity; - // This is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) - const Vector3 fR = Rot3::Logmap(fRhat); + // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) Vector r(9); r << fp, fv, fR; return r; From 54ee973309249f4db5c505d60fa1ed4ff592e5a1 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 16:27:57 -0500 Subject: [PATCH 32/65] changed naming in comments about expmap & logmap jacobians --- gtsam/geometry/Rot3.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index cdb0bf305..87a035bfb 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -319,19 +319,19 @@ namespace gtsam { /** * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * expmap(thetahat + thetatilde) \approx expmap(thetahat) * expmap(Jr * thetatilde) + * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) * where Jr = rightJacobianExpMapSO3(thetahat); - * This maps a perturbation in the tangent space (thetatilde) to - * a perturbation on the manifold (expmap(Jr * thetatilde)) + * This maps a perturbation in the tangent space (omega) to + * a perturbation on the manifold (expmap(Jr * omega)) */ static Matrix3 rightJacobianExpMapSO3(const Vector3& x); /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * logmap( Rhat * Rtilde) \approx logmap( Rhat ) + Jrinv * logmap( Rtilde ) - * where Jrinv = rightJacobianExpMapSO3inverse(logmap( Rtilde )); - * This maps a perturbation on the manifold (Rtilde) - * to a perturbation in the tangent space (Jrinv * logmap( Rtilde )) + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = rightJacobianExpMapSO3inverse(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) */ static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); From 02f92e4e04b1c3e64dff65aa74e5b5ce45ae9a3c Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 18:39:47 -0500 Subject: [PATCH 33/65] included Jacobian of logmap in quaternion mode --- gtsam/geometry/Rot3Q.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 19de92ca2..e73ed3eaf 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -146,21 +146,22 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { + Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + Vector3 thetaR; const Quaternion& q = R.quaternion_; const double qw = q.w(); if (qw > NearlyOne) { // Taylor expansion of (angle / s) at 1 - return (2 - 2 * (qw - 1) / 3) * q.vec(); + thetaR = (2 - 2 * (qw - 1) / 3) * q.vec(); } else if (qw < NearlyNegativeOne) { // Angle is zero, return zero vector - return Vector3::Zero(); + thetaR = Vector3::Zero(); } else { // Normal, away from zero case double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); @@ -169,8 +170,14 @@ namespace gtsam { angle -= twoPi; else if (angle < -M_PI) angle += twoPi; - return (angle / s) * q.vec(); + thetaR = (angle / s) * q.vec(); } + + if(H){ + H->resize(3,3); + *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + } + return thetaR; } /* ************************************************************************* */ From dc13912ce2a0e965d31cb0fee99672d4c8c400db Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 18:41:39 -0500 Subject: [PATCH 34/65] moved jacobian computation to updateMeasurement functions, and fixed noise propagation. Luca&Christian: insight is that preintegration noise acts on rotations as R * expmap(noise), while before it was expmap( logmap(R) + noise) --- gtsam/navigation/AHRSFactor.cpp | 35 ++----- gtsam/navigation/ImuFactor.cpp | 39 ++----- gtsam/navigation/PreintegratedRotation.h | 7 +- gtsam/navigation/PreintegrationBase.h | 15 ++- gtsam/navigation/tests/testImuFactor.cpp | 123 ++++++++++++++++------- 5 files changed, 120 insertions(+), 99 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2859429db..9c684658c 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -69,7 +69,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { - // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; @@ -84,42 +83,20 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; - - // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !! - const Matrix3 incrRt = incrR.transpose(); - - // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + Matrix3 Jr_theta_incr; + const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !! // Update Jacobian update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); - // Update preintegrated measurements covariance - const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - - // Update rotation and deltaTij. TODO Frank moved from end of this function !!! - updateIntegratedRotationAndDeltaT(incrR, deltaT); - - const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first - // order propagation that can be seen as a prediction phase in an EKF framework - Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11 - // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrpt preintegrated measurements (df/dx) - const Matrix3& F = H_angles_angles; + // Update rotation and deltaTij. + Matrix3 Fr; // Jacobian of the update + updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + measurementCovariance_ * deltaT; - } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index f25268350..862d081f2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -70,9 +70,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( boost::optional body_P_sensor, boost::optional F_test, boost::optional G_test) { - // NOTE: order is important here because each update uses old values (i.e., we have to update - // jacobians and covariances before updating preintegrated measurements). - Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); @@ -81,42 +78,22 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); - // Update preintegrated measurements covariance + // Update preintegrated measurements (also get Jacobian) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix F; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + + // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ -// Matrix3 Jr_theta_i; -// const Vector3 theta_i = thetaRij(Jr_theta_i); // super-expensive parametrization of so(3) -// const Matrix3 R_i = deltaRij(); - - const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - const Matrix3 R_i = deltaRij(); - - // Update preintegrated measurements - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); - - Matrix3 Jrinv_theta_j; thetaRij(Jrinv_theta_j); // computation of rightJacobianInverse - - Matrix H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - // pos vel angle - F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos - Z_3x3, I_3x3, H_vel_angles, // vel - Z_3x3, Z_3x3, H_angles_angles;// angle - - // first order uncertainty propagation: // the deltaT allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - // F_test and Gout are used for testing purposes and are not needed by the factor + // F_test and G_test are used for testing purposes and are not needed by the factor if(F_test){ // This in only for testing F_test->resize(9,9); @@ -129,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle + Z_3x3, Z_3x3, Jr_theta_incr * deltaT; // angle // Propagation with no approximation: // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b93360fab..b5dea4a6c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -74,9 +74,14 @@ public: } /// Update preintegrated measurements - void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT){ + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, + boost::optional H = boost::none){ deltaRij_ = deltaRij_ * incrR; deltaTij_ += deltaT; + if(H){ + H->resize(3,3); + *H = incrR.transpose(); + } } /** diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f91a889f2..200219fb3 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -116,7 +116,8 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT) { + const Rot3& incrR, const double deltaT, boost::optional F = boost::none) { + Matrix3 dRij = deltaRij(); // expensive Vector3 temp = dRij * correctedAcc * deltaT; if(!use2ndOrderIntegration_){ @@ -125,7 +126,17 @@ public: deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } deltaVij_ += temp; - updateIntegratedRotationAndDeltaT(incrR,deltaT); + Matrix3 F_angles_angles; + const Matrix3 R_i = deltaRij(); + updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); + Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; + if(F){ + F->resize(9,9); + // pos vel angle + *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles;// angle + } } /// Update Jacobians to be used during preintegration diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b68d4ea5d..29a2a13de 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -37,6 +37,8 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias){ @@ -49,13 +51,14 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } -// Correspond to updatePreintegratedMeasurements, but has a different syntax to test numerical derivatives -Vector updatePreintegratedMeasurementsTest( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Vector3& logDeltaRij_old, +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedPosVel( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, const bool use2ndOrderIntegration_) { - Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * correctedAcc * deltaT; Vector3 deltaPij_new; @@ -65,12 +68,20 @@ Vector updatePreintegratedMeasurementsTest( deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); - Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); - Vector result(9); result << deltaPij_new, deltaVij_new, logDeltaRij_new; + + Vector result(6); result << deltaPij_new, deltaVij_new; return result; } +Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, + const Vector3& correctedOmega, const double deltaT) { + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + return deltaRij_new; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -356,6 +367,24 @@ TEST( ImuFactor, PartialDerivativeLogmap ) EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); } +Rot3 constRot = Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0); +Rot3 testRot(const Rot3& Rk){ + return Rk * constRot; +} +/* ************************************************************************* */ +TEST( ImuFactor, understandRot ) +{ + Rot3 Rbar = Rot3::RzRyRx( M_PI, M_PI/6.0, -M_PI/4.0 ); + + Matrix Jexpected = numericalDerivative11(boost::bind( + &testRot, _1), Rbar); + + Matrix3 Jactual = constRot.transpose(); + + // Compare Jacobians + EXPECT(assert_equal(Jexpected, Jactual)); +} + /* ************************************************************************* */ TEST( ImuFactor, fistOrderExponential ) { @@ -469,7 +498,6 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement @@ -480,44 +508,67 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) bool use2ndOrderIntegration = false; - // Compute expected F wrt positions - Matrix df_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, logDeltaRij_old, + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + _1, deltaVij_old, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); - // Compute expected F wrt velocities - Matrix df_dvel = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, logDeltaRij_old, + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, _1, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); - // Compute expected F wrt angles - Matrix df_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); - Matrix Fexpected(9,9); + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix FexpectedTop6(6,9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - Fexpected << df_dpos, df_dvel, df_dangle; - EXPECT(assert_equal(Fexpected, Factual)); + EXPECT(assert_equal(FexpectedTop6, Factual.topRows(6))); - // Compute expected G wrt integration noise - Matrix df_dintNoise(9,3); - df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; + // Compute expected f_rot wrt angles + Matrix dfr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + _1, newMeasuredOmega, newDeltaT), deltaRij_old); - // Compute expected F wrt acc noise - Matrix df_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, + Matrix FexpectedBottom3(3,9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + EXPECT(assert_equal(FexpectedBottom3, Factual.bottomRows(3))); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6,3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix df_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, + Matrix dgpv_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix Gexpected(9,9); + Matrix GexpectedTop6(6,9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + EXPECT(assert_equal(GexpectedTop6, Gactual.topRows(6))); - Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; - EXPECT(assert_equal(Gexpected, Gactual)); + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + deltaRij_old, _1, newDeltaT), newMeasuredOmega); + + Matrix GexpectedBottom3(3,9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + EXPECT(assert_equal(GexpectedBottom3, Gactual.bottomRows(3))); } //#include From b126d98609e50bd6b565fcfd154471de7a3430b5 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 20:20:03 -0500 Subject: [PATCH 35/65] included suggestions from Frank --- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegratedRotation.h | 6 +----- gtsam/navigation/PreintegrationBase.h | 11 +++++++---- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 862d081f2..1aa261d34 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -101,7 +101,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( } if(G_test){ // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only for testing + // This in only for testing & documentation G_test->resize(9,9); // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b5dea4a6c..d949ee21c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -76,12 +76,8 @@ public: /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, boost::optional H = boost::none){ - deltaRij_ = deltaRij_ * incrR; + deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaTij_ += deltaT; - if(H){ - H->resize(3,3); - *H = incrR.transpose(); - } } /** diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 200219fb3..11db97642 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -126,16 +126,19 @@ public: deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; } deltaVij_ += temp; - Matrix3 F_angles_angles; - const Matrix3 R_i = deltaRij(); - updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); - Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; + if(F){ + Matrix3 F_angles_angles; + const Matrix3 R_i = deltaRij(); + updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); + Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; F->resize(9,9); // pos vel angle *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, Z_3x3, F_angles_angles;// angle + }else{ + updateIntegratedRotationAndDeltaT(incrR,deltaT); } } From 64dfde3ae62683aa19b3179940bbe8aaed4c96c9 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 8 Dec 2014 20:28:28 -0500 Subject: [PATCH 36/65] in the process of adding tests for the combined imu factor (not there yet) --- gtsam/navigation/CombinedImuFactor.cpp | 20 +- gtsam/navigation/CombinedImuFactor.h | 3 +- .../tests/testCombinedImuFactor.cpp | 239 ++++++++++++------ 3 files changed, 175 insertions(+), 87 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 31ea65ab1..ef94e4141 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -71,7 +71,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor) { + double deltaT, boost::optional body_P_sensor, + boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). @@ -129,7 +130,6 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); // BLOCK DIAGONAL TERMS G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); @@ -152,6 +152,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; + + // F_test and G_test are used for testing purposes and are not needed by the factor + if(F_test){ + F_test->resize(15,15); + (*F_test) << F; + } + if(G_test){ + G_test->resize(15,21); + // This is for testing & documentation + ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, H_angles_angles, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; + } } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3e84c3e67..f7041fdeb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -126,7 +126,8 @@ public: * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index b55396041..1842e3d55 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,19 +39,44 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -/* ************************************************************************* */ namespace { +/* ************************************************************************* */ +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedMeasurementsTest( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, + const Vector3& logDeltaRij_old, const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration_) { -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration_){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); + imuBias::ConstantBias bias_new(bias_old); + Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + return result; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + const list& deltaTs){ + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -59,7 +84,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -67,20 +91,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); + measuredAccs, measuredOmegas, deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); } @@ -89,9 +109,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij()); } @@ -101,7 +119,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases @@ -120,28 +137,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); -// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases -// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) -// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) -// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); -// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); -// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); -// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); + EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); + EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); @@ -157,50 +163,37 @@ TEST( CombinedImuFactor, ErrorWithBiases ) double deltaT = 1.0; double tol = 1e-6; - // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - Matrix I6x6(6,6); I6x6 = Matrix::Identity(6,6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - - // Actual Jacobians + // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); @@ -214,7 +207,6 @@ TEST( CombinedImuFactor, ErrorWithBiases ) /* ************************************************************************* */ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases @@ -237,22 +229,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -265,6 +257,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -283,22 +276,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); - + // Create factor + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; expectedVelocity<<0,1,0; + EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) Matrix I6x6(6,6); @@ -324,9 +316,88 @@ TEST(CombinedImuFactor, PredictRotation) { EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } -#include +/* ************************************************************************* */ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement +// +// Matrix Factual, Gactual; +// preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, +// body_P_sensor, Factual, Gactual); +// +// bool use2ndOrderIntegration = false; +// +// // Compute expected F wrt positions +// Matrix df_dpos = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// _1, deltaVij_old, logDeltaRij_old, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); +// // Compute expected F wrt velocities +// Matrix df_dvel = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, _1, logDeltaRij_old, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); +// // Compute expected F wrt angles +// Matrix df_dangle = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, _1, +// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); +// Matrix Fexpected(9,9); +// +// Fexpected << df_dpos, df_dvel, df_dangle; +// EXPECT(assert_equal(Fexpected, Factual)); +// +// // Compute expected G wrt integration noise +// Matrix df_dintNoise(9,3); +// df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; +// +// // Compute expected F wrt acc noise +// Matrix df_daccNoise = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, logDeltaRij_old, +// _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); +// // Compute expected F wrt gyro noise +// Matrix df_domegaNoise = +// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, +// deltaPij_old, deltaVij_old, logDeltaRij_old, +// newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); +// Matrix Gexpected(9,9); +// +// Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; +// EXPECT(assert_equal(Gexpected, Gactual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 53b59bf488aa172f37c5486d60241059a8cd2e4b Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 15:17:26 -0500 Subject: [PATCH 37/65] added truth revealing unit test for Combined Imu factor (and fixed latest changes, that, moving updatePreintegratedMeasurements before, were creating a bug) --- gtsam/navigation/CombinedImuFactor.cpp | 5 +- .../tests/testCombinedImuFactor.cpp | 65 ++++++++++--------- 2 files changed, 38 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ef94e4141..ba51fa31a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -95,6 +95,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 R_i = deltaRij(); // store this // Update preintegrated measurements. TODO Frank moved from end of this function !!! updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); @@ -108,10 +109,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Matrix3 H_vel_pos = Z_3x3; Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij() * deltaT; + Matrix3 H_vel_biasacc = - R_i * deltaT; Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_vel = Z_3x3; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 1842e3d55..c88e054e6 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -52,7 +52,7 @@ Vector updatePreintegratedMeasurementsTest( Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); Matrix3 dRij = deltaRij_old.matrix(); - Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; if(!use2ndOrderIntegration_){ deltaPij_new = deltaPij_old + deltaVij_old * deltaT; @@ -60,7 +60,7 @@ Vector updatePreintegratedMeasurementsTest( deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); imuBias::ConstantBias bias_new(bias_old); Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); @@ -320,7 +320,7 @@ TEST(CombinedImuFactor, PredictRotation) { TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point - imuBias::ConstantBias bias = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases + imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); // Measurements @@ -340,7 +340,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) } // Actual preintegrated values CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); // so far we only created a nontrivial linearization point for the preintegrated measurements // Now we add a new measurement and ask for Jacobians @@ -351,32 +351,37 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement -// -// Matrix Factual, Gactual; -// preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, -// body_P_sensor, Factual, Gactual); -// -// bool use2ndOrderIntegration = false; -// -// // Compute expected F wrt positions -// Matrix df_dpos = -// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, -// _1, deltaVij_old, logDeltaRij_old, -// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); -// // Compute expected F wrt velocities -// Matrix df_dvel = -// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, -// deltaPij_old, _1, logDeltaRij_old, -// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); -// // Compute expected F wrt angles -// Matrix df_dangle = -// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, -// deltaPij_old, deltaVij_old, _1, -// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); -// Matrix Fexpected(9,9); -// -// Fexpected << df_dpos, df_dvel, df_dangle; -// EXPECT(assert_equal(Fexpected, Factual)); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + // Compute expected F wrt positions (15,3) + Matrix df_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + _1, deltaVij_old, logDeltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // Compute expected F wrt velocities (15,3) + Matrix df_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, _1, logDeltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // Compute expected F wrt angles (15,3) + Matrix df_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); + // Compute expected F wrt angles (15,6) + Matrix df_dbias = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix Fexpected(15,15); + + Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; + EXPECT(assert_equal(Fexpected, Factual)); // // // Compute expected G wrt integration noise // Matrix df_dintNoise(9,3); From 2c1d72e7d762deb72780eae3a6fdadd38e2b3455 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 15:50:41 -0500 Subject: [PATCH 38/65] tested Jacobians computation (G) for combined IMU factor --- gtsam/navigation/CombinedImuFactor.cpp | 12 ++--- .../tests/testCombinedImuFactor.cpp | 49 ++++++++++++------- 2 files changed, 36 insertions(+), 25 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ba51fa31a..4d2fe14ea 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -143,9 +143,9 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); + G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9); - G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); + G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12); // NEW OFF BLOCK DIAGONAL TERMS Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); @@ -164,10 +164,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // This is for testing & documentation ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, - Z_3x3, Z_3x3, H_angles_angles, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } } diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c88e054e6..bb6828216 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -382,25 +382,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; EXPECT(assert_equal(Fexpected, Factual)); -// -// // Compute expected G wrt integration noise -// Matrix df_dintNoise(9,3); -// df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3; -// -// // Compute expected F wrt acc noise -// Matrix df_daccNoise = -// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, -// deltaPij_old, deltaVij_old, logDeltaRij_old, -// _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); -// // Compute expected F wrt gyro noise -// Matrix df_domegaNoise = -// numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, -// deltaPij_old, deltaVij_old, logDeltaRij_old, -// newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); -// Matrix Gexpected(9,9); -// -// Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise; -// EXPECT(assert_equal(Gexpected, Gactual)); + + // Compute expected G wrt integration noise + Matrix df_dintNoise(15,3); + df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; + + // Compute expected F wrt acc noise (15,3) + Matrix df_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + // Compute expected F wrt gyro noise (15,3) + Matrix df_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + // Compute expected F wrt bias random walk noise (15,6) + Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries + df_rwBias.setZero(); + df_rwBias.block<6,6>(9,0) = eye(6); + + // Compute expected F wrt gyro noise (15,3) + Matrix df_dinitBias = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, logDeltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + Matrix Gexpected(15,21); + Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; + + EXPECT(assert_equal(Gexpected, Gactual)); } /* ************************************************************************* */ From d22868d524b38b23dc664b1b2ad1fa15e4d911e4 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 15:55:24 -0500 Subject: [PATCH 39/65] removed multiple definitions of trivial matrices (eye,zero) --- gtsam/navigation/CombinedImuFactor.cpp | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 4d2fe14ea..e1afe4164 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -103,19 +103,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; // analytic expression corresponding to the following numerical derivative // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; // analytic expression corresponding to the following numerical derivative @@ -123,11 +115,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() From 013c8a4cefc9af2c09b99c82c5dc2098ab9003c0 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:05:39 -0500 Subject: [PATCH 40/65] added extra test on covariance propagation --- gtsam/navigation/tests/testCombinedImuFactor.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index bb6828216..4c4aeb7c7 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -352,6 +352,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, Factual, Gactual); @@ -412,6 +414,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Fexpected * oldPreintCovariance * Fexpected.transpose() + + (1/newDeltaT) * Gexpected * Gexpected.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual, 1e-7)); } /* ************************************************************************* */ From 5f17e1fb986c9dc8b2ad596d2d8da30cc02b6854 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:34:43 -0500 Subject: [PATCH 41/65] fixed noise propagation. Luca&Christian: insight is that preintegration noise acts on rotations as R * expmap(noise), while before it was expmap( logmap(R) + noise) --- gtsam/navigation/CombinedImuFactor.cpp | 20 +-- .../tests/testCombinedImuFactor.cpp | 117 ++++++++++++------ 2 files changed, 86 insertions(+), 51 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index e1afe4164..ad32f97c1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -81,8 +81,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -92,26 +92,16 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - const Matrix3 R_i = deltaRij(); // store this // Update preintegrated measurements. TODO Frank moved from end of this function !!! updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); - const Vector3 theta_j = thetaRij(); // super-expensive parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - // Single Jacobians to propagate covariance - Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + Matrix3 H_angles_angles = Rincr.inverse().matrix(); + Matrix3 H_angles_biasomega =- Jr_theta_incr * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 4c4aeb7c7..d14eafb7d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -45,28 +45,39 @@ namespace { // covariance propagation during preintegration /* ************************************************************************* */ Vector updatePreintegratedMeasurementsTest( - const Vector3 deltaPij_old, const Vector3& deltaVij_old, - const Vector3& logDeltaRij_old, const imuBias::ConstantBias& bias_old, + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration_) { + const bool use2ndOrderIntegration) { - Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration_){ + if(!use2ndOrderIntegration){ deltaPij_new = deltaPij_old + deltaVij_old * deltaT; }else{ deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; } Vector3 deltaVij_new = deltaVij_old + temp; Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); - Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more imuBias::ConstantBias bias_new(bias_old); Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); return result; } +Rot3 updatePreintegratedMeasurementsRot( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration){ + + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, + bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); + + return Rot3::Expmap(result.segment<3>(6)); +} + // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ @@ -347,7 +358,6 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const double newDeltaT = 0.01; - const Vector3 logDeltaRij_old = preintegrated.thetaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement @@ -360,67 +370,102 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) bool use2ndOrderIntegration = false; + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected F wrt positions (15,3) Matrix df_dpos = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, logDeltaRij_old, bias_old, + _1, deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // rotation part has to be done properly, on manifold + df_dpos.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + _1, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // Compute expected F wrt velocities (15,3) Matrix df_dvel = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, logDeltaRij_old, bias_old, + deltaPij_old, _1, deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); - // Compute expected F wrt angles (15,3) - Matrix df_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); - // Compute expected F wrt angles (15,6) - Matrix df_dbias = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); - Matrix Fexpected(15,15); + // rotation part has to be done properly, on manifold + df_dvel.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, _1, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // Compute expected F wrt angles (15,3) + Matrix df_dangle = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + // rotation part has to be done properly, on manifold + df_dangle.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + + // Compute expected F wrt biases (15,6) + Matrix df_dbias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dbias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + + Matrix Fexpected(15,15); Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; EXPECT(assert_equal(Fexpected, Factual)); + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected G wrt integration noise Matrix df_dintNoise(15,3); df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; - // Compute expected F wrt acc noise (15,3) - Matrix df_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old, + // Compute expected G wrt acc noise (15,3) + Matrix df_daccNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); - // Compute expected F wrt gyro noise (15,3) - Matrix df_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, bias_old, + // rotation part has to be done properly, on manifold + df_daccNoise.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected G wrt gyro noise (15,3) + Matrix df_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - // Compute expected F wrt bias random walk noise (15,6) + // rotation part has to be done properly, on manifold + df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + + // Compute expected G wrt bias random walk noise (15,6) Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries df_rwBias.setZero(); df_rwBias.block<6,6>(9,0) = eye(6); - // Compute expected F wrt gyro noise (15,3) - Matrix df_dinitBias = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, logDeltaRij_old, _1, + // Compute expected G wrt gyro noise (15,6) + Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dinitBias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + Matrix Gexpected(15,21); Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation - Matrix newPreintCovarianceExpected = Fexpected * oldPreintCovariance * Fexpected.transpose() + - (1/newDeltaT) * Gexpected * Gexpected.transpose(); + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual, 1e-7)); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ From b593a6a2d539fe441560356b260a48bd9e702b8b Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:41:29 -0500 Subject: [PATCH 42/65] removing redundant lines and using jacobian computation from base class --- gtsam/navigation/CombinedImuFactor.cpp | 31 +++++++++++++------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ad32f97c1..6ca1a7eb7 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -94,46 +94,45 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( /* ----------------------------------------------------------------------------------------------------------------------- */ const Matrix3 R_i = deltaRij(); // store this // Update preintegrated measurements. TODO Frank moved from end of this function !!! - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); + Matrix F_9x9; + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 H_vel_biasacc = - R_i * deltaT; - - Matrix3 H_angles_angles = Rincr.inverse().matrix(); Matrix3 H_angles_biasomega =- Jr_theta_incr * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); - F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, - Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + // for documentation: + // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, + // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, + // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + F.setZero(); + F.block<9,9>(0,0) = F_9x9; + F.block<6,6>(9,9) = eye(9); + F.block<3,3>(3,9) = H_vel_biasacc; + F.block<3,3>(6,12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() Matrix G_measCov_Gt = Matrix::Zero(15,15); + // BLOCK DIAGONAL TERMS G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); - G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9); - G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12); - - // NEW OFF BLOCK DIAGONAL TERMS + // OFF BLOCK DIAGONAL TERMS Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; // F_test and G_test are used for testing purposes and are not needed by the factor From 1e8402231c2115d70069c22bee3ba2a26b20b4ce Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:59:30 -0500 Subject: [PATCH 43/65] applied (to some extend) the naming convention proposed by Frank --- gtsam/navigation/AHRSFactor.cpp | 38 ++++++++++---------- gtsam/navigation/CombinedImuFactor.cpp | 10 +++--- gtsam/navigation/ImuFactor.cpp | 10 +++--- gtsam/navigation/PreintegratedRotation.h | 8 ++--- gtsam/navigation/PreintegrationBase.h | 44 ++++++++++++------------ 5 files changed, 55 insertions(+), 55 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 9c684658c..b85bf8e0b 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -83,11 +83,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; - Matrix3 Jr_theta_incr; - const Rot3 incrR = Rot3::Expmap(theta_incr, Jr_theta_incr); // expensive !! + Matrix3 D_Rincr_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! // Update Jacobian - update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT); + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); // Update rotation and deltaTij. Matrix3 Fr; // Jacobian of the update @@ -172,44 +172,44 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = _PIM_.predict(bias, H3); + const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); const Matrix3 coriolisHat = skewSymmetric(coriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; + const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); // Get error between actual and prediction const Rot3 actualRij = Ri.between(Rj); - const Rot3 fRhat = deltaRij_corrected.between(actualRij); - Vector3 fR = Rot3::Logmap(fRhat); + const Rot3 fRrot = correctedDeltaRij.between(actualRij); + Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR); if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat; + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; (*H1) - << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); + << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << Jrinv_fRhat * Matrix3::Identity(); + (*H2) << D_fR_fRrot * Matrix3::Identity(); } if (H3) { // dfR/dBias, note H3 contains derivative of predict - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); H3->resize(3, 3); - (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); } Vector error(3); @@ -222,16 +222,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); + const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term const Vector3 coriolis = // preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - return rot_i.compose(deltaRij_corrected); + return rot_i.compose(correctedDeltaRij); } } //namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 6ca1a7eb7..9aa4f080c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -80,13 +80,13 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we @@ -99,7 +99,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Single Jacobians to propagate covariance Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_biasomega =- Jr_theta_incr * deltaT; + Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 1aa261d34..06d00b842 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -73,12 +73,12 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 Jr_theta_incr; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(theta_incr, Jr_theta_incr); // rotation increment computed from the current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians - updatePreintegratedJacobians(correctedAcc, Jr_theta_incr, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements (also get Jacobian) const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test @@ -106,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, Jr_theta_incr * deltaT; // angle + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle // Propagation with no approximation: // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index d949ee21c..eb043fa79 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -84,10 +84,10 @@ public: * Update Jacobians to be used during preintegration * TODO: explain arguments */ - void update_delRdelBiasOmega(const Matrix3& Jr_theta_incr, const Rot3& incrR, + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT) { const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; } /// Return a bias corrected version of the integrated rotation - expensive @@ -104,11 +104,11 @@ public: if (H) { Matrix3 Jrinv_theta_bc; // This was done via an expmap, now we go *back* to so<3> - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - return theta_biascorrected; + return biascorrectedOmega; } //else return Rot3::Logmap(deltaRij_biascorrected); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 11db97642..ee4d239a2 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -144,7 +144,7 @@ public: /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& Jr_theta_incr, const Rot3& incrR, double deltaT){ + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ Matrix3 dRij = deltaRij(); // expensive Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); if (!use2ndOrderIntegration_) { @@ -156,7 +156,7 @@ public: } delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); + update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); } void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, @@ -211,12 +211,12 @@ public: const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_corrected = theta_biascorrected - + Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + Vector3 correctedOmega = biascorrectedOmega - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 deltaRij_corrected = - Rot3::Expmap( theta_corrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_corrected ); + const Rot3 correctedDeltaRij = + Rot3::Expmap( correctedOmega ); + const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant @@ -246,31 +246,31 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Vector3 theta_biascorrected = biascorrectedThetaRij(biasOmegaIncr, H5); + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 theta_corrected = theta_biascorrected - coriolis; + Vector3 correctedOmega = biascorrectedOmega - coriolis; - Rot3 deltaRij_corrected, fRhat; + Rot3 correctedDeltaRij, fRrot; Vector3 fR; // Accessory matrix, used to build the jacobians - Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; // This is done to save computation: we ask for the jacobians only when they are needed if(H1 || H2 || H3 || H4 || H5){ - deltaRij_corrected = Rot3::Expmap( theta_corrected, Jr_theta_bcc); + correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); // Residual rotation error - fRhat = deltaRij_corrected.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRhat, Jrinv_fRhat); - Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); }else{ - deltaRij_corrected = Rot3::Expmap( theta_corrected); + correctedDeltaRij = Rot3::Expmap( correctedOmega); // Residual rotation error - fRhat = deltaRij_corrected.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRhat); + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot); } if(H1) { @@ -298,7 +298,7 @@ public: // dfV/dPi dfVdPi, // dfR/dRi - Jrinv_fRhat * (- Rj.between(Ri).matrix() - fRhat.inverse().matrix() * Jtheta), + D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), // dfR/dPi Z_3x3; } @@ -322,7 +322,7 @@ public: // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; + D_fR_fRrot * ( I_3x3 ), Z_3x3; } if(H4) { H4->resize(9,3); @@ -336,7 +336,7 @@ public: } if(H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H5); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H5); H5->resize(9,6); (*H5) << // dfP/dBias @@ -347,7 +347,7 @@ public: - Ri.matrix() * delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } // Evaluate residual error, according to [3] From e82b815a48484f35e133efc1fdf44acf2b1e3c70 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 17:10:04 -0500 Subject: [PATCH 44/65] renamed right jacobian of expmap and logmap (removed "right", according to Frank's suggestion :-) --- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3.h | 11 ++++++----- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/Rot3Q.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 8 ++++---- gtsam/navigation/AHRSFactor.cpp | 4 ++-- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/tests/testAHRSFactor.cpp | 4 ++-- gtsam/navigation/tests/testImuFactor.cpp | 6 +++--- 9 files changed, 22 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070..43762fc54 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -195,7 +195,7 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { +Matrix3 Rot3::expmapDerivative(const Vector3& x) { // x is the axis-angle representation (exponential coordinates) for a rotation double normx = norm_2(x); // rotation angle Matrix3 Jr; @@ -211,7 +211,7 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { +Matrix3 Rot3::logmapDerivative(const Vector3& x) { // x is the axis-angle representation (exponential coordinates) for a rotation double normx = norm_2(x); // rotation angle Matrix3 Jrinv; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 87a035bfb..93a32302b 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -16,6 +16,7 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Luca Carlone */ // \callgraph @@ -297,7 +298,7 @@ namespace gtsam { static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3(v); + *H = Rot3::expmapDerivative(v); } if(zero(v)) return Rot3(); @@ -320,20 +321,20 @@ namespace gtsam { * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = rightJacobianExpMapSO3(thetahat); + * where Jr = expmapDerivative(thetahat); * This maps a perturbation in the tangent space (omega) to * a perturbation on the manifold (expmap(Jr * omega)) */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + static Matrix3 expmapDerivative(const Vector3& x); /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega - * where Jrinv = rightJacobianExpMapSO3inverse(omega); + * where Jrinv = logmapDerivative(omega); * This maps a perturbation on the manifold (expmap(omega)) * to a perturbation in the tangent space (Jrinv * omega) */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + static Matrix3 logmapDerivative(const Vector3& x); /// @} /// @name Group Action on Point3 diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46f5b8109..60e5a48e1 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -241,7 +241,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + *H = Rot3::logmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index e73ed3eaf..7fa7426cf 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -175,7 +175,7 @@ namespace gtsam { if(H){ H->resize(3,3); - *H = Rot3::rightJacobianExpMapSO3inverse(thetaR); + *H = Rot3::logmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 824878d13..f348c8d3c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -217,11 +217,11 @@ TEST(Rot3, log) /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, rightJacobianExpMapSO3 ) +TEST( Rot3, expmapDerivative ) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat); + Matrix Jactual = Rot3::expmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); } @@ -236,12 +236,12 @@ TEST( Rot3, jacobianExpmap ) } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +TEST( Rot3, logmapDerivative ) { Rot3 R = Rot3::Expmap(thetahat); // some rotation Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat); + Matrix3 Jactual = Rot3::logmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index b85bf8e0b..22c1c19ac 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -188,8 +188,8 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 D_cDeltaRij_cOmega = Rot3::rightJacobianExpMapSO3(correctedOmega); - const Matrix3 D_fR_fRrot = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::expmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(fR); if (H1) { // dfR/dRi diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index eb043fa79..019b35ed9 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -106,7 +106,7 @@ public: // This was done via an expmap, now we go *back* to so<3> const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_ * biasOmegaIncr); + Rot3::expmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; return biascorrectedOmega; } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..3605190d8 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::expmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::expmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 29a2a13de..c496cfb6a 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -340,7 +340,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -361,7 +361,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - Matrix3 actualDelFdeltheta = Rot3::rightJacobianExpMapSO3inverse(thetahat); + Matrix3 actualDelFdeltheta = Rot3::logmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); @@ -399,7 +399,7 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign From d809a952df6b352bc8fce7415146f216ec7937b4 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 18:03:54 -0500 Subject: [PATCH 45/65] started to include optionalJacobian: compiles after merge to develop and all unit tests pass --- gtsam/navigation/ImuFactor.cpp | 4 ++-- gtsam/navigation/ImuFactor.h | 2 +- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/PreintegrationBase.h | 14 ++++++++++++-- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 06d00b842..dfbbe9912 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -68,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor, - boost::optional F_test, boost::optional G_test) { + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); @@ -106,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle // Propagation with no approximation: // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index e0a6b861a..125c81040 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -115,7 +115,7 @@ public: */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - boost::optional Fout = boost::none, boost::optional Gout = boost::none); + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 019b35ed9..36bc9aabb 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -75,7 +75,7 @@ public: /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - boost::optional H = boost::none){ + OptionalJacobian<3, 3> H = boost::none){ deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaTij_ += deltaT; } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index ee4d239a2..e0c13f43d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -116,7 +116,7 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, boost::optional F = boost::none) { + const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { Matrix3 dRij = deltaRij(); // expensive Vector3 temp = dRij * correctedAcc * deltaT; @@ -127,9 +127,19 @@ public: } deltaVij_ += temp; +// const Matrix3 R_i = deltaRij(); +// OptionalJacobian<3,3> F_angles_angles; +// updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none); +// if(F){ +// Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; +// // pos vel angle +// *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos +// Z_3x3, I_3x3, F_vel_angles, // vel +// Z_3x3, Z_3x3, F_angles_angles;// angle +// } if(F){ - Matrix3 F_angles_angles; const Matrix3 R_i = deltaRij(); + Matrix3 F_angles_angles; updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; F->resize(9,9); From b96a463b10ccba5b0e663d6bba099790f1c6f54b Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 18:32:38 -0500 Subject: [PATCH 46/65] ok, fixed updatePreintegratedMeasurements with optionalJacobian --- gtsam/navigation/PreintegrationBase.h | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e0c13f43d..74b669c04 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -127,28 +127,16 @@ public: } deltaVij_ += temp; -// const Matrix3 R_i = deltaRij(); -// OptionalJacobian<3,3> F_angles_angles; -// updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none); -// if(F){ -// Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; -// // pos vel angle -// *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos -// Z_3x3, I_3x3, F_vel_angles, // vel -// Z_3x3, Z_3x3, F_angles_angles;// angle -// } + Matrix3 R_i; + if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + Matrix3 F_angles_angles; + updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); if(F){ - const Matrix3 R_i = deltaRij(); - Matrix3 F_angles_angles; - updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; - F->resize(9,9); // pos vel angle *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, Z_3x3, F_angles_angles;// angle - }else{ - updateIntegratedRotationAndDeltaT(incrR,deltaT); } } From 57d83be48aff4fef6b58f1ea96f6de17f9d4d891 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 19:05:14 -0500 Subject: [PATCH 47/65] matrix definition on same line --- gtsam/navigation/PreintegrationBase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 74b669c04..5f908a819 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -127,10 +127,10 @@ public: } deltaVij_ += temp; - Matrix3 R_i; + Matrix3 R_i, F_angles_angles; if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij - Matrix3 F_angles_angles; updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); + if(F){ Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; // pos vel angle From b3f0f3877c5928b7d9fe1d6ba60ec2c23f610a6b Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Dec 2014 16:16:29 -0500 Subject: [PATCH 48/65] capitalized ExpmapDerivative and LogmapDerivative --- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3.h | 10 +++++----- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/Rot3Q.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 8 ++++---- gtsam/navigation/AHRSFactor.cpp | 4 ++-- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/tests/testAHRSFactor.cpp | 4 ++-- gtsam/navigation/tests/testImuFactor.cpp | 6 +++--- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index dea76b244..bb6deafc9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -175,7 +175,7 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::expmapDerivative(const Vector3& x) { +Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { // x is the axis-angle representation (exponential coordinates) for a rotation double normx = norm_2(x); // rotation angle Matrix3 Jr; @@ -191,7 +191,7 @@ Matrix3 Rot3::expmapDerivative(const Vector3& x) { } /* ************************************************************************* */ -Matrix3 Rot3::logmapDerivative(const Vector3& x) { +Matrix3 Rot3::LogmapDerivative(const Vector3& x) { // x is the axis-angle representation (exponential coordinates) for a rotation double normx = norm_2(x); // rotation angle Matrix3 Jrinv; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7a8c8b236..86a3f127f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -291,7 +291,7 @@ namespace gtsam { static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { if(H){ H->resize(3,3); - *H = Rot3::expmapDerivative(v); + *H = Rot3::ExpmapDerivative(v); } if(zero(v)) return Rot3(); @@ -314,20 +314,20 @@ namespace gtsam { * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = expmapDerivative(thetahat); + * where Jr = ExpmapDerivative(thetahat); * This maps a perturbation in the tangent space (omega) to * a perturbation on the manifold (expmap(Jr * omega)) */ - static Matrix3 expmapDerivative(const Vector3& x); + static Matrix3 ExpmapDerivative(const Vector3& x); /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega - * where Jrinv = logmapDerivative(omega); + * where Jrinv = LogmapDerivative(omega); * This maps a perturbation on the manifold (expmap(omega)) * to a perturbation in the tangent space (Jrinv * omega) */ - static Matrix3 logmapDerivative(const Vector3& x); + static Matrix3 LogmapDerivative(const Vector3& x); /// @} /// @name Group Action on Point3 diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 9169c46ba..2e21929f6 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -225,7 +225,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { if(H){ H->resize(3,3); - *H = Rot3::logmapDerivative(thetaR); + *H = Rot3::LogmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index dc50a9687..2c56a2e0b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -162,7 +162,7 @@ namespace gtsam { if(H){ H->resize(3,3); - *H = Rot3::logmapDerivative(thetaR); + *H = Rot3::LogmapDerivative(thetaR); } return thetaR; } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 08f7681a8..6453063b5 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -216,11 +216,11 @@ TEST(Rot3, log) /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, expmapDerivative ) +TEST( Rot3, ExpmapDerivative ) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix Jactual = Rot3::expmapDerivative(thetahat); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); } @@ -235,12 +235,12 @@ TEST( Rot3, jacobianExpmap ) } /* ************************************************************************* */ -TEST( Rot3, logmapDerivative ) +TEST( Rot3, LogmapDerivative ) { Rot3 R = Rot3::Expmap(thetahat); // some rotation Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::logmapDerivative(thetahat); + Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 22c1c19ac..2bf49d9df 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -188,8 +188,8 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 D_cDeltaRij_cOmega = Rot3::expmapDerivative(correctedOmega); - const Matrix3 D_fR_fRrot = Rot3::logmapDerivative(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 36bc9aabb..c576acc38 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -106,7 +106,7 @@ public: // This was done via an expmap, now we go *back* to so<3> const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::expmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); + Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; return biascorrectedOmega; } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 3605190d8..99952f99e 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::expmapDerivative( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::expmapDerivative( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c496cfb6a..600bb4e11 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -340,7 +340,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -361,7 +361,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - Matrix3 actualDelFdeltheta = Rot3::logmapDerivative(thetahat); + Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); @@ -399,7 +399,7 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::expmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign From 36358308fdf7feb628cc2e359c195a20c890fd83 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Dec 2014 16:56:21 -0500 Subject: [PATCH 49/65] minor code optimization in ExpmapDerivative --- gtsam/geometry/Rot3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index bb6deafc9..3f681a433 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -183,9 +183,9 @@ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { Jr = I_3x3; } else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx + Jr = I_3x3 - ((1-cos(normx))/(normx)) * X + + (1 -sin(normx)/normx) * X * X; // right Jacobian } return Jr; } From 1dfd9d2ae7bb21182d0a82b590aa09fc5e121320 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Dec 2014 16:56:31 -0500 Subject: [PATCH 50/65] cleaned up unit test --- gtsam/navigation/tests/testImuFactor.cpp | 185 ++++++++++------------- 1 file changed, 78 insertions(+), 107 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 600bb4e11..36571ab43 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -185,7 +185,7 @@ TEST( ImuFactor, PreintegratedMeasurements ) } /* ************************************************************************* */ -TEST( ImuFactor, Error ) +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias @@ -213,6 +213,77 @@ TEST( ImuFactor, Error ) Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + // Expected Jacobians + /////////////////// H1 /////////////////////////// + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); + EXPECT(assert_equal(H1e, H1a)); + + /////////////////// H2 /////////////////////////// + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + EXPECT(assert_equal(H2e, H2a)); + + /////////////////// H3 /////////////////////////// + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); + EXPECT(assert_equal(H3e, H3a)); + + /////////////////// H4 /////////////////////////// + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + EXPECT(assert_equal(H4e, H4a)); + + /////////////////// H5 /////////////////////////// + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorAndJacobianWithBiases ) +{ + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); @@ -230,104 +301,22 @@ TEST( ImuFactor, Error ) boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // positions and velocities - Matrix H1etop6 = H1e.topRows(6); - Matrix H1atop6 = H1a.topRows(6); - EXPECT(assert_equal(H1etop6, H1atop6)); - // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H2e, H2a)); - - // positions and velocities - Matrix H3etop6 = H3e.topRows(6); - Matrix H3atop6 = H3a.topRows(6); - EXPECT(assert_equal(H3etop6, H3atop6)); - // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H4e, H4a)); - // EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiases ) -{ - // Linearization point -// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) -// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1(Vector3(0.5, 0.0, 0.0)); -// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); - double deltaT = 1.0; - - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); - // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); - - // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); -} - -/* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeExpmap ) +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias @@ -367,24 +356,6 @@ TEST( ImuFactor, PartialDerivativeLogmap ) EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); } -Rot3 constRot = Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0); -Rot3 testRot(const Rot3& Rk){ - return Rk * constRot; -} -/* ************************************************************************* */ -TEST( ImuFactor, understandRot ) -{ - Rot3 Rbar = Rot3::RzRyRx( M_PI, M_PI/6.0, -M_PI/4.0 ); - - Matrix Jexpected = numericalDerivative11(boost::bind( - &testRot, _1), Rbar); - - Matrix3 Jactual = constRot.transpose(); - - // Compare Jacobians - EXPECT(assert_equal(Jexpected, Jactual)); -} - /* ************************************************************************* */ TEST( ImuFactor, fistOrderExponential ) { From 9dbca87c86a3ef748a52ea61a41c5b0289768d10 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Dec 2014 20:51:26 -0500 Subject: [PATCH 51/65] fixed evaluate error to compy with additive Gaussian noise model. Still to be optimized, but unit tests pass :-) --- gtsam/navigation/PreintegrationBase.h | 50 +++++++++++++-------------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 5f908a819..3a2b78754 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -237,6 +237,7 @@ public: // we give some shorter name to rotations and translations const Rot3& Ri = pose_i.rotation(); const Rot3& Rj = pose_j.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_j = pose_j.translation().vector(); // Jacobian computation @@ -274,25 +275,23 @@ public: if(H1) { H1->resize(9,6); - Matrix3 dfPdPi; - Matrix3 dfVdPi; + Matrix3 dfPdPi = -I_3x3; + Matrix3 dfVdPi = Z_3x3; if(use2ndOrderCoriolis){ - dfPdPi = - Ri.matrix() + 0.5 * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); - dfVdPi = omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); - } - else{ - dfPdPi = - Ri.matrix(); - dfVdPi = Z_3x3; + dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); + dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); } (*H1) << // dfP/dRi - Ri.matrix() * skewSymmetric(deltaPij() - + delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), + skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij() + + omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity *deltaTij()*deltaTij())), // dfP/dPi dfPdPi, // dfV/dRi - Ri.matrix() * skewSymmetric(deltaVij() - + delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr), + skewSymmetric( Ri.inverse().matrix() * + (vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + ) ), // dfV/dPi dfVdPi, // dfR/dRi @@ -304,11 +303,11 @@ public: H2->resize(9,3); (*H2) << // dfP/dVi - - I_3x3 * deltaTij() - + omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + - Ri.transpose() * deltaTij() + + Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - - I_3x3 - + 2 * omegaCoriolisHat * deltaTij(), // Coriolis term + - Ri.transpose() + + 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } @@ -316,7 +315,7 @@ public: H3->resize(9,6); (*H3) << // dfP/dPosej - Z_3x3, Rj.matrix(), + Z_3x3, Ri.transpose() * Rj.matrix(), // dfV/dPosej Matrix::Zero(3,6), // dfR/dPosej @@ -328,7 +327,7 @@ public: // dfP/dVj Z_3x3, // dfV/dVj - I_3x3, + Ri.transpose(), // dfR/dVj Z_3x3; } @@ -338,14 +337,11 @@ public: H5->resize(9,6); (*H5) << // dfP/dBias - - Ri.matrix() * delPdelBiasAcc(), - - Ri.matrix() * delPdelBiasOmega(), + - delPdelBiasAcc(), - delPdelBiasOmega(), // dfV/dBias - - Ri.matrix() * delVdelBiasAcc(), - - Ri.matrix() * delVdelBiasOmega(), + - delVdelBiasAcc(), - delVdelBiasOmega(), // dfR/dBias - Matrix::Zero(3,3), - D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); + Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } // Evaluate residual error, according to [3] @@ -353,9 +349,11 @@ public: PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis); - const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); - const Vector3 fv = vel_j - predictedState_j.velocity; + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) From c4fafd9268ff37390066429b881b0d2f952a4f6e Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 11:06:47 -0500 Subject: [PATCH 52/65] added unit test, improved computation --- gtsam/navigation/PreintegrationBase.h | 73 ++++++++++++------------ gtsam/navigation/tests/testImuFactor.cpp | 65 +++++++++++++++++++++ 2 files changed, 102 insertions(+), 36 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 3a2b78754..5a4e300e8 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -178,7 +178,9 @@ public: //------------------------------------------------------------------------------ PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const { + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) const { const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); @@ -188,16 +190,20 @@ public: // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() - + delPdelBiasAcc() * biasAccIncr - + delPdelBiasOmega() * biasOmegaIncr) + Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; + if(deltaPij_biascorrected_out)// if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected + vel_i * deltaTij() - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij()*deltaTij(); - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() - + delVdelBiasAcc() * biasAccIncr - + delVdelBiasOmega() * biasOmegaIncr) + Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; + if(deltaVij_biascorrected_out)// if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + gravity * deltaTij()); @@ -231,15 +237,28 @@ public: boost::optional H5) const { // We need the mismatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); // we give some shorter name to rotations and translations const Rot3& Ri = pose_i.rotation(); const Rot3& Rj = pose_j.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_j = pose_j.translation().vector(); + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + // Jacobian computation /* ---------------------------------------------------------------------------------------------------- */ // Get Get so<3> version of bias corrected rotation @@ -248,7 +267,7 @@ public: Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis); + const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); Vector3 correctedOmega = biascorrectedOmega - coriolis; @@ -271,27 +290,23 @@ public: fRrot = correctedDeltaRij.between(Ri.between(Rj)); fR = Rot3::Logmap(fRrot); } - if(H1) { H1->resize(9,6); - Matrix3 dfPdPi = -I_3x3; Matrix3 dfVdPi = Z_3x3; if(use2ndOrderCoriolis){ - dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); - dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); + dfVdPi += temp * deltaTij(); } (*H1) << // dfP/dRi - skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij() - + omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity *deltaTij()*deltaTij())), + skewSymmetric(fp + deltaPij_biascorrected), // dfP/dPi dfPdPi, // dfV/dRi - skewSymmetric( Ri.inverse().matrix() * - (vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - ) ), + skewSymmetric(fv + deltaVij_biascorrected), // dfV/dPi dfVdPi, // dfR/dRi @@ -304,10 +319,10 @@ public: (*H2) << // dfP/dVi - Ri.transpose() * deltaTij() - + Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Ri.transpose() - + 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term + + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term // dfR/dVi Z_3x3; } @@ -343,20 +358,6 @@ public: // dfR/dBias Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); - - // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) - Vector r(9); r << fp, fv, fR; return r; } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 36571ab43..ce7689b82 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -315,6 +315,71 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) EXPECT(assert_equal(H5e, H5a)); } +/* ************************************************************************* */ +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) +{ + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); +} + /* ************************************************************************* */ TEST( ImuFactor, PartialDerivative_wrt_Bias ) { From af04b834b918279d7116635ee1f919657884914b Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 11:28:43 -0500 Subject: [PATCH 53/65] using fixed-size matrix/vector when possible --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 4 +--- gtsam/navigation/PreintegratedRotation.h | 2 +- gtsam/navigation/PreintegrationBase.h | 21 ++++++++++++--------- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 9aa4f080c..55caa32ee 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -94,7 +94,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( /* ----------------------------------------------------------------------------------------------------------------------- */ const Matrix3 R_i = deltaRij(); // store this // Update preintegrated measurements. TODO Frank moved from end of this function !!! - Matrix F_9x9; + Matrix9 F_9x9; updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index dfbbe9912..a3d5e4c69 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -82,7 +82,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements (also get Jacobian) const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test - Matrix F; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); // first order covariance propagation: @@ -96,13 +96,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // F_test and G_test are used for testing purposes and are not needed by the factor if(F_test){ // This in only for testing - F_test->resize(9,9); (*F_test) << F; } if(G_test){ // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // This in only for testing & documentation - G_test->resize(9,9); // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index c576acc38..544c8b79b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -97,7 +97,7 @@ public: /// Get so<3> version of bias corrected rotation, with optional Jacobian Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, - boost::optional H) const { + OptionalJacobian<3, 3> H = boost::none) const { // First, we correct deltaRij using the biasOmegaIncr, rotated const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 5a4e300e8..f007333e3 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -77,7 +77,7 @@ public: const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaVij() const {return deltaVij_;} const imuBias::ConstantBias& biasHat() const { return biasHat_;} - Vector biasHatVector() const { return biasHat_.vector();} // expensive + Vector6 biasHatVector() const { return biasHat_.vector();} // expensive const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} @@ -228,13 +228,15 @@ public: /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j //------------------------------------------------------------------------------ - Vector computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5) const { + OptionalJacobian<9, 6> H1 = boost::none, + OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const { // We need the mismatch w.r.t. the biases used for preintegration // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary @@ -264,7 +266,8 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5); + Matrix3 H5temp; + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); @@ -278,7 +281,7 @@ public: Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; // This is done to save computation: we ask for the jacobians only when they are needed - if(H1 || H2 || H3 || H4 || H5){ + if(H1 || H2 || H3 || H4 || H5){ correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); // Residual rotation error fRrot = correctedDeltaRij.between(Ri.between(Rj)); @@ -348,7 +351,7 @@ public: } if(H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H5); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * H5temp; H5->resize(9,6); (*H5) << // dfP/dBias @@ -358,7 +361,7 @@ public: // dfR/dBias Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); } - Vector r(9); r << fp, fv, fR; + Vector9 r; r << fp, fv, fR; return r; } From 295fd7385aca82efc33970360763596408bbb860 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 12:20:54 -0500 Subject: [PATCH 54/65] minor renaming --- gtsam/navigation/PreintegratedRotation.h | 1 + gtsam/navigation/PreintegrationBase.h | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 544c8b79b..aee6b6738 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -96,6 +96,7 @@ public: } /// Get so<3> version of bias corrected rotation, with optional Jacobian + // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, OptionalJacobian<3, 3> H = boost::none) const { // First, we correct deltaRij using the biasOmegaIncr, rotated diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f007333e3..a38743c0e 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -266,8 +266,8 @@ public: // Get Get so<3> version of bias corrected rotation // If H5 is asked for, we will need the Jacobian, which we store in H5 // H5 will then be corrected below to take into account the Coriolis effect - Matrix3 H5temp; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0); + Matrix3 D_cThetaRij_biasOmegaIncr; + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); @@ -351,7 +351,7 @@ public: } if(H5) { // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * H5temp; + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; H5->resize(9,6); (*H5) << // dfP/dBias From 3c840e1bf3f12f3df592edef9cf4ed1e5173d646 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 12:23:18 -0500 Subject: [PATCH 55/65] fixed matlab wrapper --- gtsam.h | 35 ++++++++++++++++------------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/gtsam.h b/gtsam.h index d40d02ae2..63345d289 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2400,28 +2400,30 @@ class ImuFactorPreintegratedMeasurements { // Standard Constructor ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; + Matrix measurementCovariance() const; // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, + const gtsam::Vector3& gravity, const gtsam::Vector3& omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { @@ -2432,9 +2434,6 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis) const; }; #include @@ -2457,39 +2456,37 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasOmegaCovariance, Matrix biasAccOmegaInit, bool use2ndOrderIntegration); - CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; + Matrix measurementCovariance() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, + const gtsam::Vector3& gravity, const gtsam::Vector3& omegaCoriolis) const; }; virtual class CombinedImuFactor : gtsam::NonlinearFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis); }; #include From dcc028518fe8e937447ea7036579726b339ef778 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 15:46:33 -0500 Subject: [PATCH 56/65] added unit test and removed approximation in covariance propagation --- gtsam/navigation/ImuFactor.cpp | 22 ++++++++++----------- gtsam/navigation/tests/testImuFactor.cpp | 25 ++++++++++++++++++------ 2 files changed, 30 insertions(+), 17 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index a3d5e4c69..2d8b94618 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -88,25 +88,25 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ - // the deltaT allows to pass from continuous time noise to discrete time noise + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, + // as G and measurementCovariance are blockdiagonal matrices + preintMeasCov_ = F * preintMeasCov_ * F.transpose(); + preintMeasCov_.block<3,3>(0,0) += measurementCovariance_.block<3,3>(0,0) * deltaT; + preintMeasCov_.block<3,3>(3,3) += R_i * measurementCovariance_.block<3,3>(3,3) * R_i.transpose() * deltaT; + preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * measurementCovariance_.block<3,3>(6,6) * D_Rincr_integratedOmega.transpose() * deltaT; - // F_test and G_test are used for testing purposes and are not needed by the factor - if(F_test){ - // This in only for testing + // F_test and G_test are given as output for testing purposes and are not needed by the factor + if(F_test){ // This in only for testing (*F_test) << F; } - if(G_test){ - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only for testing & documentation + if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos Z_3x3, R_i * deltaT, Z_3x3, // vel Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle - // Propagation with no approximation: - // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); } } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ce7689b82..31fb86376 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -538,6 +538,8 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, Factual, Gactual); @@ -552,20 +554,20 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) numericalDerivative11(boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // Compute expected f_pos_vel wrt velocities Matrix dfpv_dvel = numericalDerivative11(boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // Compute expected f_pos_vel wrt angles Matrix dfpv_dangle = numericalDerivative11(boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); - Matrix FexpectedTop6(6,9); - FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - EXPECT(assert_equal(FexpectedTop6, Factual.topRows(6))); + Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles Matrix dfr_dangle = @@ -574,7 +576,9 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) Matrix FexpectedBottom3(3,9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - EXPECT(assert_equal(FexpectedBottom3, Factual.bottomRows(3))); + Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + + EXPECT(assert_equal(Fexpected, Factual)); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR G @@ -588,6 +592,7 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) numericalDerivative11(boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, deltaRij_old, _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + // Compute expected F wrt gyro noise Matrix dgpv_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedPosVel, @@ -595,7 +600,6 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); Matrix GexpectedTop6(6,9); GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; - EXPECT(assert_equal(GexpectedTop6, Gactual.topRows(6))); // Compute expected f_rot wrt gyro noise Matrix dgr_dangle = @@ -604,7 +608,16 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) Matrix GexpectedBottom3(3,9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - EXPECT(assert_equal(GexpectedBottom3, Gactual.bottomRows(3))); + Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } //#include From d9a7f516ef2cbbfcfb84fe8c2cb0aa3f55b08e05 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 19:34:20 -0500 Subject: [PATCH 57/65] making test more complicated --- gtsam/navigation/tests/testImuFactor.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 31fb86376..0c4c9e3a6 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -82,14 +82,17 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), + omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity()); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -613,8 +616,13 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) EXPECT(assert_equal(Gexpected, Gactual)); // Check covariance propagation + Matrix9 measurementCovariance; + measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, + Z_3x3, accNoiseVar*I_3x3, Z_3x3, + Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + - (1/newDeltaT) * Gactual * Gactual.transpose(); + (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); From c4bd02c3fabf320e54297e5572c1b8bcc2d0c2b6 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Dec 2014 19:54:42 -0500 Subject: [PATCH 58/65] split of measurement covariance into diagonal blocks. --- gtsam.h | 3 --- gtsam/navigation/AHRSFactor.cpp | 15 +++++------ gtsam/navigation/AHRSFactor.h | 6 ----- gtsam/navigation/CombinedImuFactor.cpp | 32 ++++++++++++------------ gtsam/navigation/CombinedImuFactor.h | 7 +++--- gtsam/navigation/ImuFactor.cpp | 18 ++++++------- gtsam/navigation/ImuFactor.h | 5 ---- gtsam/navigation/PreintegratedRotation.h | 12 ++++++--- gtsam/navigation/PreintegrationBase.h | 22 +++++++++++++--- 9 files changed, 60 insertions(+), 60 deletions(-) diff --git a/gtsam.h b/gtsam.h index 63345d289..f0ad16096 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2417,7 +2417,6 @@ class ImuFactorPreintegratedMeasurements { Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - Matrix measurementCovariance() const; // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); @@ -2473,7 +2472,6 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - Matrix measurementCovariance() const; // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); @@ -2501,7 +2499,6 @@ class AHRSFactorPreintegratedMeasurements { bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); // get Data - Matrix measurementCovariance() const; Matrix deltaRij() const; double deltaTij() const; Vector biasHat() const; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2bf49d9df..68c0afccb 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -29,15 +29,15 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias) { - measurementCovariance_ << measuredOmegaCovariance; + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()) { - measurementCovariance_.setZero(); + PreintegratedRotation(I_3x3), biasHat_(Vector3()) +{ preintMeasCov_.setZero(); } @@ -45,7 +45,6 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { PreintegratedRotation::print(s); cout << "biasHat [" << biasHat_.transpose() << "]" << endl; - cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl; cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } @@ -53,9 +52,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { return PreintegratedRotation::equals(other, tol) - && equal_with_abs_tol(biasHat_, other.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol); + && equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ @@ -96,7 +93,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() - + measurementCovariance_ * deltaT; + + gyroscopeCovariance() * deltaT; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index d6a6c8dd9..fd4b9be87 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -42,8 +42,6 @@ public: protected: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -59,9 +57,6 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } Vector3 biasHat() const { return biasHat_; } @@ -105,7 +100,6 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); } }; diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 55caa32ee..914bbe1ca 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -36,28 +36,28 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, use2ndOrderIntegration) + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), + biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), + biasAccOmegaInit_(biasAccOmegaInit) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; - measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; - measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ PreintegrationBase::print(s); - cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; + cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; + cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; + cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) + &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } @@ -120,17 +120,17 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Matrix G_measCov_Gt = Matrix::Zero(15,15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); + G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * (H_vel_biasacc.transpose()); G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * measurementCovariance_.block<3,3>(9,9); - G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * measurementCovariance_.block<3,3>(12,12); + G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f7041fdeb..a7c6ecd39 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -82,8 +82,9 @@ public: protected: - Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] @@ -130,7 +131,6 @@ public: boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -140,7 +140,6 @@ public: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 2d8b94618..0aaa0122e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -35,26 +35,22 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, use2ndOrderIntegration) + PreintegrationBase(bias, + measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } @@ -94,9 +90,9 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, // as G and measurementCovariance are blockdiagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3,3>(0,0) += measurementCovariance_.block<3,3>(0,0) * deltaT; - preintMeasCov_.block<3,3>(3,3) += R_i * measurementCovariance_.block<3,3>(3,3) * R_i.transpose() * deltaT; - preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * measurementCovariance_.block<3,3>(6,6) * D_Rincr_integratedOmega.transpose() * deltaT; + preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; + preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; // F_test and G_test are given as output for testing purposes and are not needed by the factor if(F_test){ // This in only for testing diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 125c81040..b91c76ade 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -75,9 +75,6 @@ public: protected: - Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of - ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). @@ -118,7 +115,6 @@ public: OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -128,7 +124,6 @@ public: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index aee6b6738..731e11f2e 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -37,33 +37,39 @@ class PreintegratedRotation { /// Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements public: /** * Default constructor, initializes the variables in the base class */ - PreintegratedRotation() : + PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : deltaRij_(Rot3()), deltaTij_(0.0), - delRdelBiasOmega_(Z_3x3) {} + delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} /// methods to access class variables Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive Vector3 thetaRij(boost::optional H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive const double& deltaTij() const{return deltaTij_;} const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; + std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; deltaRij_.print(" deltaRij "); + std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; + std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; } /// Needed for testable bool equals(const PreintegratedRotation& expected, double tol) const { return deltaRij_.equals(expected.deltaRij_, tol) && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) + && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); } /// Re-initialize PreintegratedMeasurements diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index a38743c0e..8214c1930 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -59,6 +59,10 @@ class PreintegrationBase : public PreintegratedRotation { Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + public: /** @@ -67,11 +71,16 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - PreintegrationBase(const imuBias::ConstantBias& bias, const bool use2ndOrderIntegration) : + PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3) {} + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) {} /// methods to access class variables const Vector3& deltaPij() const {return deltaPij_;} @@ -83,9 +92,14 @@ public: const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} + const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} + const Matrix3& integrationCovariance() const { return integrationCovariance_;} + /// Needed for testable void print(const std::string& s) const { PreintegratedRotation::print(s); + std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; + std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; biasHat_.print(" biasHat"); @@ -100,7 +114,9 @@ public: && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol); + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); } /// Re-initialize PreintegratedMeasurements From d7ba38e476f3a196f68c65894e6bc6e12f90d19b Mon Sep 17 00:00:00 2001 From: krunalchande Date: Fri, 19 Dec 2014 12:56:41 -0500 Subject: [PATCH 59/65] fixed testAHRSFactor in debug mode. --- gtsam/navigation/tests/testAHRSFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 99952f99e..0d7626b95 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias; // Bias + Vector3 bias(0.,0.,0.); // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); From 6120bf08465064eba3864cec69945076c33e73e0 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Fri, 19 Dec 2014 14:15:14 -0500 Subject: [PATCH 60/65] Fixed matrix block index of jacobians in CombinedIMUFactor. --- gtsam/navigation/CombinedImuFactor.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 914bbe1ca..385a00993 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -111,7 +111,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; F.setZero(); F.block<9,9>(0,0) = F_9x9; - F.block<6,6>(9,9) = eye(9); + F.block<6,6>(9,9) = eye(6); F.block<3,3>(3,9) = H_vel_biasacc; F.block<3,3>(6,12) = H_angles_biasomega; @@ -119,7 +119,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS +// BLOCK DIAGONAL TERMS G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * @@ -219,37 +219,37 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ H1->resize(15,6); H1->block<9,6>(0,0) = H1_pvR; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block<6,6>(0,9) = Matrix::Zero(6,6); + H1->block<6,6>(9,0) = Matrix::Zero(6,6); } if(H2) { H2->resize(15,3); H2->block<9,3>(0,0) = H2_pvR; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block<6,3>(0,9) = Matrix::Zero(6,3); + H2->block<6,3>(9,0) = Matrix::Zero(6,3); } if(H3) { H3->resize(15,6); H3->block<9,6>(0,0) = H3_pvR; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block<6,6>(0,9) = Matrix::Zero(6,6); + H3->block<6,6>(9,0) = Matrix::Zero(6,6); } if(H4) { H4->resize(15,3); H4->block<9,3>(0,0) = H4_pvR; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block<6,3>(0,9) = Matrix::Zero(6,3); + H4->block<6,3>(9,0) = Matrix::Zero(6,3); } if(H5) { H5->resize(15,6); H5->block<9,6>(0,0) = H5_pvR; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block<6,6>(0,9) = Hbias_i; + H5->block<6,6>(9,0) = Hbias_i; } if(H6) { H6->resize(15,6); - H6->block<9,6>(0,0) = Matrix::Zero(6,6); + H6->block<9,6>(0,0) = Matrix::Zero(9,6); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block<6,6>(0,9) = Hbias_j; + H6->block<6,6>(9,0) = Hbias_j; } Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; From d3c8d348c55731e83bd82d1cceb9e3c0aca6e92f Mon Sep 17 00:00:00 2001 From: krunalchande Date: Tue, 23 Dec 2014 11:32:02 -0500 Subject: [PATCH 61/65] Changed dynamic allocations of ones, zeros and identity matrices to static. --- gtsam/navigation/AHRSFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 68c0afccb..2f03d72a4 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -122,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - _PIM_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Z_3x3) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 385a00993..fd761388a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -111,7 +111,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; F.setZero(); F.block<9,9>(0,0) = F_9x9; - F.block<6,6>(9,9) = eye(6); + F.block<6,6>(9,9) = I_6x6; F.block<3,3>(3,9) = H_vel_biasacc; F.block<3,3>(6,12) = H_angles_biasomega; @@ -156,7 +156,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, @@ -219,7 +219,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ H1->resize(15,6); H1->block<9,6>(0,0) = H1_pvR; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block<6,6>(9,0) = Matrix::Zero(6,6); + H1->block<6,6>(9,0) = Z_6x6; } if(H2) { H2->resize(15,3); @@ -231,7 +231,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ H3->resize(15,6); H3->block<9,6>(0,0) = H3_pvR; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block<6,6>(9,0) = Matrix::Zero(6,6); + H3->block<6,6>(9,0) = Z_6x6; } if(H4) { H4->resize(15,3); From d9f5681dc2c89398408d13be92c55a277f121c68 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 24 Dec 2014 11:34:53 +0100 Subject: [PATCH 62/65] Fixed warning --- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 05260521e..4691da384 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -45,7 +45,7 @@ static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, static double rankTol = 1.0; static double linThreshold = -1.0; -static bool manageDegeneracy = true; +// static bool manageDegeneracy = true; // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(3)); From 2f31500170283e43387cd8e14829e86f27a76c0b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 24 Dec 2014 11:35:17 +0100 Subject: [PATCH 63/65] Removed print/equals testing (not its place, spurious output) --- gtsam/base/chartTesting.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index d2f453521..b1b47f020 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -39,12 +39,6 @@ void testDefaultChart(TestResult& result_, BOOST_CONCEPT_ASSERT((ChartConcept)); T other = value; - // Check for the existence of a print function. - gtsam::traits::print()(value, "value"); - gtsam::traits::print()(other, "other"); - - // Check for the existence of "equals" - EXPECT(gtsam::traits::equals()(value, other, 1e-12)); // Check that the dimension of the local value matches the chart dimension. Vector dx = Chart::local(value, other); From 2ffa9dc6d26bb91aef2c71b75f1bb0883620323b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 24 Dec 2014 11:36:06 +0100 Subject: [PATCH 64/65] Renamed old Rot3 methods --- gtsam/navigation/AHRSFactor.cpp | 14 +++++++------- gtsam/navigation/CombinedImuFactor.cpp | 14 +++++++------- gtsam/navigation/ImuFactor.cpp | 14 +++++++------- gtsam/navigation/tests/testAHRSFactor.cpp | 4 ++-- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- 5 files changed, 25 insertions(+), 25 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..195490e87 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -99,7 +99,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Matrix3 incrRt = incrR.transpose(); // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Update Jacobians // --------------------------------------------------------------------------- @@ -108,11 +108,11 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // Update preintegrated measurements covariance // --------------------------------------------------------------------------- const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); + const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * incrR; const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF framework @@ -145,9 +145,9 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); if (H) { const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + Rot3::LogmapDerivative(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); + Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; } return theta_biascorrected; @@ -238,8 +238,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, Vector3 fR = Rot3::Logmap(fRhat); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2ce631ea..10f12ec66 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -116,7 +116,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -138,11 +138,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); // Single Jacobians to propagate covariance Matrix3 H_pos_pos = I_3x3; @@ -293,11 +293,11 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); if(H1) { H1->resize(15,6); @@ -382,8 +382,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; H5->resize(15,6); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9a9cc9d62..cdebffa63 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -113,7 +113,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -133,11 +133,11 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); Rot3 Rot_j = deltaRij_ * Rincr; const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); Matrix H_pos_pos = I_3x3; Matrix H_pos_vel = I_3x3 * deltaT; @@ -275,11 +275,11 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); if(H1) { H1->resize(9,6); @@ -348,8 +348,8 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const } if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; H5->resize(9,6); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..99952f99e 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index cf81c32ae..06e2c105e 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -307,7 +307,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -355,7 +355,7 @@ TEST( ImuFactor, fistOrderExponential ) Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign From e0a767e7fddca33fe65eba7f6a54d385db77fd43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 24 Dec 2014 12:25:53 +0100 Subject: [PATCH 65/65] Renamed all dexpL/dexpInvL, merged Luca/Duy versions in Rot3 --- gtsam/base/LieScalar.h | 4 +- gtsam/geometry/Point2.h | 8 +-- gtsam/geometry/Point3.h | 4 +- gtsam/geometry/Pose2.cpp | 6 +- gtsam/geometry/Pose2.h | 4 +- gtsam/geometry/Rot2.h | 4 +- gtsam/geometry/Rot3.cpp | 97 ++++++++++++++---------------- gtsam/geometry/Rot3.h | 42 +++---------- gtsam/geometry/Rot3M.cpp | 9 +-- gtsam/geometry/Rot3Q.cpp | 9 +-- gtsam/geometry/tests/testPose2.cpp | 10 +-- gtsam/geometry/tests/testRot2.cpp | 6 +- gtsam/geometry/tests/testRot3.cpp | 48 ++++++++------- 13 files changed, 108 insertions(+), 143 deletions(-) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index b208a584a..4e40e03a9 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -107,12 +107,12 @@ namespace gtsam { static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& v) { return eye(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { + static Matrix LogmapDerivative(const Vector& v) { return eye(1); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index fadea652b..da630a803 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -174,14 +174,10 @@ public: static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector2& v) { - return eye(2); - } + static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;} /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector2& v) { - return eye(2); - } + static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;} /// @} /// @name Vector Space diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 56d9b8bef..0afe2acf8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -142,12 +142,12 @@ namespace gtsam { static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector& v) { + static Matrix3 ExpmapDerivative(const Vector& v) { return I_3x3; } /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector& v) { + static Matrix3 LogmapDerivative(const Vector& v) { return I_3x3; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 719856f78..699994c3b 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -135,7 +135,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) { } /* ************************************************************************* */ -Matrix3 Pose2::dexpL(const Vector3& v) { +Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { double alpha = v[2]; if (fabs(alpha) > 1e-5) { // Chirikjian11book2, pg. 36 @@ -145,7 +145,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) { * \dot{g} g^{-1} = dexpR_{q}\dot{q} * where q = A, and g = exp(A) * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26. - * Hence, to compute dexpL, we have to use the formula of J_r Chirikjian11book2, pg.36 + * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36 */ double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; @@ -164,7 +164,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) { } /* ************************************************************************* */ -Matrix3 Pose2::dexpInvL(const Vector3& v) { +Matrix3 Pose2::LogmapDerivative(const Vector3& v) { double alpha = v[2]; if (fabs(alpha) > 1e-5) { double alphaInv = 1/alpha; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index ba85ed0ac..aa201cfa2 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -182,10 +182,10 @@ public: } /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); + static Matrix3 ExpmapDerivative(const Vector3& v); /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); + static Matrix3 LogmapDerivative(const Vector3& v); /// @} diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 21a402426..231522102 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -176,12 +176,12 @@ namespace gtsam { } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { + static Matrix ExpmapDerivative(const Vector& v) { return ones(1); } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { + static Matrix LogmapDerivative(const Vector& v) { return ones(1); } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 3f681a433..f494df07b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -107,32 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, return q; } -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix3 res = I_3x3 + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Point3 Rot3::column(int index) const{ if(index == 3) @@ -176,35 +150,56 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx - Jr = I_3x3 - ((1-cos(normx))/(normx)) * X + - (1 -sin(normx)/normx) * X * X; // right Jacobian - } - return Jr; + if(zero(x)) return I_3x3; + double theta = x.norm(); // rotation angle +#ifdef DUY_VERSION + /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + return I_3x3 - 0.5*s1*s1*X + s2*X2; +#else // Luca's version + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) + * where Jr = ExpmapDerivative(thetahat); + * This maps a perturbation in the tangent space (omega) to + * a perturbation on the manifold (expmap(Jr * omega)) + */ + // element of Lie algebra so(3): X = x^, normalized by normx + const Matrix3 Y = skewSymmetric(x) / theta; + return I_3x3 - ((1 - cos(theta)) / (theta)) * Y + + (1 - sin(theta) / theta) * Y * Y; // right Jacobian +#endif } /* ************************************************************************* */ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = I_3x3 + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; + if(zero(x)) return I_3x3; + double theta = x.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + return I_3x3 + 0.5*X - s2*X2; +#else // Luca's version + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = LogmapDerivative(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) + */ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 86a3f127f..630dad356 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -216,7 +216,7 @@ namespace gtsam { } /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, @@ -288,45 +288,21 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v, boost::optional H = boost::none) { - if(H){ - H->resize(3,3); - *H = Rot3::ExpmapDerivative(v); - } - if(zero(v)) - return Rot3(); - else - return rodriguez(v); + static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + if(H) *H = Rot3::ExpmapDerivative(v); + if (zero(v)) return Rot3(); else return rodriguez(v); } /** - * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R, boost::optional H = boost::none); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); - - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); - - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = ExpmapDerivative(thetahat); - * This maps a perturbation in the tangent space (omega) to - * a perturbation on the manifold (expmap(Jr * omega)) - */ + /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& x); - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega - * where Jrinv = LogmapDerivative(omega); - * This maps a perturbation on the manifold (expmap(omega)) - * to a perturbation in the tangent space (Jrinv * omega) - */ + /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& x); /// @} diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 2e21929f6..041a1b854 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const { } /* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } @@ -184,7 +184,7 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { static const double PI = boost::math::constants::pi(); @@ -223,10 +223,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { rot(1,0)-rot(0,1)); } - if(H){ - H->resize(3,3); - *H = Rot3::LogmapDerivative(thetaR); - } + if(H) *H = Rot3::LogmapDerivative(thetaR); return thetaR; } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 2c56a2e0b..5c2158ab4 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -101,7 +101,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -133,7 +133,7 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R, boost::optional H) { + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, @@ -160,10 +160,7 @@ namespace gtsam { thetaR = (angle / s) * q.vec(); } - if(H){ - H->resize(3,3); - *H = Rot3::LogmapDerivative(thetaR); - } + if(H) *H = Rot3::LogmapDerivative(thetaR); return thetaR; } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index da860b226..5d3f52516 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -204,22 +204,22 @@ Vector3 testDexpL(const Vector3 w, const Vector3& dw) { return y; } -TEST( Pose2, dexpL) { - Matrix actualDexpL = Pose2::dexpL(w); +TEST( Pose2, ExpmapDerivative) { + Matrix actualDexpL = Pose2::ExpmapDerivative(w); Matrix expectedDexpL = numericalDerivative11( boost::bind(testDexpL, w, _1), zero(3), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix actualDexpInvL = Pose2::dexpInvL(w); + Matrix actualDexpInvL = Pose2::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); // test case where alpha = 0 - Matrix actualDexpL0 = Pose2::dexpL(w0); + Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0); Matrix expectedDexpL0 = numericalDerivative11( boost::bind(testDexpL, w0, _1), zero(3), 1e-2); EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5)); - Matrix actualDexpInvL0 = Pose2::dexpInvL(w0); + Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 2b9e5c046..45fecb244 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -166,14 +166,14 @@ Vector1 testDexpL(const Vector& dw) { return y; } -TEST( Rot2, dexpL) { - Matrix actualDexpL = Rot2::dexpL(w); +TEST( Rot2, ExpmapDerivative) { + Matrix actualDexpL = Rot2::ExpmapDerivative(w); Matrix expectedDexpL = numericalDerivative11( boost::function( boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - Matrix actualDexpInvL = Rot2::dexpInvL(w); + Matrix actualDexpInvL = Rot2::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6453063b5..a8874fbfd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -214,14 +214,39 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } +/* ************************************************************************* */ +Vector w = Vector3(0.1, 0.27, -0.2); + +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); +} + +TEST( Rot3, ExpmapDerivative) { + Matrix actualDexpL = Rot3::ExpmapDerivative(w); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); + + Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); +} + /* ************************************************************************* */ Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative ) +TEST( Rot3, ExpmapDerivative2) { Matrix Jexpected = numericalDerivative11( boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); CHECK(assert_equal(Jexpected, Jactual)); + + Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); } /* ************************************************************************* */ @@ -412,27 +437,6 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } -/* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, dexpL) { - Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); -} - /* ************************************************************************* */ TEST( Rot3, xyz ) {