From b721a7ce1f22b032543ebeafa1fd1c0c7ac0a913 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Sat, 12 Jul 2014 20:42:42 -0400 Subject: [PATCH] Added tests in testAHRSFactor and corrected AHRSFactor so that it works. added target in .cproject. Note that not all tests work. In particular the IMUbias jacobian fails because the dimensions of expected and actual are different. --- .cproject | 122 +++--- gtsam/navigation/AHRSFactor.h | 341 +++++++++++------ gtsam/navigation/tests/testAHRSFactor.cpp | 439 ++++++++++++++++++++++ 3 files changed, 731 insertions(+), 171 deletions(-) create mode 100644 gtsam/navigation/tests/testAHRSFactor.cpp 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);} +/* ************************************************************************* */