diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 917b80c9a..92ec0dd9b 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -27,52 +27,36 @@ namespace gtsam { //------------------------------------------------------------------------------ // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - PreintegratedRotation(I_3x3), biasHat_(Vector3()) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { +void PreintegratedAhrsMeasurements::print(const string& s) const { PreintegratedRotation::print(s); cout << "biasHat [" << biasHat_.transpose() << "]" << endl; cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ -bool AHRSFactor::PreintegratedMeasurements::equals( - const PreintegratedMeasurements& other, double tol) const { - return PreintegratedRotation::equals(other, tol) - && equal_with_abs_tol(biasHat_, other.biasHat_, tol); +bool PreintegratedAhrsMeasurements::equals( + const PreintegratedAhrsMeasurements& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && + equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::resetIntegration() { +void PreintegratedAhrsMeasurements::resetIntegration() { PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { +void PreintegratedAhrsMeasurements::integrateMeasurement( + const Vector3& measuredOmega, double deltaT) { // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; // 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(); + if (p().body_P_sensor) { + Matrix3 body_R_sensor = p().body_P_sensor->rotation().matrix(); // rotation rate vector in the body frame correctedOmega = body_R_sensor * correctedOmega; } @@ -92,18 +76,17 @@ 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() - + gyroscopeCovariance() * deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT; } //------------------------------------------------------------------------------ -Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, - boost::optional H) const { +Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias, + OptionalJacobian<3,3> H) const { const Vector3 biasOmegaIncr = bias - biasHat_; return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ -Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( +Vector PreintegratedAhrsMeasurements::DeltaAngles( const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles) { @@ -121,22 +104,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( //------------------------------------------------------------------------------ // AHRSFactor methods //------------------------------------------------------------------------------ -AHRSFactor::AHRSFactor() : - _PIM_(Vector3(), Z_3x3) { -} +AHRSFactor::AHRSFactor( + Key rot_i, Key rot_j, Key bias, + const PreintegratedAhrsMeasurements& preintegratedMeasurements) + : Base(noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), + rot_i, rot_j, bias), + _PIM_(preintegratedMeasurements) {} -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), _PIM_( - preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor) { -} - -//------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { +//------------------------------------------------------------------------------ return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -147,20 +124,13 @@ void AHRSFactor::print(const string& s, cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; _PIM_.print(" preintegrated measurements:"); - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); - if (body_P_sensor_) - 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) && _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_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -172,7 +142,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri); const Matrix3 coriolisHat = skewSymmetric(coriolis); const Vector3 correctedOmega = biascorrectedOmega - coriolis; @@ -215,15 +185,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - +Rot3 AHRSFactor::Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements) { const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); + const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); @@ -231,4 +199,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, return rot_i.compose(correctedDeltaRij); } -} //namespace gtsam +//------------------------------------------------------------------------------ +AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->body_P_sensor = body_P_sensor; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedMeasurements pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + PreintegratedMeasurements newPim = pim; + newPim.p_ = p; + return Predict(rot_i, bias, newPim); +} + +} // namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index fbf7d51a1..f9f846790 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -26,91 +26,92 @@ namespace gtsam { -class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { -public: +/** + * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope + * measurements (rotation rates) and the corresponding covariance matrix. + * Can be built incrementally so as to avoid costly integration at time of factor construction. + */ +class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation { + + protected: + + Vector3 biasHat_; ///< Angular rate bias values used during preintegration. + Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + /// Default constructor, only for serialization + PreintegratedAhrsMeasurements() {} + + friend class AHRSFactor; + + public: /** - * CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope - * measurements (rotation rates) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated AHRS factor. - * Can be built incrementally so as to avoid costly integration at time of - * factor construction. + * Default constructor, initialize with no measurements + * @param bias Current estimate of acceleration and rotation rate biases */ - class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation { + PreintegratedAhrsMeasurements(const boost::shared_ptr& p, const Vector3& biasHat) + : PreintegratedRotation(p), biasHat_(biasHat) { + resetIntegration(); + } - friend class AHRSFactor; + const Params& p() const { return *boost::static_pointer_cast(p_);} + const Vector3& biasHat() const { return biasHat_; } + const Matrix3& preintMeasCov() const { return preintMeasCov_; } - protected: - Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + /// print + void print(const std::string& s = "Preintegrated Measurements: ") const; - public: + /// equals + bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const; - /// Default constructor - PreintegratedMeasurements(); + /// Reset inetgrated quantities to zero + void resetIntegration(); - /** - * Default constructor, initialize with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredOmegaCovariance Covariance matrix of measured angular rate - */ - PreintegratedMeasurements(const Vector3& bias, - const Matrix3& measuredOmegaCovariance); + /** + * Add a single Gyroscope measurement to the preintegration. + * @param measureOmedga Measured angular velocity (in body frame) + * @param deltaT Time step + */ + void integrateMeasurement(const Vector3& measuredOmega, double deltaT); - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } + /// Predict bias-corrected incremental rotation + /// TODO: The matrix Hbias is the derivative of predict? Unit-test? + Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; - /// print - void print(const std::string& s = "Preintegrated Measurements: ") const; + // 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, + const Vector3& delta_angles); - /// equals - bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - - /// TODO: Document - void resetIntegration(); - - /** - * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) - * @param deltaT Time step - * @param body_P_sensor Optional sensor frame - */ - void integrateMeasurement(const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); - - /// Predict bias-corrected incremental rotation - /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const Vector3& bias, boost::optional H = - boost::none) const; - - // 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, - const Vector3& delta_angles); - - private: - - /** Serialization function */ - 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_); - } - }; + /// @deprecated constructor + PreintegratedAhrsMeasurements(const Vector3& biasHat, + const Matrix3& measuredOmegaCovariance) + : PreintegratedRotation(boost::make_shared()), + biasHat_(biasHat) { + resetIntegration(); + } private: + + /** Serialization function */ + 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(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + } +}; + +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { + typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - 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 + PreintegratedAhrsMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + AHRSFactor() {} public: @@ -121,22 +122,15 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - AHRSFactor(); - /** * 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); + const PreintegratedAhrsMeasurements& preintegratedMeasurements); virtual ~AHRSFactor() { } @@ -152,14 +146,10 @@ public: virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; /// Access the preintegrated measurements. - const PreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { return _PIM_; } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - /** implement functions needed to derive from Factor */ /// vector of errors @@ -169,10 +159,25 @@ public: boost::none) const; /// predicted states from IMU + /// TODO(frank): relationship with PIM predict ?? + static Rot3 Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements); + + /// @deprecated name + typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; + + /// @deprecated constructor + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + + /// @deprecated static function static Rot3 predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none); + const boost::optional& body_P_sensor = boost::none); private: @@ -184,13 +189,9 @@ private: & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // AHRSFactor -typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; - } //namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1db2f1861..c1ec7f361 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -29,53 +29,30 @@ namespace gtsam { using namespace std; //------------------------------------------------------------------------------ -// Inner class CombinedPreintegratedMeasurements +// Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, - const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, - const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( - biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( - biasAccOmegaInit) { - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::print( +void PreintegratedCombinedMeasurements::print( const string& s) const { PreintegrationBase::print(s); - 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(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); +bool PreintegratedCombinedMeasurements::equals( + const PreintegratedCombinedMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) && + equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { +void PreintegratedCombinedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( +void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, 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. @@ -83,7 +60,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - correctedAcc, correctedOmega, body_P_sensor); + &correctedAcc, &correctedOmega); 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 @@ -91,20 +68,19 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, 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 // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Matrix3 R_i = deltaRij(); // store this + const Matrix3 dRij = deltaRij().matrix(); // expensive when quaternion // Update preintegrated measurements. TODO Frank moved from end of this function !!! Matrix9 F_9x9; updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = -R_i * deltaT; + Matrix3 H_vel_biasacc = -dRij * deltaT; Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) @@ -127,18 +103,18 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( G_measCov_Gt.setZero(15, 15); // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); + G_measCov_Gt.block<3, 3>(0, 0) = deltaT * p().integrationCovariance; G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) - * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) * (H_vel_biasacc.transpose()); G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) - * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; - G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; + G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * p().biasAccCovariance; + G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) + Matrix3 block23 = H_vel_biasacc * p().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(); @@ -162,26 +138,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( } } +//------------------------------------------------------------------------------ +PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, + const bool use2ndOrderIntegration) { + biasHat_ = biasHat; + boost::shared_ptr p = boost::make_shared(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p->biasAccCovariance = biasAccCovariance; + p->biasOmegaCovariance = biasOmegaCovariance; + p->biasAccOmegaInit = biasAccOmegaInit; + p->use2ndOrderIntegration = use2ndOrderIntegration; + p_ = p; + resetIntegration(); + preintMeasCov_.setZero(); +} //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor() : - 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, - 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), ImuFactorBase(gravity, omegaCoriolis, - body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { -} +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -196,17 +181,14 @@ void CombinedImuFactor::print(const string& s, << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; - ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } //------------------------------------------------------------------------------ -bool CombinedImuFactor::equals(const NonlinearFactor& expected, - double tol) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); +bool CombinedImuFactor::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); } //------------------------------------------------------------------------------ @@ -226,8 +208,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, - bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, + Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); @@ -275,4 +256,42 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, return r; } +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& pim, const Vector3& gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->gravity = gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} +//------------------------------------------------------------------------------ +void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& pim, + const Vector3& gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // NOTE(frank): hack below only for backward compatibility + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->gravity = gravity; + p->omegaCoriolis = omegaCoriolis; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + CombinedPreintegratedMeasurements newPim = pim; + newPim.p_ = p; + PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i); + pose_j = pvb.pose; + vel_j = pvb.velocity; +} + } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 7fdafd8ce..a70d1d24f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,13 +24,113 @@ /* GTSAM includes */ #include #include -#include -#include +#include namespace gtsam { /** - * + * PreintegratedCombinedMeasurements 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 PreintegratedCombinedMeasurements : public PreintegrationBase { + + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params : PreintegrationBase::Params { + 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 + + Params():biasAccCovariance(I_3x3), biasOmegaCovariance(I_3x3), biasAccOmegaInit(I_6x6) {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit); + } + }; + + protected: + /* Covariance matrix of the preintegrated measurements + * COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + * (first-order propagation from *measurementCovariance*). + * PreintegratedCombinedMeasurements also include the biases and keep the correlation + * between the preintegrated measurements and the biases + */ + Eigen::Matrix preintMeasCov_; + + PreintegratedCombinedMeasurements() {} + + friend class CombinedImuFactor; + + public: + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + */ + PreintegratedCombinedMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) + : PreintegrationBase(p, biasHat) { + preintMeasCov_.setZero(); + } + + const Params& p() const { return *boost::static_pointer_cast(p_);} + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedCombinedMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedCombinedMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the + * sensor) + * @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) + */ + void integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional F_test = boost::none, + boost::optional G_test = boost::none); + + /// methods to access class variables + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// deprecated constructor + PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = false); + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); + } +}; + +/** * @addtogroup SLAM * * If you are using the factor, please cite: @@ -46,14 +146,12 @@ namespace gtsam { * 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). Following the pre- * integration scheme proposed in [2], the CombinedImuFactor includes many IMU - * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * measurements, which are "summarized" using the PreintegratedCombinedMeasurements * 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 @@ -61,105 +159,24 @@ namespace gtsam { * "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 + * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. */ class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { + Vector3, imuBias::ConstantBias, imuBias::ConstantBias> { public: - /** - * 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 { - - friend class CombinedImuFactor; - - protected: - - 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] - ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation - ///< between the preintegrated measurements and the biases - - public: - - /** - * Default constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredAccCovariance Covariance matrix of measuredAcc - * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate - * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) - * @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution) - * @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution) - * @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements - * @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) - */ - CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = - false); - - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const CombinedPreintegratedMeasurements& expected, double tol = - 1e-9) const; - - /// Re-initialize CombinedPreintegratedMeasurements - void resetIntegration(); - - /** - * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) - * @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) - */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT, - 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 preintMeasCov() const { - return preintMeasCov_; - } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); - } - }; - private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements _PIM_; + PreintegratedCombinedMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + CombinedImuFactor() {} public: @@ -170,9 +187,6 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - CombinedImuFactor(); - /** * Constructor * @param pose_i Previous pose key @@ -181,21 +195,13 @@ public: * @param vel_j Current velocity key * @param bias_i Previous bias key * @param bias_j Current bias key - * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements 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 + * @param PreintegratedCombinedMeasurements Combined IMU measurements */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, - Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false); + CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& preintegratedMeasurements); - virtual ~CombinedImuFactor() { - } + virtual ~CombinedImuFactor() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -211,7 +217,7 @@ public: /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedCombinedMeasurements& preintegratedMeasurements() const { return _PIM_; } @@ -226,16 +232,22 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - // @deprecated The following function has been deprecated, use PreintegrationBase::predict + /// @deprecated typename + typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements; + + /// @deprecated constructor + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, const CombinedPreintegratedMeasurements& pim, + const Vector3& gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + // @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, - Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { - PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis); - pose_j = PVB.pose; - vel_j = PVB.velocity; - } + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& pim, + const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis = false); private: @@ -246,13 +258,8 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // class CombinedImuFactor -typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; - } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 17c68139c..024bdd65e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -31,43 +31,32 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ -ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration) { - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::print(const string& s) const { +void PreintegratedImuMeasurements::print(const string& s) const { PreintegrationBase::print(s); } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals( - const PreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); +bool PreintegratedImuMeasurements::equals( + const PreintegratedImuMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) && + equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration() { +void PreintegratedImuMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::integrateMeasurement( +void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - correctedAcc, correctedOmega, body_P_sensor); + &correctedAcc, &correctedOmega); // rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; @@ -79,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( 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 + const Matrix3 dRij = deltaRij().matrix(); // store this, which is useful to compute G_test Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); @@ -92,18 +81,18 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise, // as G and measurementCovariance are block-diagonal matrices preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - 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>(0, 0) += p().integrationCovariance * deltaT; + preintMeasCov_.block<3, 3>(3, 3) += dRij * p().accelerometerCovariance + * dRij.transpose() * deltaT; preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega - * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; + * p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT; Matrix3 F_pos_noiseacc; - if (use2ndOrderIntegration()) { - F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; + if (p().use2ndOrderIntegration) { + F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT; preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc - * accelerometerCovariance() * F_pos_noiseacc.transpose(); - Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // has 1/deltaT + * p().accelerometerCovariance * F_pos_noiseacc.transpose(); + Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance * dRij.transpose(); // has 1/deltaT preintMeasCov_.block<3, 3>(0, 3) += temp; preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); } @@ -114,34 +103,40 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( } if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise - if (!use2ndOrderIntegration()) + if (!p().use2ndOrderIntegration) F_pos_noiseacc = Z_3x3; // intNoise accNoise omegaNoise (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, dRij * deltaT, Z_3x3, // vel Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } +//------------------------------------------------------------------------------ +PreintegratedImuMeasurements::PreintegratedImuMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration) + { + biasHat_ = biasHat; + boost::shared_ptr p = boost::make_shared(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p->use2ndOrderIntegration = use2ndOrderIntegration; + p_ = p; + resetIntegration(); +} //------------------------------------------------------------------------------ // ImuFactor methods -//------------------------------------------------------------------------------ -ImuFactor::ImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { -} - //------------------------------------------------------------------------------ ImuFactor::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, 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), _PIM_(preintegratedMeasurements) { -} + const PreintegratedImuMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), + _PIM_(pim) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -155,17 +150,17 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - ImuFactorBase::print(""); + Base::print(""); _PIM_.print(" preintegrated measurements:"); // Print standard deviations on covariance only cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast(&expected); +bool ImuFactor::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) - && ImuFactorBase::equals(*e, tol); + && Base::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -174,9 +169,46 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const { - return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); + H1, H2, H3, H4, H5); +} + +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& pim, + const Vector3& gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->gravity = gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const PreintegratedMeasurements& pim, + const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // NOTE(frank): hack below only for backward compatibility + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->gravity = gravity; + p->omegaCoriolis = omegaCoriolis; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + PreintegratedMeasurements newPim = pim; + newPim.p_ = p; + PoseVelocityBias pvb = newPim.predict(pose_i, vel_i, bias_i); + pose_j = pvb.pose; + vel_j = pvb.velocity; } } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 57f4d0e89..eb94675fa 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,11 +24,86 @@ /* GTSAM includes */ #include #include -#include #include namespace gtsam { +/** + * PreintegratedIMUMeasurements 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. + * 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 PreintegratedImuMeasurements: public PreintegrationBase { + + friend class ImuFactor; + +protected: + + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). + + /// Default constructor for serialization + PreintegratedImuMeasurements() {} + +public: + + /** + * Constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param p Parameters, typically fixed in a single application + */ + PreintegratedImuMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + PreintegrationBase(p,biasHat) { + preintMeasCov_.setZero(); + } + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedImuMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedIMUMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between this and the last IMU measurement + * @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, + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); + + /// Return pre-integrated measurement covariance + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// @deprecated constructor + PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration = false); + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); + } +}; + /** * * @addtogroup SLAM @@ -53,96 +128,22 @@ namespace gtsam { * 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. + * are "summarized" using the PreintegratedIMUMeasurements 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 { + imuBias::ConstantBias> { 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. - * 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 { - - friend class ImuFactor; - - protected: - - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] - ///< (first-order propagation from *measurementCovariance*). - - public: - - /** - * Default constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredAccCovariance Covariance matrix of measuredAcc - * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate - * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) - * @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) - */ - PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const bool use2ndOrderIntegration = false); - - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const PreintegratedMeasurements& expected, - double tol = 1e-9) const; - - /// Re-initialize PreintegratedMeasurements - void resetIntegration(); - - /** - * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) - * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between this and the last IMU measurement - * @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, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = - boost::none); - - /// methods to access class variables - Matrix preintMeasCov() const { - return preintMeasCov_; - } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); - } - }; - private: typedef ImuFactor This; typedef NoiseModelFactor5 Base; - PreintegratedMeasurements _PIM_; + PreintegratedImuMeasurements _PIM_; public: @@ -163,17 +164,9 @@ public: * @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); + const PreintegratedImuMeasurements& preintegratedMeasurements); virtual ~ImuFactor() { } @@ -192,7 +185,7 @@ public: /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedImuMeasurements& preintegratedMeasurements() const { return _PIM_; } @@ -206,16 +199,21 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - /// @deprecated The following function has been deprecated, use PreintegrationBase::predict + /// @deprecated typename + typedef PreintegratedImuMeasurements PreintegratedMeasurements; + + /// @deprecated constructor + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + /// @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const PreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) { - PoseVelocityBias PVB = PIM.predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis); - pose_j = PVB.pose; - vel_j = PVB.velocity; - } + const PreintegratedMeasurements& pim, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: @@ -226,13 +224,8 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - 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/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h deleted file mode 100644 index 1e4e51220..000000000 --- a/gtsam/navigation/ImuFactorBase.h +++ /dev/null @@ -1,94 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 -#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: - - /** 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_; - } - - /// 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/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index ba10fd090..a1276b91c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,7 +21,8 @@ #pragma once -#include +#include +#include namespace gtsam { @@ -31,49 +32,64 @@ namespace gtsam { * It includes the definitions of the preintegrated rotation. */ class PreintegratedRotation { - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + public: + + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + Params():gyroscopeCovariance(I_3x3) {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } + }; + + private: double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - /// Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; + protected: + /// Parameters + boost::shared_ptr p_; - ///< continuous-time "Covariance" of gyroscope measurements - const Matrix3 gyroscopeCovariance_; + /// Default constructor for serialization + PreintegratedRotation() {} public: - /// Default constructor, initializes the variables in the base class - explicit PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : - deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( - measuredOmegaCovariance) { + /// Default constructor, resets integration to zero + explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { + resetIntegration(); } /// Re-initialize PreintegratedMeasurements void resetIntegration() { - deltaRij_ = Rot3(); deltaTij_ = 0.0; + deltaRij_ = Rot3(); delRdelBiasOmega_ = Z_3x3; } /// @name Access instance variables /// @{ - - // Return 3*3 matrix of rotation from time i to time j. Expensive in quaternion case - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - // Return log(rotation) from time i to time j. Expensive in both Rot3M and quaternion case. - Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { - return Rot3::Logmap(deltaRij_, H); - } const double& deltaTij() const { return deltaTij_; } + const Rot3& deltaRij() const { + return deltaRij_; + } const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } - const Matrix3& gyroscopeCovariance() const { - return gyroscopeCovariance_; - } /// @} /// @name Testable @@ -85,13 +101,10 @@ class PreintegratedRotation { std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; } - 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(gyroscopeCovariance_, - expected.gyroscopeCovariance_, tol); + bool equals(const PreintegratedRotation& other, double tol) const { + return deltaRij_.equals(other.deltaRij_, tol) && + fabs(deltaTij_ - other.deltaTij_) < tol && + equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } /// @} @@ -128,8 +141,7 @@ class PreintegratedRotation { if (H) { Matrix3 Jrinv_theta_bc; // This was done via an expmap, now we go *back* to so<3> - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, - Jrinv_theta_bc); + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; return biascorrectedOmega; @@ -139,9 +151,9 @@ class PreintegratedRotation { } /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij(); + Vector3 integrateCoriolis(const Rot3& rot_i) const { + if (!p_->omegaCoriolis) return Vector3::Zero(); + return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij(); } private: @@ -149,8 +161,9 @@ class PreintegratedRotation { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT - ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index cc4fcee16..bef45e044 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,27 +20,12 @@ **/ #include "PreintegrationBase.h" +#include + +using namespace std; namespace gtsam { -PreintegrationBase::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), - accelerometerCovariance_(measuredAccCovariance), - integrationCovariance_(integrationErrorCovariance) { -} - /// Re-initialize PreintegratedMeasurements void PreintegrationBase::resetIntegration() { PreintegratedRotation::resetIntegration(); @@ -53,24 +38,23 @@ void PreintegrationBase::resetIntegration() { } /// Needed for testable -void PreintegrationBase::print(const std::string& s) const { +void PreintegrationBase::print(const string& s) const { PreintegratedRotation::print(s); - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; biasHat_.print(" biasHat"); } /// Needed for testable bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) - && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) - && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); + return PreintegratedRotation::equals(other, tol) && + biasHat_.equals(other.biasHat_, tol) && + equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) && + equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) && + equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) && + equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) && + equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) && + equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); } /// Update preintegrated measurements @@ -78,9 +62,10 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - const Matrix3 dRij = deltaRij(); // expensive + const Matrix3 dRij = deltaRij().matrix(); // expensive const Vector3 temp = dRij * correctedAcc * deltaT; - if (!use2ndOrderIntegration_) { + + if (!p().use2ndOrderIntegration) { deltaPij_ += deltaVij_ * deltaT; } else { deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; @@ -89,13 +74,13 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte Matrix3 R_i, F_angles_angles; if (F) - R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + R_i = dRij; // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); if (F) { const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_pos_angles; - if (use2ndOrderIntegration_) + if (p().use2ndOrderIntegration) F_pos_angles = 0.5 * F_vel_angles * deltaT; else F_pos_angles = Z_3x3; @@ -112,9 +97,9 @@ void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correcte void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT) { - const Matrix3 dRij = deltaRij(); // expensive + const Matrix3 dRij = deltaRij().matrix(); // expensive const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); - if (!use2ndOrderIntegration_) { + if (!p().use2ndOrderIntegration) { delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; } else { @@ -127,19 +112,19 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc } void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); + const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3* correctedAcc, + Vector3* correctedOmega) { + *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(); + if (p().body_P_sensor) { + Matrix3 body_R_sensor = p().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 * p().body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } } @@ -148,31 +133,27 @@ void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( //------------------------------------------------------------------------------ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const Rot3& deltaRij_biascorrected, const Vector3& deltaPij_biascorrected, - const Vector3& deltaVij_biascorrected, - const bool use2ndOrderCoriolis) const { + const Vector3& deltaVij_biascorrected) const { const double dt = deltaTij(), dt2 = dt * dt; - - // Rotation const Matrix3 Ri = pose_i.rotation().matrix(); - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - const Vector3 dR = biascorrectedOmega - - Ri.transpose() * omegaCoriolis * dt; // Coriolis term - // Translation - Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * gravity * dt2 - - omegaCoriolis.cross(vel_i) * dt2; // Coriolis term - we got rid of the 2 wrt INS paper + // Rotation, translation, and velocity: + Vector3 dR = Rot3::Logmap(deltaRij_biascorrected); + Vector3 dP = Ri * deltaPij_biascorrected + vel_i * dt + 0.5 * p().gravity * dt2; + Vector3 dV = Ri * deltaVij_biascorrected + p().gravity * dt; - // Velocity - Vector3 dV = Ri * deltaVij_biascorrected + gravity * dt - - 2 * omegaCoriolis.cross(vel_i) * dt; // Coriolis term - - if (use2ndOrderCoriolis) { - Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector())); - dP -= 0.5 * temp * dt2; - dV -= temp * dt; + if (p().omegaCoriolis) { + const Vector3& omegaCoriolis = *p().omegaCoriolis; + dR -= Ri.transpose() * omegaCoriolis * dt; // Coriolis term + dP -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper + dV -= 2 * omegaCoriolis.cross(vel_i) * dt; + if (p().use2ndOrderCoriolis) { + Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(pose_i.translation().vector())); + dP -= 0.5 * temp * dt2; + dV -= temp * dt; + } } // TODO(frank): pose update below is separate expmap for R,t. Is that kosher? @@ -183,13 +164,12 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, /// Predict state at time j //------------------------------------------------------------------------------ PoseVelocityBias PreintegrationBase::predict( - const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) const { + const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i) const { const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - return predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - biascorrectedDeltaRij(biasIncr.gyroscope()), - biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr), - use2ndOrderCoriolis); + return predict( + pose_i, vel_i, bias_i, biascorrectedDeltaRij(biasIncr.gyroscope()), + biascorrectedDeltaPij(biasIncr), biascorrectedDeltaVij(biasIncr)); } /// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j @@ -197,9 +177,6 @@ PoseVelocityBias PreintegrationBase::predict( Vector9 PreintegrationBase::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, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, @@ -219,9 +196,9 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasIncr.gyroscope()); const Vector3 deltaPij_biascorrected = biascorrectedDeltaPij(biasIncr); const Vector3 deltaVij_biascorrected = biascorrectedDeltaVij(biasIncr); - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, deltaRij_biascorrected, deltaPij_biascorrected, - deltaVij_biascorrected, use2ndOrderCoriolis); + PoseVelocityBias predictedState_j = + predict(pose_i, vel_i, bias_i, deltaRij_biascorrected, + 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()); @@ -241,7 +218,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const H5 ? &D_cThetaRij_biasOmegaIncr : 0); // Coriolis term, NOTE inconsistent with AHRS, where coriolisHat is *after* integration - const Vector3 coriolis = integrateCoriolis(rot_i, omegaCoriolis); + // TODO(frank): move derivatives to predict and do coriolis branching there + const Vector3 coriolis = integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Residual rotation error @@ -253,17 +231,16 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const const Vector3 fR = Rot3::Logmap(fRrot, H1 || H3 || H5 ? &D_fR_fRrot : 0); const double dt = deltaTij(), dt2 = dt*dt; - Matrix3 Ritranspose_omegaCoriolisHat; - if (H1 || H2) - Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); + Matrix3 RitOmegaCoriolisHat = Z_3x3; + if ((H1 || H2) && p().omegaCoriolis) + RitOmegaCoriolisHat = Ri.transpose() * skewSymmetric(*p().omegaCoriolis); if (H1) { const Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; - if (use2ndOrderCoriolis) { - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri - const Matrix3 temp = Ritranspose_omegaCoriolisHat - * (-Ritranspose_omegaCoriolisHat.transpose()); + if (p().use2ndOrderCoriolis) { + // this is the same as: Ri.transpose() * p().omegaCoriolisHat * p().omegaCoriolisHat * Ri + const Matrix3 temp = RitOmegaCoriolisHat * (-RitOmegaCoriolisHat.transpose()); dfPdPi += 0.5 * temp * dt2; dfVdPi += temp * dt; } @@ -278,8 +255,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const if (H2) { (*H2) << Z_3x3, // dfR/dVi - -Ri.transpose() * dt + Ritranspose_omegaCoriolisHat * dt2, // dfP/dVi - -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * dt; // dfV/dVi + -Ri.transpose() * dt + RitOmegaCoriolisHat * dt2, // dfP/dVi + -Ri.transpose() + 2 * RitOmegaCoriolisHat * dt; // dfV/dVi } if (H3) { (*H3) << @@ -306,19 +283,16 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const return r; } -ImuBase::ImuBase() - : gravity_(Vector3(0.0, 0.0, 9.81)), - omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), - body_P_sensor_(boost::none), - use2ndOrderCoriolis_(false) { +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility + boost::shared_ptr q = boost::make_shared(p()); + q->gravity = gravity; + q->omegaCoriolis = omegaCoriolis; + q->use2ndOrderCoriolis = use2ndOrderCoriolis; + p_ = q; + return predict(pose_i, vel_i, bias_i); } - -ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, const bool use2ndOrderCoriolis) - : gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis) { -} - } /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 722091b40..c810eb538 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -37,11 +37,9 @@ struct PoseVelocityBias { Vector3 velocity; imuBias::ConstantBias bias; - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) - : pose(_pose), - velocity(_velocity), - bias(_bias) { - } + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) + : pose(_pose), velocity(_velocity), bias(_bias) {} }; /** @@ -52,71 +50,85 @@ struct PoseVelocityBias { */ class PreintegrationBase : public PreintegratedRotation { - const imuBias::ConstantBias biasHat_; ///< Acceleration and gyro bias used for preintegration - const bool use2ndOrderIntegration_; ///< Controls the order of integration + public: + + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params : PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + bool 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) + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 gravity; ///< Gravity constant + + Params() + : accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderIntegration(false), + use2ndOrderCoriolis(false), + gravity(0, 0, 9.8) {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); + ar & BOOST_SERIALIZATION_NVP(integrationCovariance); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderIntegration); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); + ar & BOOST_SERIALIZATION_NVP(gravity); + } + }; + + protected: + + /// Acceleration and gyro bias used for preintegration + imuBias::ConstantBias biasHat_; 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) - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + 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 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty - /// (to compensate errors in Euler integration) + /// Default constructor for serialization + PreintegrationBase() {} public: /** - * Default constructor, initializes the variables in the base class + * 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) + * @param p Parameters, typically fixed in a single application */ - PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); + PreintegrationBase(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) + : PreintegratedRotation(p), biasHat_(biasHat) { + resetIntegration(); + } /// Re-initialize PreintegratedMeasurements void resetIntegration(); - /// methods to access class variables - bool use2ndOrderIntegration() const { - return use2ndOrderIntegration_; - } - const Vector3& deltaPij() const { - return deltaPij_; - } - const Vector3& deltaVij() const { - return deltaVij_; - } - const imuBias::ConstantBias& biasHat() const { - return biasHat_; - } - 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_; - } - const Matrix3& delVdelBiasOmega() const { - return delVdelBiasOmega_; - } + const Params& p() const { return *boost::static_pointer_cast(p_);} - const Matrix3& accelerometerCovariance() const { - return accelerometerCovariance_; - } - const Matrix3& integrationCovariance() const { - return integrationCovariance_; - } + /// getters + const imuBias::ConstantBias& biasHat() const { return biasHat_; } + const Vector3& deltaPij() const { return deltaPij_; } + const Vector3& deltaVij() const { return deltaVij_; } + const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } + const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_; } + const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_; } + const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_; } + + // Exposed for MATLAB + Vector6 biasHatVector() const { return biasHat_.vector(); } /// print void print(const std::string& s) const; @@ -134,9 +146,9 @@ class PreintegrationBase : public PreintegratedRotation { double deltaT); void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, - boost::optional body_P_sensor); + const Vector3& measuredOmega, + Vector3* correctedAcc, + Vector3* correctedOmega); Vector3 biascorrectedDeltaPij(const imuBias::ConstantBias& biasIncr) const { return deltaPij_ + delPdelBiasAcc_ * biasIncr.accelerometer() @@ -150,33 +162,36 @@ class PreintegrationBase : public PreintegratedRotation { /// Predict state at time j, with bias-corrected quantities given PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, const Vector3& gravity, const Vector3& omegaCoriolis, - const Rot3& deltaRij_biascorrected, - const Vector3& deltaPij_biascorrected, const Vector3& deltaVij_biascorrected, - const bool use2ndOrderCoriolis = false) const; + const imuBias::ConstantBias& bias_i, + const Rot3& deltaRij_biascorrected, + const Vector3& deltaPij_biascorrected, + const Vector3& deltaVij_biascorrected) const; /// 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 imuBias::ConstantBias& bias_i) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j 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, OptionalJacobian<9, 6> H1 = - boost::none, + 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; + /// @deprecated predict + 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); + private: /** Serialization function */ 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(p_); ar & BOOST_SERIALIZATION_NVP(biasHat_); ar & BOOST_SERIALIZATION_NVP(deltaPij_); ar & BOOST_SERIALIZATION_NVP(deltaVij_); @@ -187,32 +202,4 @@ class PreintegrationBase : public PreintegratedRotation { } }; -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: - - /// Default constructor, with decent gravity and zero coriolis - ImuBase(); - - /// Fully fledge constructor - ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false); - - const Vector3& gravity() const { - return gravity_; - } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - -}; - } /// namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 928c0f74f..73e30ac32 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -50,7 +50,7 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const Vector3& bias, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3()) { + const Vector3& initialRotationRate = Vector3::Zero()) { AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); list::const_iterator itOmega = measuredOmegas.begin(); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index c5e9886d9..a6703272c 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -146,15 +146,9 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { 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(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), - tol)); + EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); + EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); + EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } @@ -373,7 +367,6 @@ TEST(CombinedImuFactor, PredictRotation) { TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor = Pose3(); // Measurements list measuredAccs, measuredOmegas; @@ -408,7 +401,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); + newDeltaT, Factual, Gactual); bool use2ndOrderIntegration = false; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c6aa48b30..bffc62d90 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -90,11 +90,11 @@ const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( +PreintegratedImuMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, const bool use2ndOrderIntegration = false) { - ImuFactor::PreintegratedMeasurements result(bias, kMeasuredAccCovariance, + PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, use2ndOrderIntegration); @@ -159,7 +159,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, kMeasuredAccCovariance, + PreintegratedImuMeasurements actual1(bias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -180,7 +180,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { double expectedDeltaT2(1); // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual2 = actual1; + PreintegratedImuMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT( @@ -211,7 +211,7 @@ double deltaT = 1.0; TEST(ImuFactor, ErrorAndJacobians) { using namespace common; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, + PreintegratedImuMeasurements pre_int_data(bias, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -290,7 +290,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data( + PreintegratedImuMeasurements pre_int_data( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); @@ -330,7 +330,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data( + PreintegratedImuMeasurements pre_int_data( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); @@ -452,7 +452,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = + PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); @@ -495,7 +495,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { 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; @@ -514,7 +513,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { } bool use2ndOrderIntegration = false; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = + PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); @@ -531,7 +530,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); + newDeltaT, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F @@ -619,7 +618,6 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) { TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { // 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; @@ -638,7 +636,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { } bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = + PreintegratedImuMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); @@ -655,7 +653,7 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) { Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); + newDeltaT, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F @@ -760,7 +758,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - ImuFactor::PreintegratedMeasurements pre_int_data( + PreintegratedImuMeasurements pre_int_data( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); @@ -797,7 +795,7 @@ TEST(ImuFactor, PredictPositionAndVelocity) { Matrix I6x6(6, 6); I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data( + PreintegratedImuMeasurements pre_int_data( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); @@ -835,7 +833,7 @@ TEST(ImuFactor, PredictRotation) { Matrix I6x6(6, 6); I6x6 = Matrix::Identity(6, 6); - ImuFactor::PreintegratedMeasurements pre_int_data( + PreintegratedImuMeasurements pre_int_data( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true);