From e9df6198ffe44268053f234e566dc3941e4b31f4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 23 Nov 2014 11:51:54 +0100 Subject: [PATCH] Moved AHRSFactor methods to cpp --- gtsam/navigation/AHRSFactor.cpp | 151 +++++++++++++++++++++ gtsam/navigation/AHRSFactor.h | 227 ++++++++------------------------ 2 files changed, 209 insertions(+), 169 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 88effd1b1..748c57138 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -151,4 +151,155 @@ Vector AHRSFactor::PreintegratedMeasurements::PreIntegrateIMUObservations_delta_ return Rot3::Logmap(R_t_to_t0); } +//------------------------------------------------------------------------------ +// AHRSFactor methods +//------------------------------------------------------------------------------ +AHRSFactor::AHRSFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) { +} + +AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& omegaCoriolis, boost::optional body_P_sensor) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( + body_P_sensor) { +} + +/// @return a deep copy of this factor +gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void AHRSFactor::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" + << std::endl; + this->noiseModel_->print(" noise model: "); + if (this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); +} + +//------------------------------------------------------------------------------ +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) + && 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_))); +} + +//------------------------------------------------------------------------------ +Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, + const imuBias::ConstantBias& bias, boost::optional H1, + boost::optional H2, boost::optional H3) const { + + double deltaTij = preintegratedMeasurements_.deltaTij_; + + Vector3 biasOmegaIncr = bias.gyroscope() + - preintegratedMeasurements_.biasHat_.gyroscope(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract( + preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, + Rot3::EXPMAP); + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected + - rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( + theta_biascorrected_corioliscorrected); + + Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between( + rot_i.between(rot_j)); + + Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3( + theta_biascorrected_corioliscorrected); + + Matrix3 Jtheta = -Jr_theta_bcc + * skewSymmetric(rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse( + Rot3::Logmap(fRhat)); + + if (H1) { + H1->resize(3, 3); + (*H1) << // dfR/dRi + Jrinv_fRhat + * (-rot_j.between(rot_i).matrix() + - fRhat.inverse().matrix() * Jtheta); + } + if (H2) { + + H2->resize(3, 3); + (*H2) << + // dfR/dPosej + Jrinv_fRhat * (Matrix3::Identity()); + } + + if (H3) { + + Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse( + theta_biascorrected); + Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3( + preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr + * preintegratedMeasurements_.delRdelBiasOmega_; + + H3->resize(3, 6); + (*H3) << + // dfR/dBias + Matrix::Zero(3, 3), Jrinv_fRhat + * (-fRhat.inverse().matrix() * JbiasOmega); + } + + Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(3); + r << fR; + return r; +} + +//------------------------------------------------------------------------------ +Rot3 AHRSFactor::predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, + const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& omegaCoriolis, boost::optional body_P_sensor) { + + const double& deltaTij = preintegratedMeasurements.deltaTij_; +// const Vector3 biasAccIncr = bias.accelerometer() + -preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() + - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = rot_i; + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + 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 = + return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected)); + +} + } //namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 9bccacb1c..de70b1d45 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -39,8 +39,8 @@ public: friend class AHRSFactor; protected: - imuBias::ConstantBias 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) + imuBias::ConstantBias 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 @@ -66,12 +66,24 @@ public: /// 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_;} - Vector6 biasHat() const {return biasHat_.vector();} - const Matrix3& delRdelBiasOmega() const {return delRdelBiasOmega_;} - const Matrix3& preintMeasCov() const {return preintMeasCov_;} + const Matrix3& measurementCovariance() const { + return measurementCovariance_; + } + Matrix3 deltaRij() const { + return deltaRij_.matrix(); + } + double deltaTij() const { + return deltaTij_; + } + Vector6 biasHat() const { + return biasHat_.vector(); + } + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } /// TODO: Document void resetIntegration(); @@ -112,7 +124,7 @@ private: PreintegratedMeasurements preintegratedMeasurements_; 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 + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame public: @@ -124,184 +136,57 @@ public: #endif /** Default constructor - only use for serialization */ - AHRSFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) {} + AHRSFactor(); - AHRSFactor( - Key rot_i, ///< previous rot key - Key rot_j, ///< current rot key - Key bias,///< previous bias key - const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated measurements - const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame - boost::optional body_P_sensor = boost::none ///< The Pose of the sensor frame in the body frame - ) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( - preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor) { + /** + * Constructor + * @param rot_i previous rot key + * @param rot_j current rot key + * @param bias previous bias key + * @param preintegratedMeasurements preintegrated measurements + * @param omegaCoriolis rotation rate of the inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + */ + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none); + + virtual ~AHRSFactor() { } - virtual ~AHRSFactor() {} - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr( - new This(*this) - ) - ); - } + virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** implement functions needed for Testable */ - - /** print */ + /// print virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) - << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; - this->noiseModel_->print(" noise model: "); - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } + DefaultKeyFormatter) const; - /** equals */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, 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_))); - } - /** Access the preintegrated measurements. */ + /// equals + virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; + + /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } /** implement functions needed to derive from Factor */ - /** vector of errors */ + /// vector of errors Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const - { + const imuBias::ConstantBias& bias, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const; - double deltaTij = preintegratedMeasurements_.deltaTij_; - - Vector3 biasOmegaIncr = bias.gyroscope() - - preintegratedMeasurements_.biasHat_.gyroscope(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - Rot3 deltaRij_biascorrected = - preintegratedMeasurements_.deltaRij_.retract( - preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, - Rot3::EXPMAP); - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( - theta_biascorrected_corioliscorrected); - - Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between( - rot_i.between(rot_j)); - - Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3( - theta_biascorrected_corioliscorrected); - - Matrix3 Jtheta = -Jr_theta_bcc - * skewSymmetric(rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse( - Rot3::Logmap(fRhat)); - - if (H1) { - H1->resize(3, 3); - (*H1) << // dfR/dRi - Jrinv_fRhat - * (-rot_j.between(rot_i).matrix() - - fRhat.inverse().matrix() * Jtheta); - } - if(H2) { - - H2->resize(3,3); - (*H2) << - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ); - } - - if (H3) { - - Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse( - theta_biascorrected); - Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3( - preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc - * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H3->resize(3, 6); - (*H3) << - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega); - } - - Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(3); - r << fR; - return r; - } - - /** predicted states from IMU */ - static Rot3 predict(const Rot3& rot_i, - const imuBias::ConstantBias& bias, + /// predicted states from IMU + static Rot3 predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none - ) { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; -// const Vector3 biasAccIncr = bias.accelerometer() - - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = rot_i; - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - 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 = - return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected)); - - } + boost::optional body_P_sensor = boost::none); private: @@ -310,12 +195,16 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // AHRSFactor + +}; +// AHRSFactor + typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; + } //namespace gtsam