diff --git a/gtsam.h b/gtsam.h index a28bade6a..65c7ffcaa 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2079,8 +2079,9 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree { gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; - Matrix marginalCovariance(size_t key) const; + gtsam::Value calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; gtsam::VectorValues getDelta() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::Ordering getOrdering() const; @@ -2122,8 +2123,8 @@ class NonlinearISAM { #include #include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { +template +virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2133,8 +2134,8 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { #include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { +template +virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2144,8 +2145,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { +template +virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation @@ -2284,6 +2285,47 @@ pair load2D(string filename, pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { +#include + +virtual class ConstantBias : gtsam::Value { + // Standard Constructor + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + //************************************************************************* // Utilities //************************************************************************* diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 352bb1918..12e702b7c 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -70,21 +70,21 @@ namespace imuBias { const Vector3& gyroscope() const { return biasGyro_; } /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector correctAccelerometer(const Vector3& measurment, boost::optional H=boost::none) const { + Vector correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << -Matrix3::Identity(), Matrix3::Zero(); } - return measurment - biasAcc_; + return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector correctGyroscope(const Vector3& measurment, boost::optional H=boost::none) const { + Vector correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << Matrix3::Zero(), -Matrix3::Identity(); } - return measurment - biasGyro_; + return measurement - biasGyro_; } // // H1: Jacobian w.r.t. IMUBias @@ -148,30 +148,14 @@ namespace imuBias { inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& t2) const { return t2.vector() - vector(); } + inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } /// @} /// @name Group /// @{ - ConstantBias compose(const ConstantBias& b2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - - return ConstantBias(biasAcc_ + b2.biasAcc_, biasGyro_ + b2.biasGyro_); - } - - /** between operation */ - ConstantBias between(const ConstantBias& b2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - - return ConstantBias(b2.biasAcc_ - biasAcc_, b2.biasGyro_ - biasGyro_); - } + /** identity for group operation */ + static ConstantBias identity() { return ConstantBias(); } /** invert the object and yield a new one */ inline ConstantBias inverse(boost::optional H=boost::none) const { @@ -180,6 +164,25 @@ namespace imuBias { return ConstantBias(-biasAcc_, -biasGyro_); } + ConstantBias compose(const ConstantBias& b, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = gtsam::Matrix::Identity(6,6); + if(H2) *H2 = gtsam::Matrix::Identity(6,6); + + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } + + /** between operation */ + ConstantBias between(const ConstantBias& b, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::Matrix::Identity(6,6); + if(H2) *H2 = gtsam::Matrix::Identity(6,6); + + return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); + } + /// @} /// @name Lie Group /// @{ @@ -188,7 +191,7 @@ namespace imuBias { static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const ConstantBias& p) { return p.vector(); } + static inline Vector Logmap(const ConstantBias& b) { return b.vector(); } /// @} diff --git a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp index 6622e8bf9..4ba59d052 100644 --- a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -677,6 +677,74 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } + + + + +#include +/* ************************************************************************* */ +TEST( InertialNavFactor_GlobalVelocity, LinearizeTiming) +{ + gtsam::Key PoseKey1(11); + gtsam::Key PoseKey2(12); + gtsam::Key VelKey1(21); + gtsam::Key VelKey2(22); + gtsam::Key BiasKey1(31); + + double measurement_dt(0.1); + Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); + Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + + // Second test: zero angular motion, some acceleration - generated in matlab + Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1(3,0.5,-0.5,0.4); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Pose3 Pose2(R2, t2); + Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); + LieVector Vel2 = Vel1.compose( dv ); + imuBias::ConstantBias Bias1; + + Values values; + values.insert(PoseKey1, Pose1); + values.insert(PoseKey2, Pose2); + values.insert(VelKey1, Vel1); + values.insert(VelKey2, Vel2); + values.insert(BiasKey1, Bias1); + + Ordering ordering; + ordering.push_back(PoseKey1); + ordering.push_back(VelKey1); + ordering.push_back(BiasKey1); + ordering.push_back(PoseKey2); + ordering.push_back(VelKey2); + + GaussianFactorGraph graph; + gttic_(LinearizeTiming); + for(size_t i = 0; i < 100000; ++i) { + GaussianFactor::shared_ptr g = f.linearize(values, ordering); + graph.push_back(g); + } + gttoc_(LinearizeTiming); + tictoc_finishedIteration_(); + tictoc_print_(); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index dc764c164..dd3361fb2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -1019,10 +1019,10 @@ Values ISAM2::calculateEstimate() const { } /* ************************************************************************* */ -Matrix ISAM2::marginalCovariance(Index key) const { - return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) - ->information().inverse(); +const Value& ISAM2::calculateEstimate(Key key) const { + const Index index = getOrdering()[key]; + const Vector& delta = getDelta()[index]; + return *theta_.at(key).retract_(delta); } /* ************************************************************************* */ @@ -1032,6 +1032,13 @@ Values ISAM2::calculateBestEstimate() const { return theta_.retract(delta, ordering_); } +/* ************************************************************************* */ +Matrix ISAM2::marginalCovariance(Index key) const { + return marginalFactor(ordering_[key], + params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) + ->information().inverse(); +} + /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { if(!deltaUptodate_) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 687723392..fb7762ffa 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -585,6 +585,15 @@ public: template VALUE calculateEstimate(Key key) const; + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. This is a non-templated version + * that returns a Value base class for use with the MATLAB wrapper. + * @param key + * @return + */ + GTSAM_EXPORT const Value& calculateEstimate(Key key) const; + /** Return marginal on any variable as a covariance matrix */ GTSAM_EXPORT Matrix marginalCovariance(Index key) const; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index dd2671530..99964c9db 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -93,7 +93,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { /* ************************************************************************* */ PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV PoseRTV::inverse(boost::optional H1) const { - if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); + if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); return PoseRTV(Rt_.inverse(), v_.inverse()); } diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 2f4254cdc..061dd6ec3 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -13,6 +13,7 @@ virtual class gtsam::Point3; virtual class gtsam::Rot3; virtual class gtsam::Pose3; virtual class gtsam::noiseModel::Base; +virtual class gtsam::imuBias::ConstantBias; virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; @@ -614,6 +615,74 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor { InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; +#include +class ImuFactorPreintegratedMeasurements { + // Standard Constructor + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::ImuFactorPreintegratedMeasurements& 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); +}; + +virtual class ImuFactor : gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::noiseModel::Base* model); + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::noiseModel::Base* model, const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + const gtsam::imuBias::ConstantBias& bias, + const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector gravity, Vector omegaCoriolis) const; +}; + + +#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::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); +}; + +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, + const gtsam::noiseModel::Base* model); + + // Standard Interface + gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, + const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector gravity, Vector omegaCoriolis) const; +}; + + #include class Mechanization_bRn2 { Mechanization_bRn2(); diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h new file mode 100644 index 000000000..1879802da --- /dev/null +++ b/gtsam_unstable/slam/CombinedImuFactor.h @@ -0,0 +1,646 @@ +/* ---------------------------------------------------------------------------- + + * 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 CombinedImuFactor.h + * @author Luca Carlone, Stephen Williams, Richard Roberts + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * + * 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. + */ + + class CombinedImuFactor: public NoiseModelFactor6 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements 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*/ + class CombinedPreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + 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 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 + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + /** Default constructor, initialize with no IMU measurements */ + CombinedPreintegratedMeasurements( + 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 + ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) + ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + + } + + CombinedPreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + { + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) 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) + && std::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); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // 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); + + // 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 = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + // delPdelBiasAcc += delVdelBiasAcc * deltaT; + // delPdelBiasOmega += delVdelBiasOmega * deltaT; + 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; + + // 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 + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j); + + // 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); + + // 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; + + + // 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(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); + + G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); + + G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); + + G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); + + G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); + + // OFF BLOCK DIAGONAL TERMS + Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); + G_measCov_Gt.block(3,9,3,3) = block24; + G_measCov_Gt.block(9,3,3,3) = block24.transpose(); + + Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); + G_measCov_Gt.block(6,12,3,3) = block35; + G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + + PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // 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); + } + }; + + 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 + + public: + + /** Shorthand for a smart pointer to a factor */ + typedef typename boost::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} + + /** Constructor */ + 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, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor) { + } + + virtual ~CombinedImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "CombinedImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && 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_))); + } + + /** Access the preintegrated measurements. */ + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + 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, + boost::optional H6 = boost::none) const + { + + const double& deltaTij = preintegratedMeasurements_.deltaTij; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.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, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + 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 = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(15,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H2) { + H2->resize(15,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); + + } + + if(H3) { + + H3->resize(15,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), + //dBiasAcc/dPosej + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPosej + Matrix3::Zero(), Matrix3::Zero(); + } + + if(H4) { + H4->resize(15,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(), + //dBiasAcc/dVj + Matrix3::Zero(), + //dBiasOmega/dVj + Matrix3::Zero(); + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(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 + -Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_i + Matrix3::Zero(), -Matrix3::Identity(); + } + + if(H6) { + + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfV/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfR/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + //dBiasAcc/dBias_j + Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_j + Matrix3::Zero(), Matrix3::Identity(); + } + + + // 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 + + return r; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + { + + 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 + /* ---------------------------------------------------------------------------------------------------- */ + const 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; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + 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 ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + + bias_j = bias_i; + } + + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + 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(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } + }; // \class CombinedImuFactor + + typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; + +} /// namespace gtsam diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h new file mode 100644 index 000000000..4a8a84b6d --- /dev/null +++ b/gtsam_unstable/slam/ImuFactor.h @@ -0,0 +1,559 @@ +/* ---------------------------------------------------------------------------- + + * 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 ImuFactor.h + * @author Luca Carlone, Stephen Williams, Richard Roberts + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include +#include +#include +#include +#include + +/* External or standard includes */ +#include + + +namespace gtsam { + + /** + * + * @addtogroup SLAM + * * 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. + */ + + class ImuFactor: public NoiseModelFactor5 { + + public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jr; + if (normx < 10e-8){ + Jr = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian + } + return Jr; + } + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) { + // x is the axis-angle representation (exponential coordinates) for a rotation + double normx = norm_2(x); // rotation angle + Matrix3 Jrinv; + + if (normx < 10e-8){ + Jrinv = Matrix3::Identity(); + } + else{ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + Jrinv = Matrix3::Identity() + + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + } + return Jrinv; + } + + /** CombinedPreintegratedMeasurements 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*/ + class PreintegratedMeasurements { + public: + imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration + Matrix measurementCovariance; ///< (Raw measurements 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 + + Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + /** Default constructor, initialize with no IMU measurements */ + PreintegratedMeasurements( + 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 + ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + { + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; + PreintMeasCov = Matrix::Zero(9,9); + } + + PreintegratedMeasurements() : + biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), + delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), + delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + { + measurementCovariance = Matrix::Zero(9,9); + PreintMeasCov = Matrix::Zero(9,9); + } + + /** print */ + void print(const std::string& s = "Preintegrated Measurements:") const { + std::cout << s << std::endl; + biasHat.print(" biasHat"); + std::cout << " deltaTij " << deltaTij << std::endl; + std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl; + deltaRij.print(" deltaRij "); + std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl; + std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl; + } + + /** equals */ + bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) 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) + && std::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); + } + + /** Add a single IMU measurement to the preintegration. */ + void integrateMeasurement( + const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) + const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) + double deltaT, ///< Time step + boost::optional body_P_sensor = boost::none ///< Sensor frame + ) { + + // NOTE: order is important here because each update uses old values. + // 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 = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + // delPdelBiasAcc += delVdelBiasAcc * deltaT; + // delPdelBiasOmega += delVdelBiasOmega * deltaT; + 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; + + // Update preintegrated mesurements covariance + /* ----------------------------------------------------------------------------------------------------------------------- */ + Matrix3 Z_3x3 = Matrix3::Zero(); + Matrix3 I_3x3 = Matrix3::Identity(); + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) + const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(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 + 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() + measurementCovariance * deltaT ; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + deltaVij += deltaRij.matrix() * correctedAcc * deltaT; + deltaRij = deltaRij * Rincr; + deltaTij += deltaT; + } + + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // 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); + } + }; + + 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 + + public: + + /** Shorthand for a smart pointer to a factor */ + typedef typename boost::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} + + /** Constructor */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + Base(model, pose_i, vel_i, pose_j, vel_j, bias), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor) { + } + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "ImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && 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_))); + } + + /** Access the preintegrated measurements. */ + const PreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& 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 + { + + 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 = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + - Rot_i.matrix(), + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + Matrix3::Zero(), + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - Matrix3::Identity() * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Matrix3::Identity() + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); + + } + if(H3) { + + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Matrix3::Zero(), Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Matrix3::Zero(), + // dfV/dVj + Matrix3::Identity(), + // dfR/dVj + Matrix3::Zero(); + } + if(H5) { + + const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(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; + } + + + /** predicted states from IMU */ + static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + { + + const double& deltaTij = preintegratedMeasurements.deltaTij; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + const 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; + + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + 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 ); + + pose_j = Pose3( Rot_j, Point3(pos_j) ); + } + + + 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(preintegratedMeasurements_); + 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_unstable/slam/tests/testCombinedImuFactor.cpp b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp new file mode 100644 index 000000000..370ccc3b9 --- /dev/null +++ b/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp @@ -0,0 +1,298 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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 ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); +} + +Rot3 evaluateRotationError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +} + +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()); + + 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; +} + +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) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs, initialRotationRate).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) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaVij; +} + +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).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( 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 + + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + double deltaT = 0.5; + double tol = 1e-6; + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero()); + expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, + 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); +} + + +/* ************************************************************************* */ +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)); + LieVector v1(3, 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)); + LieVector v2(3, 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; + 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); + + 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); + + + // Create factor + noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + 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, Combinedmodel); + + + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + + + 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 + Matrix H1a, H2a, H3a, H4a, H5a, H6a; + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + + EXPECT(assert_equal(H1e, H1a.topRows(9))); + EXPECT(assert_equal(H2e, H2a.topRows(9))); + EXPECT(assert_equal(H3e, H3a.topRows(9))); + EXPECT(assert_equal(H4e, H4a.topRows(9))); + EXPECT(assert_equal(H5e, H5a.topRows(9))); +} + +/* ************************************************************************* */ +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) +{ + cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; + // 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 + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // Compute numerical derivatives + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), 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); + 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); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); + EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); + EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); +} + +#include + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testImuFactor.cpp b/gtsam_unstable/slam/tests/testImuFactor.cpp new file mode 100644 index 000000000..9601e778e --- /dev/null +++ b/gtsam_unstable/slam/tests/testImuFactor.cpp @@ -0,0 +1,569 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#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 ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); +} + +Rot3 evaluateRotationError(const ImuFactor& factor, + const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const imuBias::ConstantBias& bias) +{ + return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +} + +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()); + + 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; +} + +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) ) +{ + return evaluatePreintegratedMeasurements(bias, + 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) ) +{ + return evaluatePreintegratedMeasurements(bias, + measuredAccs, measuredOmegas, deltaTs).deltaVij; +} + +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( ImuFactor, 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 + Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaV1(0.05, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT1(0.5); + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6)); + EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij), 1e-6)); + EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6); + + // Integrate again + Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + double expectedDeltaT2(1); + + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements actual2 = actual1; + actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij), 1e-6)); + EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij), 1e-6)); + EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6)); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6); +} + +/* ************************************************************************* */ +TEST( ImuFactor, Error ) +{ + // Linearization point + imuBias::ConstantBias bias; // Bias + 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(3, 0.5, 0.0, 0.0); + Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + LieVector v2(3, 0.5, 0.0, 0.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.rotation().unrotate(-Point3(gravity)).vector(); + double deltaT = 1.0; + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + + // 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); + + // 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); + + // 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))); // evaluate only the rotation residue + + 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))); // evaluate only the rotation residue + + EXPECT(assert_equal(H4e, H4a)); +// EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +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(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(3, 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)); + LieVector v1(3, 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)); + LieVector v2(3, 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.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 + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // 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); + + // 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); + + 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 ) +{ + // 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), biasOmega); + + const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + + Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + + // Compare Jacobians + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega)); + +} + +/* ************************************************************************* */ +TEST( ImuFactor, 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), 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( ImuFactor, 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 = ImuFactor::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( ImuFactor, 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 + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // Compute numerical derivatives + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), 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); + 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); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); + + // Compare Jacobians + EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc)); + EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega)); + EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); + EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); +} + +//#include +///* ************************************************************************* */ +//TEST( ImuFactor, LinearizeTiming) +//{ +// // Linearization point +// 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(3, 0.5, 0.0, 0.0); +// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); +// LieVector v2(3, 0.5, 0.0, 0.0); +// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); +// +// // Pre-integrator +// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); +// Vector3 gravity; gravity << 0, 0, 9.81; +// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; +// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); +// +// // Pre-integrate Measurements +// Vector3 measuredAcc(0.1, 0.0, 0.0); +// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); +// double deltaT = 0.5; +// for(size_t i = 0; i < 50; ++i) { +// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// } +// +// // Create factor +// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance()); +// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); +// +// Values values; +// values.insert(X(1), x1); +// values.insert(X(2), x2); +// values.insert(V(1), v1); +// values.insert(V(2), v2); +// values.insert(B(1), bias); +// +// Ordering ordering; +// ordering.push_back(X(1)); +// ordering.push_back(V(1)); +// ordering.push_back(X(2)); +// ordering.push_back(V(2)); +// ordering.push_back(B(1)); +// +// GaussianFactorGraph graph; +// gttic_(LinearizeTiming); +// for(size_t i = 0; i < 100000; ++i) { +// GaussianFactor::shared_ptr g = factor.linearize(values, ordering); +// graph.push_back(g); +// } +// gttoc_(LinearizeTiming); +// tictoc_finishedIteration_(); +// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl; +// tictoc_print_(); +//} + + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) +{ + + 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)); + LieVector v1(3, 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)); + LieVector v2(3, 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; + + 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); + + + 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); + + // Create factor + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); + + // 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); + + // 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)); +} + + +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */