diff --git a/.cproject b/.cproject index 62330cdbc..946897361 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -584,7 +582,6 @@ make - tests/testBayesTree.run true false @@ -592,7 +589,6 @@ make - testBinaryBayesNet.run true false @@ -640,7 +636,6 @@ make - testSymbolicBayesNet.run true false @@ -648,7 +643,6 @@ make - tests/testSymbolicFactor.run true false @@ -656,7 +650,6 @@ make - testSymbolicFactorGraph.run true false @@ -672,7 +665,6 @@ make - tests/testBayesTree true false @@ -902,6 +894,14 @@ true true + + make + -j5 + testAHRSFactor.run + true + true + true + make -j5 @@ -1008,7 +1008,6 @@ make - testErrors.run true false @@ -1238,46 +1237,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 @@ -1360,6 +1319,7 @@ make + testSimulated2DOriented.run true false @@ -1399,6 +1359,7 @@ make + testSimulated2D.run true false @@ -1406,6 +1367,7 @@ make + testSimulated3D.run true false @@ -1419,6 +1381,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 @@ -1676,7 +1678,6 @@ cpack - -G DEB true false @@ -1684,7 +1685,6 @@ cpack - -G RPM true false @@ -1692,7 +1692,6 @@ cpack - -G TGZ true false @@ -1700,7 +1699,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2427,7 +2425,6 @@ make - testGraph.run true false @@ -2435,7 +2432,6 @@ make - testJunctionTree.run true false @@ -2443,7 +2439,6 @@ make - testSymbolicBayesNetB.run true false @@ -2907,6 +2902,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index b013f3b33..4b2b1754b 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -17,9 +17,9 @@ namespace gtsam { -class AHRSFactor: public NoiseModelFactor3{ +class AHRSFactor: public NoiseModelFactor3 { public: - class PreintegratedMeasurements{ + class PreintegratedMeasurements { public: imuBias::ConstantBias biasHat; Matrix measurementCovariance; @@ -30,87 +30,121 @@ public: Matrix PreintMeasCov; bool use2ndOrderIntegration_; - - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, + PreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measurementAccCovariance, - const Matrix3& measurementGyroCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, - const bool use2ndOrderIntegration =false - ): biasHat(bias), measurementCovariance(9,9), delRdelBiasOmega(Matirx3::Zero()), PreintMeasCov(9,9), - use2ndOrderIntegration_(use2ndOrderIntegration) - { - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov = Matrix::Zero(9,9); + const bool use2ndOrderIntegration = false) : + biasHat(bias), measurementCovariance(9, 9), delRdelBiasOmega( + Matrix3::Zero()), PreintMeasCov(9, 9), use2ndOrderIntegration_( + use2ndOrderIntegration) { + measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; + PreintMeasCov = Matrix::Zero(9, 9); } - PreintegratedMeasurements(): - biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) - { - measurementCovariance = Matrix::Zero(9,9); - PreintMeasCov = Matrix::Zero(9,9); + PreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(9, 9), delRdelBiasOmega( + Matrix3::Zero()), PreintMeasCov(9, 9) { + measurementCovariance = Matrix::Zero(9, 9); + PreintMeasCov = Matrix::Zero(9, 9); } - void print (const std::string& s = "Preintegrated Measurements: ") const { - std::cout< body_P_sensor = boost::none - ){ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none) { Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc); Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega); - if(body_P_sensor) { + if (body_P_sensor) { Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); correctedOmega = body_R_sensor * correctedOmega; 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(); + correctedAcc = body_R_sensor * correctedAcc + - body_omega_body_cross * body_omega_body_cross + * body_P_sensor->translation().vector(); } const Vector3 theta_incr = correctedOmega * deltaT; const Rot3 Rincr = Rot3::Expmap(theta_incr); const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); - delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; + delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega + - Jr_theta_incr * deltaT; - Matrix3 Z_3x3 = Matrix::Zero(); - Matrix3 I_3x3 = Matrix::Identity(); + // Matrix3 Z_3x3 = Matrix::Zero(); + // Matrix3 I_3x3 = Matrix::Identity(); const Vector3 theta_i = Rot3::Logmap(deltaRij); - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix F(3,3); - F< + void serialize(ARCHIVE & ar, const unsigned int version) { + 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); + } }; private: @@ -127,117 +161,208 @@ 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; + typedef typename boost::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ - AHRSFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} + AHRSFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero()) { + } - AHRSFactor( - Key rot_i, - Key rot_j, - Key bias, + AHRSFactor(Key rot_i, Key rot_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, - const Vector3& omegaCoriolis, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false - ): - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), Rot_i, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){} + const bool use2ndOrderCoriolis = false) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements), gravity_(gravity), omegaCoriolis_( + omegaCoriolis), body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_( + use2ndOrderCoriolis) { + } + + virtual ~AHRSFactor() {} - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout<key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << ","; + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr( + new This(*this) + ) + ); + } + + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) + << ","; preintegratedMeasurements_.print(" preintegrated measurements:"); std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" + << std::endl; this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) + if (this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); } - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); return e != NULL && Base::equals(*e, tol) && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(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_))); + && ((!body_P_sensor_ && !e->body_P_sensor_) + || (body_P_sensor_ && e->body_P_sensor_ + && body_P_sensor_->equals(*e->body_P_sensor_))); } /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + return preintegratedMeasurements_; + } - const Vector3& gravity() const { return gravity_; } + const Vector3& gravity() const { + return gravity_; + } - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } - - Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none) const + Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + const double& deltaTij = preintegratedMeasurements_.deltaTij; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope(); +// 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 Rot3 Rot_i = rot_i; + const Rot3 Rot_j = rot_j; // We compute factor's Jacobians /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); + 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 + 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 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( + theta_biascorrected_corioliscorrected); - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between( + Rot_i.between(Rot_j)); - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3( + theta_biascorrected_corioliscorrected); - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + const Matrix3 Jtheta = -Jr_theta_bcc + * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse( + Rot3::Logmap(fRhat)); - if(H1) { - H1->resize(3,3); - (*H1)<resize(3, 3); + (*H1) << // dfR/dRi + Jrinv_fRhat + * (-Rot_j.between(Rot_i).matrix() + - fRhat.inverse().matrix() * Jtheta); } - if(H2) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; - - H5->resize(3,3); - (*H5) << - // dfR/dBias - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + H2->resize(3,3); + (*H2) << + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ); } + if (H3) { + + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse( + theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3( + preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc + * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; + + H3->resize(3, 3); + (*H3) << + // dfR/dBias + Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega); + } const Vector3 fR = Rot3::Logmap(fRhat); - Vector r(3); r << fR; + Vector r(3); + r << fR; return r; } -}; // AHRSFactor + + /** predicted states from IMU */ + static void Predict(const Rot3& rot_i, const Rot3& rot_j, + const imuBias::ConstantBias& bias, + const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { + + const double& deltaTij = preintegratedMeasurements.deltaTij; +// const Vector3 biasAccIncr = bias.accelerometer() + - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() + - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = rot_i; + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + + const Rot3 deltaRij_biascorrected = + preintegratedMeasurements.deltaRij.retract( + preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, + Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected + - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap( + theta_biascorrected_corioliscorrected); + const Rot3 Rot_j = Rot_i.compose(deltaRij_biascorrected_corioliscorrected); + + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor3", + 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_); + } +}; +// AHRSFactor typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; } //namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp new file mode 100644 index 000000000..090baf8ab --- /dev/null +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -0,0 +1,439 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImuFactor.cpp + * @brief Unit test for ImuFactor + * @author Luca Carlone, Stephen Williams, Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + + +/* ************************************************************************* */ +namespace { +Vector callEvaluateError(const AHRSFactor& factor, + const Rot3 rot_i, const Rot3 rot_j, + const imuBias::ConstantBias& bias) +{ + return factor.evaluateError(rot_i, rot_j, bias); +} + +Rot3 evaluateRotationError(const AHRSFactor& factor, + const Rot3 rot_i, const Rot3 rot_j, + const imuBias::ConstantBias& bias) +{ + return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3) ) ; +} + +AHRSFactor::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) + ) +{ + AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity()); + + list::const_iterator itAcc = measuredAccs.begin(); + list::const_iterator itOmega = measuredOmegas.begin(); + list::const_iterator itDeltaT = deltaTs.begin(); + for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); + } + + return result; +} + +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) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij; +} + +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) +{ + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); +} + + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) +{ + return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +} + +} +/* ************************************************************************* */ +TEST( AHRSFactor, PreintegratedMeasurements ) +{ + // Linearization point + imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + double deltaT = 0.5; + + // Expected preintegrated values + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT1(0.5); + + bool use2ndOrderIntegration = true; + // Actual preintegrated values + AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6); + + // Integrate again + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT2(1); + + // Actual preintegrated values + AHRSFactor::PreintegratedMeasurements actual2 = actual1; + actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6); +} + +/* ************************************************************************* */ +TEST( ImuFactor, Error ) +{ + // Linearization point + imuBias::ConstantBias bias; // 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)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 measuredAcc = x1.unrotate(-Point3(gravity)).vector(); + double deltaT = 1.0; + bool use2ndOrderIntegration = true; + AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis, false); + + Vector errorActual = factor.evaluateError(x1, x2, bias); + + // Expected error + Vector errorExpected(3); errorExpected << 0, 0, 0; + EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + Matrix RH2e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + + // Actual Jacobians + Matrix H1a, H2a, H3a; + (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); + + + // rotations + EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations + + EXPECT(assert_equal(H2e, H2a, 1e-5)); + + // rotations + EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations + +// EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions. +} + + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorWithBiases ) +{ + // Linearization point +// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) +// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); +// LieVector v1((Vector(3) << 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)); +// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + + + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0))); + Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.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.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + AHRSFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + +// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); +// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, x2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error + Vector errorExpected(3); errorExpected << 0, 0, 0; +// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + Matrix RH2e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a; + (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); +// EXPECT(assert_equal(H3e, H3a)); +} + + +/* ************************************************************************* */ +TEST( AHRSFactor, PartialDerivativeExpmap ) +{ + // Linearization point + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 0.5; + + + // Compute numerical derivatives + Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( + &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); + + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + + Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + + // Compare Jacobians + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + +} + +/* ************************************************************************* */ +TEST( AHRSFactor, PartialDerivativeLogmap ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 deltatheta; deltatheta << 0, 0, 0; + + + // Compute numerical derivatives + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + &evaluateLogRotation, thetahat, _1), LieVector(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; + +// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; +// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; + + // Compare Jacobians + EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); + +} + +/* ************************************************************************* */ +TEST( AHRSFactor, fistOrderExponential ) +{ + // Linearization point + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 1.0; + + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + + + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + + Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + + const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + + const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + + // Compare Jacobians + EXPECT(assert_equal(expectedRot, actualRot)); +} + +/* ************************************************************************* */ +TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + + Pose3 body_P_sensor(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 + AHRSFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // Compute numerical derivatives + Matrix expectedDelRdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations +} + + +/* ************************************************************************* */ +TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) +{ + + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) + Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0))); + Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.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.unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,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(), measuredOmega); + + + AHRSFactor::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); + + // Create factor + AHRSFactor factor(X(1), X(2), B(1), pre_int_data, gravity, omegaCoriolis); + + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); + + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); + Matrix RH2e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias); + + // Actual Jacobians + Matrix H1a, H2a, H3a; + (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); + + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); +// EXPECT(assert_equal(H3e, H3a)); +} + + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */