diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index da6341653..1b533d417 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -1,39 +1,57 @@ -/* - * ImuFactor.h - * - * Created on: Jun 29, 2014 - * Author: krunal - */ +/* ---------------------------------------------------------------------------- + + * 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 AHRSFactor.h + * @author Krunal Chande, Luca Carlone + **/ #pragma once +/* GTSAM includes */ #include #include #include #include #include +/* External or standard includes */ #include namespace gtsam { class AHRSFactor: public NoiseModelFactor3 { public: + + /** Struct to store results of preintegrating IMU measurements. Can be build + * incrementally so as to avoid costly integration at time of factor construction. */ + + /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) + * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/ class PreintegratedMeasurements { public: - imuBias::ConstantBias biasHat; - Matrix measurementCovariance; + imuBias::ConstantBias biasHat;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer + Matrix measurementCovariance;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - Rot3 deltaRij; - double deltaTij; - Matrix3 delRdelBiasOmega; - Matrix PreintMeasCov; + Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) + double deltaTij; ///< Time interval from i to j + 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*) - PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredOmegaCovariance) : - biasHat(bias), measurementCovariance(3,3), deltaTij(0.0), + /** Default constructor, initialize with no measurements */ + PreintegratedMeasurements( + const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases + const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate + ) : biasHat(bias), measurementCovariance(3,3), deltaTij(0.0), delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(3,3) { -// measurementCovariance << integrationErrorCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measurementAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; measurementCovariance < body_P_sensor = boost::none) { + 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 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; + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame Matrix3 body_omega_body_cross = skewSymmetric(correctedOmega); + // linear acceleration vector in the body frame } - const Vector3 theta_incr = correctedOmega * deltaT; - const Rot3 Rincr = Rot3::Expmap(theta_incr); - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); + 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 = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - // Matrix3 Z_3x3 = Matrix::Zero(); - // Matrix3 I_3x3 = Matrix::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij); + // Update preintegrated measurements covariance + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3) const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); Rot3 Rot_j = deltaRij * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) const Matrix3 Jrinv_theta_j = Rot3::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_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(3, 3); F << 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 + /* ----------------------------------------------------------------------------------------------------------------------- */ deltaRij = deltaRij * Rincr; deltaTij += deltaT; } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ // 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, @@ -155,8 +197,8 @@ private: PreintegratedMeasurements preintegratedMeasurements_; Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; + Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + boost::optional body_P_sensor_;///< The pose of the sensor in the body frame public: @@ -171,20 +213,24 @@ public: AHRSFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) {} - AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_( - preintegratedMeasurements), omegaCoriolis_( - omegaCoriolis), body_P_sensor_(body_P_sensor) { + AHRSFactor( + Key rot_i, ///< previous rot key + Key rot_j, ///< current rot key + Key bias,///< previous bias key + const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated measurements + const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame + boost::optional body_P_sensor = boost::none ///< The Pose of the sensor frame in the body frame + ) : + Base( + noiseModel::Gaussian::Covariance( + preintegratedMeasurements.PreintMeasCov), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( + body_P_sensor) { } virtual ~AHRSFactor() {} - + /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr( @@ -193,6 +239,9 @@ public: ); } + /** implement functions needed for Testable */ + + /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," @@ -206,6 +255,7 @@ public: 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); @@ -226,6 +276,9 @@ public: return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ + + /** vector of errors */ Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, const imuBias::ConstantBias& bias, boost::optional H1 = boost::none, @@ -321,7 +374,6 @@ public: // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract( preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, @@ -349,7 +401,6 @@ private: ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; -// AHRSFactor +}; // AHRSFactor typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; } //namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2c9827852..cf6d0adbd 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -77,8 +77,8 @@ namespace gtsam { 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 Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors const bool use2ndOrderIntegration = false ///< Controls the order of integration ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), @@ -95,7 +95,7 @@ namespace gtsam { 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) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(false) { measurementCovariance = Matrix::Zero(9,9); PreintMeasCov = Matrix::Zero(9,9); @@ -324,7 +324,7 @@ namespace gtsam { #endif /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} + ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){} /** Constructor */ ImuFactor( diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 7f6fa69a8..8d368b7c1 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -12,7 +12,7 @@ /** * @file testImuFactor.cpp * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Krunal Chande, Luca Carlone */ #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b4faf79a0..4a7c4495d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -441,60 +441,60 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } -#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((Vector(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((Vector(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_(); -} +//#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((Vector(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((Vector(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.MeasurementCovariance()); +// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); +// +// 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_(); +//} /* ************************************************************************* */ diff --git a/gtsam/slam/DroneDynamicsFactor.h b/gtsam/slam/DroneDynamicsFactor.h index aae2e204a..e471e0172 100644 --- a/gtsam/slam/DroneDynamicsFactor.h +++ b/gtsam/slam/DroneDynamicsFactor.h @@ -16,7 +16,7 @@ * @author Duy-Nguyen Ta * @date Sep 29, 2014 */ - +// Implementation is incorrect use DroneDynamicsVelXYFactor instead. #pragma once #include diff --git a/gtsam/slam/tests/testDistanceFactor.cpp b/gtsam/slam/tests/testDistanceFactor.cpp index b16519715..b4c35a98f 100644 --- a/gtsam/slam/tests/testDistanceFactor.cpp +++ b/gtsam/slam/tests/testDistanceFactor.cpp @@ -13,7 +13,7 @@ * @file testDistanceFactor.cpp * @brief Unit tests for DistanceFactor Class * @author Duy-Nguyen Ta - * @date Oct 2012 + * @date Oct 2014 */ #include diff --git a/gtsam/slam/tests/testDroneDynamicsFactor.cpp b/gtsam/slam/tests/testDroneDynamicsFactor.cpp index 14004da3b..bf11ed805 100644 --- a/gtsam/slam/tests/testDroneDynamicsFactor.cpp +++ b/gtsam/slam/tests/testDroneDynamicsFactor.cpp @@ -12,8 +12,8 @@ /** * @file testRangeFactor.cpp * @brief Unit tests for DroneDynamicsFactor Class - * @author Stephen Williams - * @date Oct 2012 + * @author Duy-Nguyen Ta + * @date Oct 2014 */ #include diff --git a/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp b/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp index e0bb1356d..d9b94f1cb 100644 --- a/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp +++ b/gtsam/slam/tests/testDroneDynamicsVelXYFactor.cpp @@ -12,8 +12,8 @@ /** * @file testRangeFactor.cpp * @brief Unit tests for DroneDynamicsVelXYFactor Class - * @author Stephen Williams - * @date Oct 2012 + * @author Duy-Nguyen Ta + * @date Oct 2014 */ #include