diff --git a/.cproject b/.cproject index 617aa3795..9d2b1dca0 100644 --- a/.cproject +++ b/.cproject @@ -592,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -600,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -648,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -656,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -664,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -680,7 +675,6 @@ make - tests/testBayesTree true false @@ -1136,7 +1130,6 @@ make - testErrors.run true false @@ -1366,46 +1359,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1488,6 +1441,7 @@ make + testSimulated2DOriented.run true false @@ -1527,6 +1481,7 @@ make + testSimulated2D.run true false @@ -1534,6 +1489,7 @@ make + testSimulated3D.run true false @@ -1547,6 +1503,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1804,7 +1800,6 @@ cpack - -G DEB true false @@ -1812,7 +1807,6 @@ cpack - -G RPM true false @@ -1820,7 +1814,6 @@ cpack - -G TGZ true false @@ -1828,7 +1821,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2002,6 +1994,14 @@ true true + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + make -j2 @@ -2683,7 +2683,6 @@ make - testGraph.run true false @@ -2691,7 +2690,6 @@ make - testJunctionTree.run true false @@ -2699,7 +2697,6 @@ make - testSymbolicBayesNetB.run true false @@ -2809,14 +2806,6 @@ true true - - make - -j4 - testBasisDecompositions.run - true - true - true - make -j4 @@ -3251,6 +3240,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam.h b/gtsam.h index 5f5f4c5b1..4cccea87b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2404,25 +2404,24 @@ class ConstantBias { }///\namespace imuBias #include -class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, Vector velocity); +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; @@ -2430,10 +2429,11 @@ class ImuFactorPreintegratedMeasurements { Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { @@ -2444,11 +2444,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, +}; + +#include +class CombinedImuFactorPreintegratedMeasurements { + // Standard Constructor + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + + double deltaTij() const; + Matrix deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, Vector gravity, Vector omegaCoriolis) const; }; +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; +}; + #include class AHRSFactorPreintegratedMeasurements { // Standard Constructor @@ -2461,7 +2510,6 @@ class AHRSFactorPreintegratedMeasurements { bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); // get Data - Matrix measurementCovariance() const; Matrix deltaRij() const; double deltaTij() const; Vector biasHat() const; @@ -2488,64 +2536,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { Vector omegaCoriolis) const; }; -#include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class CombinedImuFactorPreintegratedMeasurements { - // Standard Constructor - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - - Matrix measurementCovariance() const; - Matrix deltaRij() const; - double deltaTij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHat() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() const; -}; - -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - - // Standard Interface - gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis); -}; - #include //virtual class AttitudeFactor : gtsam::NonlinearFactor { // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 195490e87..2f03d72a4 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -18,9 +18,9 @@ **/ #include +#include -/* External or standard includes */ -#include +using namespace std; namespace gtsam { @@ -29,47 +29,35 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias), deltaTij_(0.0) { - measurementCovariance_ << measuredOmegaCovariance; - delRdelBiasOmega_.setZero(); + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()), deltaTij_(0.0) { - measurementCovariance_.setZero(); - delRdelBiasOmega_.setZero(); - delRdelBiasOmega_.setZero(); + PreintegratedRotation(I_3x3), biasHat_(Vector3()) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" - << std::endl; - std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; +void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegratedRotation::print(s); + cout << "biasHat [" << biasHat_.transpose() << "]" << endl; + cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return equal_with_abs_tol(biasHat_, other.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol) - && deltaRij_.equals(other.deltaRij_, tol) - && std::fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); + return PreintegratedRotation::equals(other, tol) + && equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_.setZero(); + PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } @@ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { - // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; @@ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; + Matrix3 D_Rincr_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! - // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); - const Matrix3 incrRt = incrR.transpose(); + // Update Jacobian + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); - // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); - - // Update Jacobians - // --------------------------------------------------------------------------- - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - // --------------------------------------------------------------------------- - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i); - - Rot3 Rot_j = deltaRij_ * incrR; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first - // order propagation that can be seen as a prediction phase in an EKF framework - Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11 - // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrpt preintegrated measurements (df/dx) - const Matrix3& F = H_angles_angles; + // Update rotation and deltaTij. + Matrix3 Fr; // Jacobian of the update + updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = F * preintMeasCov_ * F.transpose() - + measurementCovariance_ * deltaT; - - // Update preintegrated measurements - // --------------------------------------------------------------------------- - deltaRij_ = deltaRij_ * incrR; - deltaTij_ += deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + + gyroscopeCovariance() * deltaT; } //------------------------------------------------------------------------------ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - if (H) { - const Matrix3 Jrinv_theta_bc = // - Rot3::LogmapDerivative(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - } - return theta_biascorrected; + return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( @@ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Z_3x3) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const Vector3& omegaCoriolis, boost::optional body_P_sensor) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } @@ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { } //------------------------------------------------------------------------------ -void AHRSFactor::print(const std::string& s, +void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; + _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: "); @@ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + 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_ @@ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, +Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = // - preintegratedMeasurements_.predict(bias, H3); + const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); - const Vector3 theta_corrected = theta_biascorrected - coriolis; + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); // Get error between actual and prediction - const Rot3 actualRij = rot_i.between(rot_j); - const Rot3 fRhat = deltaRij_corrected.between(actualRij); - Vector3 fR = Rot3::Logmap(fRhat); + const Rot3 actualRij = Ri.between(Rj); + const Rot3 fRrot = correctedDeltaRij.between(actualRij); + Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; (*H1) - << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); + << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << Jrinv_fRhat * Matrix3::Identity(); + (*H2) << D_fR_fRrot * Matrix3::Identity(); } if (H3) { // dfR/dBias, note H3 contains derivative of predict - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); H3->resize(3, 3); - (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); } Vector error(3); @@ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); + const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term const Vector3 coriolis = // preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - return rot_i.compose(deltaRij_corrected); + return rot_i.compose(correctedDeltaRij); } } //namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e62a24cca..fd4b9be87 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,6 +20,7 @@ #pragma once /* GTSAM includes */ +#include #include #include @@ -35,17 +36,12 @@ public: * Can be built incrementally so as to avoid costly integration at time of * factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements : public PreintegratedRotation { friend class AHRSFactor; protected: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -61,31 +57,19 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); + Vector3 biasHat() const { + return biasHat_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; /// equals bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - double deltaTij() const { - return deltaTij_; - } - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } - /// TODO: Document void resetIntegration(); @@ -103,12 +87,6 @@ public: Vector3 predict(const Vector3& bias, boost::optional H = boost::none) const; - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij_; - } - // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, @@ -120,11 +98,8 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; @@ -132,7 +107,7 @@ private: typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; Vector3 gravity_; Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -178,7 +153,7 @@ public: /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -208,7 +183,7 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 10f12ec66..fd761388a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -36,197 +36,136 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), + biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), + biasAccOmegaInit_(biasAccOmegaInit) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; - measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; - measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); - cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; + 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 biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(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); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; - PreintMeasCov_.setZero(); + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor) { + double deltaT, boost::optional body_P_sensor, + boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, 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 Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); + const Matrix3 R_i = deltaRij(); // store this + // 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_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + Matrix3 H_vel_biasacc = - R_i * deltaT; + Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + // for documentation: + // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, + // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, + // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + F.setZero(); + F.block<9,9>(0,0) = F_9x9; + F.block<6,6>(9,9) = I_6x6; + F.block<3,3>(3,9) = H_vel_biasacc; + F.block<3,3>(6,12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); +// BLOCK DIAGONAL TERMS + G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * (H_angles_biasomega.transpose()); - - G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); - - G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); - - // NEW OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; + // OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + // F_test and G_test are used for testing purposes and are not needed by the factor + if(F_test){ + F_test->resize(15,15); + (*F_test) << F; + } + if(G_test){ + G_test->resize(15,21); + // This is for testing & documentation + ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, 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), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(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) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -243,22 +182,17 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -268,230 +202,69 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ boost::optional H3, boost::optional H4, boost::optional H5, boost::optional H6) const { - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + // if we need the jacobians + if(H1 || H2 || H3 || H4 || H5 || H6){ + Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); + // error wrt preintegrated measurements + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // + H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + if(H1) { + H1->resize(15,6); + H1->block<9,6>(0,0) = H1_pvR; + // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] + H1->block<6,6>(9,0) = Z_6x6; } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; + if(H2) { + H2->resize(15,3); + H2->block<9,3>(0,0) = H2_pvR; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H2->block<6,3>(9,0) = Matrix::Zero(6,3); } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3, - //dBiasAcc/dPi - Z_3x3, Z_3x3, - //dBiasOmega/dPi - Z_3x3, Z_3x3; + if(H3) { + H3->resize(15,6); + H3->block<9,6>(0,0) = H3_pvR; + // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] + H3->block<6,6>(9,0) = Z_6x6; + } + if(H4) { + H4->resize(15,3); + H4->block<9,3>(0,0) = H4_pvR; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H4->block<6,3>(9,0) = Matrix::Zero(6,3); + } + if(H5) { + H5->resize(15,6); + H5->block<9,6>(0,0) = H5_pvR; + // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] + H5->block<6,6>(9,0) = Hbias_i; + } + if(H6) { + H6->resize(15,6); + H6->block<9,6>(0,0) = Matrix::Zero(9,6); + // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] + H6->block<6,6>(9,0) = Hbias_j; + } + Vector r(15); r << r_pvR, fbias; // vector of size 15 + return r; } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3, - //dBiasAcc/dVi - Z_3x3, - //dBiasOmega/dVi - Z_3x3; - } - - if(H3) { - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3, - //dBiasAcc/dPosej - Z_3x3, Z_3x3, - //dBiasOmega/dPosej - Z_3x3, Z_3x3; - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3, - //dBiasAcc/dVj - Z_3x3, - //dBiasOmega/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -I_3x3, Z_3x3, - //dBiasOmega/dBias_i - Z_3x3, -I_3x3; - } - - if(H6) { - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Z_3x3, Z_3x3, - // dfV/dBias_j - Z_3x3, Z_3x3, - // dfR/dBias_j - Z_3x3, Z_3x3, - //dBiasAcc/dBias_j - I_3x3, Z_3x3, - //dBiasOmega/dBias_j - Z_3x3, I_3x3; - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - + // else, only compute the error vector: + // error wrt preintegrated measurements + Vector r_pvR(9); + r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // + boost::none, boost::none, boost::none, boost::none, boost::none); + // error wrt bias evolution model (random walk) + Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + // overall error + Vector r(15); r << r_pvR, fbias; // vector of size 15 return r; } -//------------------------------------------------------------------------------ -PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); -} - } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5e2bfeadb..a7c6ecd39 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,7 +23,8 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { @@ -33,78 +34,63 @@ namespace gtsam { * @addtogroup SLAM * * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * Struct to hold all state variables of CombinedImuFactor returned by Predict function + * CombinedImuFactor is a 6-ways factor involving previous state (pose and + * velocity of the vehicle, as well as bias at previous time step), and current + * state (pose, velocity, bias at current time step). Following the pre- + * integration scheme proposed in [2], the CombinedImuFactor includes many IMU + * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * class. There are 3 main differences wrpt the ImuFactor class: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous + * and current time step).Therefore, the factor internally imposes the biases + * to be slowly varying; in particular, the matrices "biasAccCovariance" and + * "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias + * estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * the correlation between the bias uncertainty and the preintegrated + * measurements uncertainty. */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - -/** - * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias - * at previous time step), and current state (pose, velocity, bias at current time step). According to the - * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are - * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: - * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). - * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices - * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. - * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. - * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty - * and the preintegrated measurements uncertainty. - */ -class CombinedImuFactor: public NoiseModelFactor6 { +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + /** + * CombinedPreintegratedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. */ - class CombinedPreintegratedMeasurements { + class CombinedPreintegratedMeasurements: public PreintegrationBase { friend class CombinedImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Matrix3 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 - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases - bool use2ndOrderIntegration_; ///< Controls the order of integration - public: /** @@ -141,60 +127,20 @@ public: * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + Matrix preintMeasCov() const { return preintMeasCov_;} private: - /** Serialization function */ + + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; @@ -203,12 +149,7 @@ private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + CombinedPreintegratedMeasurements _PIM_; public: @@ -257,11 +198,7 @@ public: /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -275,12 +212,6 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - private: /** Serialization function */ @@ -289,7 +220,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index cdebffa63..0aaa0122e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -35,165 +35,82 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, + measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - PreintMeasCov_.setZero(9,9); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); - cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; + PreintegrationBase::print(s); + cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; - PreintMeasCov_.setZero(); + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { + boost::optional body_P_sensor, + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { - // NOTE: order is important here because each update uses old values (i.e., we have to update - // jacobians and covariances before updating preintegrated measurements). + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); - // Update preintegrated measurements covariance + // Update preintegrated measurements (also get Jacobian) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + + // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); - - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation: - // the deltaT allows to pass from continuous time noise to discrete time noise + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, + // as G and measurementCovariance are blockdiagonal matrices + preintMeasCov_ = F * preintMeasCov_ * F.transpose(); + preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; + preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only kept for documentation. - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + // F_test and G_test are given as output for testing purposes and are not needed by the factor + if(F_test){ // This in only for testing + (*F_test) << F; + } + if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise + // intNoise accNoise omegaNoise + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -202,13 +119,10 @@ ImuFactor::ImuFactor( const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(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) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -224,215 +138,28 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ -Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5) const -{ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4, boost::optional H5) const { - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3; - } - - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; -} - -//------------------------------------------------------------------------------ -PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) -{ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2af634926..b91c76ade 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,7 +23,8 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { @@ -38,66 +39,46 @@ namespace gtsam { * Int. Conf. on Robotics and Automation (ICRA), 2014. * ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * Struct to hold return variables by the Predict Function + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of + * the vehicle at previous time step), current state (pose and velocity at + * current time step), and the bias estimate. Following the preintegration + * scheme proposed in [2], the ImuFactor includes many IMU measurements, which + * are "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not model "temporal consistency" of the biases + * (which are usually slowly varying quantities), which is up to the caller. + * See also CombinedImuFactor for a class that does this for you. */ -struct PoseVelocity { - Pose3 pose; - Vector3 velocity; - PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { - } -}; - -/** - * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), - * current state (pose and velocity at current time step), and the bias estimate. According to the - * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are - * "summarized" using the PreintegratedMeasurements class. - * Note that this factor does not force "temporal consistency" of the biases (which are usually - * slowly varying quantities), see also CombinedImuFactor for more details. - */ -class ImuFactor: public NoiseModelFactor5 { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** * PreintegratedMeasurements accumulates (integrates) the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements: public PreintegrationBase { friend class ImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). - bool use2ndOrderIntegration_; ///< Controls the order of integration - - public: + public: /** * Default constructor, initializes the class with no measurements @@ -127,160 +108,107 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time interval between two consecutive IMU measurements * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + * @param Fout, Gout Jacobians used internally (only needed for testing) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; + Matrix preintMeasCov() const { return preintMeasCov_;} private: - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - - /** Default constructor - only use for serialization */ - ImuFactor(); - - /** - * Constructor - * @param pose_i Previous pose key - * @param vel_i Previous velocity key - * @param pose_j Current pose key - * @param vel_j Current velocity key - * @param bias Previous bias key - * @param preintegratedMeasurements Preintegrated IMU measurements - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect - */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); - - virtual ~ImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; - - /** implement functions needed for Testable */ - - /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; - - /** Access the preintegrated measurements. */ - - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; - - /// predicted states from IMU - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - - private: - - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } - }; // class ImuFactor + }; - typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; + private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements _PIM_; + + public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + ImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + + /** Access the preintegrated measurements. */ + + const PreintegratedMeasurements& preintegratedMeasurements() const { + return _PIM_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_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 new file mode 100644 index 000000000..1b7919a82 --- /dev/null +++ b/gtsam/navigation/ImuFactorBase.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 new file mode 100644 index 000000000..731e11f2e --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.h @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegratedRotation.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * PreintegratedRotation is the base class for all PreintegratedMeasurements + * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). + * It includes the definitions of the preintegrated rotation. + */ +class PreintegratedRotation { + + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + /// Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements + +public: + + /** + * Default constructor, initializes the variables in the base class + */ + PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : + deltaRij_(Rot3()), deltaTij_(0.0), + delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} + + /// methods to access class variables + Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive + Vector3 thetaRij(boost::optional H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive + const double& deltaTij() const{return deltaTij_;} + const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; + deltaRij_.print(" deltaRij "); + std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; + std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; + } + + /// Needed for testable + bool equals(const PreintegratedRotation& expected, double tol) const { + return deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) + && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, + OptionalJacobian<3, 3> H = boost::none){ + deltaRij_ = deltaRij_.compose(incrR, H, boost::none); + deltaTij_ += deltaT; + } + + /** + * Update Jacobians to be used during preintegration + * TODO: explain arguments + */ + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT) { + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; + } + + /// Return a bias corrected version of the integrated rotation - expensive + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { + return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + } + + /// Get so<3> version of bias corrected rotation, with optional Jacobian + // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) + Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = boost::none) const { + // First, we correct deltaRij using the biasOmegaIncr, rotated + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + + 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 Matrix3 Jr_JbiasOmegaIncr = // + Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); + (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + return biascorrectedOmega; + } + //else + return Rot3::Logmap(deltaRij_biascorrected); + } + + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i, + const Vector3& omegaCoriolis) const { + return rot_i.transpose() * omegaCoriolis * deltaTij(); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h new file mode 100644 index 000000000..8214c1930 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.h @@ -0,0 +1,425 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#include +#include + +namespace gtsam { + +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } +}; + +/** + * PreintegrationBase is the base class for PreintegratedMeasurements + * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). + * It includes the definitions of the preintegrated variables and the methods + * to access, print, and compare them. + */ +class PreintegrationBase : public PreintegratedRotation { + + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + +public: + + /** + * Default constructor, initializes the variables in the base class + * @param bias Current estimate of acceleration and rotation rate biases + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegrationBase(const imuBias::ConstantBias& bias, + const 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) {} + + /// methods to access class variables + 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 Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} + const Matrix3& integrationCovariance() const { return integrationCovariance_;} + + /// Needed for testable + void print(const std::string& s) const { + PreintegratedRotation::print(s); + std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; + std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + biasHat_.print(" biasHat"); + } + + /// Needed for testable + bool equals(const PreintegrationBase& expected, double tol) const { + return PreintegratedRotation::equals(expected, tol) + && biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { + + Matrix3 dRij = deltaRij(); // expensive + Vector3 temp = dRij * correctedAcc * deltaT; + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; + } + deltaVij_ += temp; + + Matrix3 R_i, F_angles_angles; + if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); + + if(F){ + Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; + // pos vel angle + *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles;// angle + } + } + + /// Update Jacobians to be used during preintegration + void updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ + Matrix3 dRij = deltaRij(); // expensive + Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); + } + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); + } + + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + } + + /// 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, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) const { + + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; + if(deltaPij_biascorrected_out)// if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected + + vel_i * deltaTij() + - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij()*deltaTij(); + + Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; + if(deltaVij_biascorrected_out)// if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected + - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + + gravity * deltaTij()); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + Vector3 correctedOmega = biascorrectedOmega - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + const Rot3 correctedDeltaRij = + Rot3::Expmap( correctedOmega ); + const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant + } + + /// 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, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const { + + // We need the mismatch w.r.t. the biases used for preintegration + // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Ri = pose_i.rotation(); + const Rot3& Rj = pose_j.rotation(); + const Vector3& pos_j = pose_j.translation().vector(); + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + + // Jacobian computation + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Matrix3 D_cThetaRij_biasOmegaIncr; + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); + + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + Vector3 correctedOmega = biascorrectedOmega - coriolis; + + Rot3 correctedDeltaRij, fRrot; + Vector3 fR; + + // Accessory matrix, used to build the jacobians + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; + + // This is done to save computation: we ask for the jacobians only when they are needed + if(H1 || H2 || H3 || H4 || H5){ + correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); + }else{ + correctedDeltaRij = Rot3::Expmap( correctedOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot); + } + if(H1) { + H1->resize(9,6); + Matrix3 dfPdPi = -I_3x3; + Matrix3 dfVdPi = Z_3x3; + if(use2ndOrderCoriolis){ + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); + dfVdPi += temp * deltaTij(); + } + (*H1) << + // dfP/dRi + skewSymmetric(fp + deltaPij_biascorrected), + // dfP/dPi + dfPdPi, + // dfV/dRi + skewSymmetric(fv + deltaVij_biascorrected), + // dfV/dPi + dfVdPi, + // dfR/dRi + D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), + // dfR/dPi + Z_3x3; + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - Ri.transpose() * deltaTij() + + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Ri.transpose() + + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term + // dfR/dVi + Z_3x3; + } + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Ri.transpose() * Rj.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + D_fR_fRrot * ( I_3x3 ), Z_3x3; + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + Ri.transpose(), + // dfR/dVj + Z_3x3; + } + if(H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; + H5->resize(9,6); + (*H5) << + // dfP/dBias + - delPdelBiasAcc(), - delPdelBiasOmega(), + // dfV/dBias + - delVdelBiasAcc(), - delVdelBiasOmega(), + // dfR/dBias + Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); + } + Vector9 r; r << fp, fv, fR; + return r; + } + +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_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + } +}; + +class ImuBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + ImuBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 99952f99e..0d7626b95 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias; // Bias + Vector3 bias(0.,0.,0.); // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aab38f258..d14eafb7d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,19 +39,55 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -/* ************************************************************************* */ namespace { +/* ************************************************************************* */ +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedMeasurementsTest( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more + imuBias::ConstantBias bias_new(bias_old); + Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + return result; +} + +Rot3 updatePreintegratedMeasurementsRot( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration){ + + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, + bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); + + return Rot3::Expmap(result.segment<3>(6)); +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + const list& deltaTs){ + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -59,7 +95,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -67,20 +102,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); + measuredAccs, measuredOmegas, deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); } @@ -89,9 +120,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij()); } @@ -101,7 +130,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases @@ -120,28 +148,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); -// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases -// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) -// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) -// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); -// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); -// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); -// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); + EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); + EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); @@ -157,50 +174,37 @@ TEST( CombinedImuFactor, ErrorWithBiases ) double deltaT = 1.0; double tol = 1e-6; - // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - Matrix I6x6(6,6); I6x6 = Matrix::Identity(6,6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - - // Actual Jacobians + // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); @@ -214,7 +218,6 @@ TEST( CombinedImuFactor, ErrorWithBiases ) /* ************************************************************************* */ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases @@ -237,22 +240,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -265,6 +268,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -283,22 +287,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); - + // Create factor + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; expectedVelocity<<0,1,0; + EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) Matrix I6x6(6,6); @@ -319,14 +322,152 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } -#include +/* ************************************************************************* */ +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; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected F wrt positions (15,3) + Matrix df_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + _1, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // rotation part has to be done properly, on manifold + df_dpos.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + _1, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + + // Compute expected F wrt velocities (15,3) + Matrix df_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, _1, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // rotation part has to be done properly, on manifold + df_dvel.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, _1, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + + // Compute expected F wrt angles (15,3) + Matrix df_dangle = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + // rotation part has to be done properly, on manifold + df_dangle.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + + // Compute expected F wrt biases (15,6) + Matrix df_dbias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dbias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + + Matrix Fexpected(15,15); + Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected G wrt integration noise + Matrix df_dintNoise(15,3); + df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; + + // Compute expected G wrt acc noise (15,3) + Matrix df_daccNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + // rotation part has to be done properly, on manifold + df_daccNoise.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected G wrt gyro noise (15,3) + Matrix df_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + // rotation part has to be done properly, on manifold + df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + + // Compute expected G wrt bias random walk noise (15,6) + Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries + df_rwBias.setZero(); + df_rwBias.block<6,6>(9,0) = eye(6); + + // Compute expected G wrt gyro noise (15,6) + Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dinitBias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + + Matrix Gexpected(15,21); + Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 06e2c105e..0c4c9e3a6 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -37,6 +37,8 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias){ @@ -49,14 +51,48 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedPosVel( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration_) { + + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration_){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + + Vector result(6); result << deltaPij_new, deltaVij_new; + return result; +} + +Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, + const Vector3& correctedOmega, const double deltaT) { + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + return deltaRij_new; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), + omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity()); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -152,7 +188,7 @@ TEST( ImuFactor, PreintegratedMeasurements ) } /* ************************************************************************* */ -TEST( ImuFactor, Error ) +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias @@ -180,6 +216,77 @@ TEST( ImuFactor, Error ) Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + // Expected Jacobians + /////////////////// H1 /////////////////////////// + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); + EXPECT(assert_equal(H1e, H1a)); + + /////////////////// H2 /////////////////////////// + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + EXPECT(assert_equal(H2e, H2a)); + + /////////////////// H3 /////////////////////////// + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); + EXPECT(assert_equal(H3e, H3a)); + + /////////////////// H4 /////////////////////////// + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + EXPECT(assert_equal(H4e, H4a)); + + /////////////////// H5 /////////////////////////// + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorAndJacobianWithBiases ) +{ + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); @@ -197,45 +304,27 @@ TEST( ImuFactor, Error ) boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // positions and velocities - Matrix H1etop6 = H1e.topRows(6); - Matrix H1atop6 = H1a.topRows(6); - EXPECT(assert_equal(H1etop6, H1atop6)); - // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H2e, H2a)); - - // positions and velocities - Matrix H3etop6 = H3e.topRows(6); - Matrix H3atop6 = H3a.topRows(6); - EXPECT(assert_equal(H3etop6, H3atop6)); - // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H4e, H4a)); - // EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiases ) +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { - // Linearization point -// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) -// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1(Vector3(0.5, 0.0, 0.0)); -// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements @@ -245,56 +334,57 @@ TEST( ImuFactor, ErrorWithBiases ) Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); - // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeExpmap ) +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias @@ -324,20 +414,14 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Measurements Vector3 deltatheta; deltatheta << 0, 0, 0; - // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); - const Matrix3 actualDelFdeltheta = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - } /* ************************************************************************* */ @@ -354,7 +438,6 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -366,7 +449,7 @@ TEST( ImuFactor, fistOrderExponential ) hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - // Compare Jacobians + // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); } @@ -423,6 +506,128 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + + Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + + // Compute expected f_rot wrt angles + Matrix dfr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + _1, newMeasuredOmega, newDeltaT), deltaRij_old); + + Matrix FexpectedBottom3(3,9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6,3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected F wrt gyro noise + Matrix dgpv_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + Matrix GexpectedTop6(6,9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + deltaRij_old, _1, newDeltaT), newMeasuredOmega); + + Matrix GexpectedBottom3(3,9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix9 measurementCovariance; + measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, + Z_3x3, accNoiseVar*I_3x3, Z_3x3, + Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} + //#include ///* ************************************************************************* */ //TEST( ImuFactor, LinearizeTiming) @@ -561,13 +766,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); - - } /* ************************************************************************* */ @@ -595,7 +798,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose));