diff --git a/gtsam.h b/gtsam.h index 6b63e038b..a28bade6a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2080,7 +2080,6 @@ 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; gtsam::VectorValues getDelta() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; @@ -2123,8 +2122,8 @@ class NonlinearISAM { #include #include -template -virtual class PriorFactor : gtsam::NonlinearFactor { +template +virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2134,8 +2133,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor { #include -template -virtual class BetweenFactor : gtsam::NonlinearFactor { +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); T measured() const; @@ -2145,8 +2144,8 @@ virtual class BetweenFactor : gtsam::NonlinearFactor { #include -template -virtual class NonlinearEquality : gtsam::NonlinearFactor { +template +virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation @@ -2285,47 +2284,6 @@ 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/EquivInertialNavFactor_GlobalVel.h b/gtsam/navigation/EquivInertialNavFactor_GlobalVel.h index 650e11ff0..564f5a2c8 100644 --- a/gtsam/navigation/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam/navigation/EquivInertialNavFactor_GlobalVel.h @@ -282,12 +282,8 @@ public: // Predict POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); - // Luca: difference between Pose2 and Pose2Pred - POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() ); -// DiffPose = Pose2.between(Pose2Pred); - return DiffPose; // Calculate error - //return Pose2.between(Pose2Pred); + return Pose2.between(Pose2Pred); } VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { @@ -458,13 +454,10 @@ public: Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); - noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; - // Luca: force identity covariance matrix (for testing purposes) - // EquivCov_Overall = Matrix::Identity(15,15); // Update Jacobian_wrt_t0_Overall Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 12e702b7c..352bb1918 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& measurement, boost::optional H=boost::none) const { + Vector correctAccelerometer(const Vector3& measurment, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << -Matrix3::Identity(), Matrix3::Zero(); } - return measurement - biasAcc_; + return measurment - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { + Vector correctGyroscope(const Vector3& measurment, boost::optional H=boost::none) const { if (H){ H->resize(3,6); (*H) << Matrix3::Zero(), -Matrix3::Identity(); } - return measurement - biasGyro_; + return measurment - biasGyro_; } // // H1: Jacobian w.r.t. IMUBias @@ -148,14 +148,30 @@ 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& b) const { return b.vector() - vector(); } + inline Vector localCoordinates(const ConstantBias& t2) const { return t2.vector() - vector(); } /// @} /// @name Group /// @{ - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + 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_); + } /** invert the object and yield a new one */ inline ConstantBias inverse(boost::optional H=boost::none) const { @@ -164,25 +180,6 @@ 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 /// @{ @@ -191,7 +188,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& b) { return b.vector(); } + static inline Vector Logmap(const ConstantBias& p) { return p.vector(); } /// @} diff --git a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp index 4ba59d052..6622e8bf9 100644 --- a/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -677,74 +677,6 @@ 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 a9b58a5e1..dc764c164 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -1001,13 +1001,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const { deltaUptodate_ = true; } -/* ************************************************************************* */ -Matrix ISAM2::marginalCovariance(Index key) const { - return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) - ->information().inverse(); -} - /* ************************************************************************* */ Values ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former @@ -1026,10 +1019,10 @@ Values ISAM2::calculateEstimate() const { } /* ************************************************************************* */ -const Value& ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const Vector& delta = getDelta()[index]; - return *theta_.at(key).retract_(delta); +Matrix ISAM2::marginalCovariance(Index key) const { + return marginalFactor(ordering_[key], + params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) + ->information().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index fb7762ffa..687723392 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -585,15 +585,6 @@ 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 99964c9db..dd2671530 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 4b8a659d0..c4feeccef 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -13,7 +13,6 @@ 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; @@ -615,74 +614,6 @@ 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 deleted file mode 100644 index 1879802da..000000000 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ /dev/null @@ -1,646 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file 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 deleted file mode 100644 index 4a8a84b6d..000000000 --- a/gtsam_unstable/slam/ImuFactor.h +++ /dev/null @@ -1,559 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file 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 deleted file mode 100644 index 370ccc3b9..000000000 --- a/gtsam_unstable/slam/tests/testCombinedImuFactor.cpp +++ /dev/null @@ -1,298 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file 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 deleted file mode 100644 index 9601e778e..000000000 --- a/gtsam_unstable/slam/tests/testImuFactor.cpp +++ /dev/null @@ -1,569 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file 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);} -/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testImuFactorv1.cpp b/gtsam_unstable/slam/tests/testImuFactorv1.cpp deleted file mode 100644 index 3888e9685..000000000 --- a/gtsam_unstable/slam/tests/testImuFactorv1.cpp +++ /dev/null @@ -1,674 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file 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 - -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 Pose3& body_P_sensor = Pose3() - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity(), body_P_sensor); - - 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 Pose3& body_P_sensor = Pose3() ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, body_P_sensor).deltaPij; -} - -Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Pose3& body_P_sensor = Pose3() ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, body_P_sensor).deltaVij; -} - -Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const Pose3& body_P_sensor = Pose3() ) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, body_P_sensor).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); - - // 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)); - - // positions and velocities - Matrix H5etop6 = H5e.topRows(6); - Matrix H5atop6 = H5a.topRows(6); - EXPECT(assert_equal(H5etop6, H5atop6)); - // rotations - EXPECT(assert_equal(RH5e, H5a.bottomRows(3))); // evaluate only the rotation residue - - cout << "RH5e= "<< RH5e << endl; - cout << "RH5a= "<< H5a.bottomRows(3) << endl; -} - -/* ************************************************************************* */ -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, body_P_sensor); - - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), 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)); -} - -/* ************************************************************************* */ -TEST( ImuFactor, MeasurementCovarianceEstimation ) -{ - // 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, body_P_sensor); - - // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); - Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); - Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, body_P_sensor), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); - Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); - - Matrix Jexpected(9,9); - Jexpected << //deltaP VS (measuredAcc measuredOmega intError) - - expectedDelPdelBiasAcc, - expectedDelPdelBiasOmega, - expectedDelPdelBiasAcc, - //deltaV VS (measuredAcc measuredOmega intError) - - expectedDelVdelBiasAcc, - expectedDelVdelBiasOmega, Matrix3::Zero(), - //deltaR VS (measuredAcc measuredOmega intError) - expectedDelRdelBiasAcc, -expectedDelRdelBiasOmega, Matrix3::Zero(); - - Matrix MeasurementCovarianceexpected(9,9); - MeasurementCovarianceexpected = Jexpected * preintegrated.measurementCovariance * Jexpected.transpose(); - -// std::cout << "preintegratedMeasurementsCovariance inverse" << preintegrated.preintegratedMeasurementsCovariance.inverse() << std::endl; - - - // Compare Jacobians - EXPECT(assert_equal(MeasurementCovarianceexpected, preintegrated.preintegratedMeasurementsCovariance(), 2e-5)); -} - -#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, body_P_sensor); - - 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); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - // positions and velocities - Matrix H1etop6 = H1e.topRows(6); - Matrix H1atop6 = H1a.topRows(6); - EXPECT(assert_equal(H1etop6, H1atop6)); - // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // 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)); - - // positions and velocities - Matrix H5etop6 = H5e.topRows(6); - Matrix H5atop6 = H5a.topRows(6); - EXPECT(assert_equal(H5etop6, H5atop6)); - // rotations - EXPECT(assert_equal(RH5e, H5a.bottomRows(3))); // evaluate only the rotation residue -} - -/* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */