From fe55148dd72be6f749bdb509b6e43b132d7f9f7f Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 27 Jan 2014 13:44:24 -0500 Subject: [PATCH] deleted redundant files for imu factors --- gtsam/navigation/CombinedImuFactor.h | 10 +- gtsam/navigation/ImuFactor.h | 4 + gtsam_unstable/slam/CombinedImuFactor.h | 673 ------------------------ gtsam_unstable/slam/ImuFactor.h | 565 -------------------- 4 files changed, 11 insertions(+), 1241 deletions(-) delete mode 100644 gtsam_unstable/slam/CombinedImuFactor.h delete mode 100644 gtsam_unstable/slam/ImuFactor.h diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 0a7fb270c..f7e3cfec8 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -236,7 +236,7 @@ namespace gtsam { // 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_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; @@ -274,6 +274,7 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ + // deltaPij += deltaVij * deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; @@ -341,8 +342,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; - +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c3753e5e7..bd8a0f80b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -304,7 +304,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ +#ifndef _MSC_VER + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h deleted file mode 100644 index bf86b6933..000000000 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ /dev/null @@ -1,673 +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 - **/ - -#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; - 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) = (H_vel_biasacc) * (1/deltaT) * measurementCovariance.block(3,3,3,3) * (H_vel_biasacc.transpose()) + -// (H_vel_biasacc) * (1/deltaT) * -// ( measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * -// (H_vel_biasacc.transpose()); - - 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(); - - /* - // overall Jacobian wrt raw measurements (df/du) - Matrix3 H_vel_initbiasacc = H_vel_biasacc; - Matrix3 H_angles_initbiasomega = H_angles_biasomega; - - // COMBINED IMU FACTOR, preserves correlation with bias evolution and considers initial uncertainty on biases - Matrix G(15,21); - G << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, - H_vel_biasacc, Z_3x3, H_vel_biasacc, Z_3x3, H_vel_initbiasacc, Z_3x3, - Z_3x3, Z_3x3, - H_angles_biasomega, Z_3x3, H_angles_biasomega, Z_3x3, H_angles_initbiasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3; - - Matrix ErrorMatrix = (1/deltaT) * G * measurementCovariance * G.transpose() - G_measCov_Gt; - std::cout << "---- matrix multiplication error = [" << ErrorMatrix << "];"<< std::endl; - double max_err=0; - for(int i=0;i<15;i++) - { - for(int j=0;j<15;j++) - { - if(fabs(ErrorMatrix(i,j))>max_err) - max_err = fabs(ErrorMatrix(i,j)); - } - } - std::cout << "---- max matrix multiplication error = [" << max_err << "];"<< std::endl; - - if(max_err>10e-15) - std::cout << "---- max matrix multiplication error *large* = [" << max_err << "];"<< std::endl; - - PreintMeasCov = F * PreintMeasCov * F.transpose() + (1/deltaT) * G * measurementCovariance * G.transpose(); - */ - - PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * 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_; - - public: - - /** Shorthand for a smart pointer to a factor */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** 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) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis) { - } - - 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: "); - } - - /** 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); - } - - /** 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) - { - - 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_); - } - }; // \class CombinedImuFactor - -} /// namespace gtsam diff --git a/gtsam_unstable/slam/ImuFactor.h b/gtsam_unstable/slam/ImuFactor.h deleted file mode 100644 index 0c08ed7be..000000000 --- a/gtsam_unstable/slam/ImuFactor.h +++ /dev/null @@ -1,565 +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*) - - Vector3 initialRotationRate; ///< initial rotation rate reading from the IMU (at time i) - Vector3 finalRotationRate; ///< final rotation rate reading from the IMU (at time j) - - /** 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 - const Vector3& initialRotationRate = Vector3::Zero() ///< initial rotation rate reading from the IMU (at time i) - ) : 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), - initialRotationRate(initialRotationRate), finalRotationRate(initialRotationRate) - { - 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), - initialRotationRate(Vector3::Zero()), finalRotationRate(Vector3::Zero()) - { - 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); - - finalRotationRate = correctedOmega; - - // 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; - 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; - 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 */ -#ifndef _MSC_VER - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - /** 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 - -} /// namespace gtsam