diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ea81c85e..de9b4152f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,7 @@ option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -485,13 +486,14 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") -print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") -print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") -print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") -print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") +print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") +print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/README.md b/README.md index 040f1134f..88f69dec4 100644 --- a/README.md +++ b/README.md @@ -1,59 +1,77 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== - -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) -- A modern compiler, i.e., at least gcc 4.7.3 on Linux. - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -GTSAM 4 Compatibility ---------------------- - -GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. - -Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. - -Additional Information ----------------------- - -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. - -See the [`INSTALL`](INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. - -Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +GTSAM 4 Compatibility +--------------------- + +GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. + +Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. + +The Preintegrated IMU Factor +---------------------------- + +GTSAM includes a state of the art IMU handling scheme based on + +- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + +Our implementation improves on this using integration on the manifold, as detailed in + +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. +- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. + +If you are using the factor in academic work, please cite the publications above. + +In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF. + + + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. + +See the [`INSTALL`](INSTALL) file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file diff --git a/gtsam.h b/gtsam.h index c12055916..f9a7483bb 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2506,30 +2506,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const; }; -#include -virtual class PreintegrationBase { - // Constructors - PreintegrationBase(const gtsam::PreintegrationParams* params); - PreintegrationBase(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationBase& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - - // Standard Interface - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; - #include -virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { +class PreintegratedImuMeasurements { // Constructors PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, @@ -2544,6 +2522,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class ImuFactor: gtsam::NonlinearFactor { @@ -2559,7 +2544,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor { }; #include -virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { +class PreintegratedCombinedMeasurements { // Testable void print(string s) const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, @@ -2570,6 +2555,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { double deltaT); void resetIntegration(); Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; }; virtual class CombinedImuFactor: gtsam::NonlinearFactor { diff --git a/gtsam/config.h.in b/gtsam/config.h.in index f9a576d14..42d5e7517 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -68,3 +68,6 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION + +// Support Metis-based nested dissection +#cmakedefine GTSAM_TANGENT_PREINTEGRATION diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a961a79bd..21c4200a9 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -31,22 +31,21 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -void PreintegratedCombinedMeasurements::print( - const string& s) const { - PreintegrationBase::print(s); +void PreintegratedCombinedMeasurements::print(const string& s) const { + PreintegrationType::print(s); cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedCombinedMeasurements::equals( const PreintegratedCombinedMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) && - equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); + return PreintegrationType::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedCombinedMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements. - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // Update preintegrated measurements covariance: as in [2] we consider a first // order propagation that can be seen as a prediction phase in an EKF @@ -79,8 +78,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // and preintegrated measurements // Single Jacobians to propagate covariance - // TODO(frank): should we not also accout for bias on position? - Matrix3 theta_H_biasOmega = - C.topRows<3>(); + // TODO(frank): should we not also account for bias on position? + Matrix3 theta_H_biasOmega = -C.topRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); // overall Jacobian wrt preintegrated measurements (df/dx) @@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( // BLOCK DIAGONAL TERMS D_t_t(&G_measCov_Gt) = dt * iCov; - D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * - (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * - (vel_H_biasAcc.transpose()); - D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * - (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * - (theta_H_biasOmega.transpose()); + D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc + * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) + * (vel_H_biasAcc.transpose()); + D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega + * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) + * (theta_H_biasOmega.transpose()); D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; // OFF BLOCK DIAGONAL TERMS - Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * - theta_H_biasOmega.transpose(); + Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) + * theta_H_biasOmega.transpose(); D_v_R(&G_measCov_Gt) = temp; D_R_v(&G_measCov_Gt) = temp.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; @@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration) { if (!use2ndOrderIntegration) - throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); biasHat_ = biasHat; boost::shared_ptr p = Params::MakeSharedD(); p->gyroscopeCovariance = measuredOmegaCovariance; @@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const PreintegratedCombinedMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) {} +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, + Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), _PIM_(pim) { +} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); // if we need the jacobians @@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor( const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, const bool use2ndOrderCoriolis) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, - pose_j, vel_j, bias_i, bias_j), - _PIM_(pim) { +: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), +_PIM_(pim) { boost::shared_ptr p = - boost::make_shared(pim.p()); + boost::make_shared(pim.p()); p->n_gravity = n_gravity; p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; @@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor( } void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - CombinedPreintegratedMeasurements& pim, - const Vector3& n_gravity, - const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis) { + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { // use deprecated predict PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, omegaCoriolis, use2ndOrderCoriolis); @@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, } #endif -} /// namespace gtsam +} + /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 3141f8245..bcad9d8f7 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -22,12 +22,19 @@ #pragma once /* GTSAM includes */ +#include +#include #include -#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating @@ -57,7 +64,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationBase { +class PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -123,7 +130,7 @@ public: PreintegratedCombinedMeasurements( const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) - : PreintegrationBase(p, biasHat) { + : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } @@ -133,10 +140,10 @@ public: /// @{ /// Re-initialize PreintegratedCombinedMeasurements - void resetIntegration(); + void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *boost::static_pointer_cast(this->p_);} /// @} /// @name Access instance variables @@ -146,7 +153,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const; /// @} @@ -163,7 +170,7 @@ public: * frame) */ void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT); + const Vector3& measuredOmega, const double dt) override; /// @} @@ -183,7 +190,7 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 6d36539e4..7210f4dd2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -32,20 +32,20 @@ using namespace std; // Inner class PreintegratedImuMeasurements //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::print(const string& s) const { - PreintegrationBase::print(s); + PreintegrationType::print(s); cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ bool PreintegratedImuMeasurements::equals( const PreintegratedImuMeasurements& other, double tol) const { - return PreintegrationBase::equals(other, tol) + return PreintegrationType::equals(other, tol) && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::resetIntegration() { - PreintegrationBase::resetIntegration(); + PreintegrationType::resetIntegration(); preintMeasCov_.setZero(); } @@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { // Update preintegrated measurements (also get Jacobian) - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; - PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); + PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a @@ -73,31 +73,33 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 - preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; + preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt; } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, - const Matrix& measuredOmegas, - const Matrix& dts) { - assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); +void PreintegratedImuMeasurements::integrateMeasurements( + const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts) { + assert( + measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(dts.cols() >= 1); assert(measuredAccs.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols()); size_t n = static_cast(dts.cols()); for (size_t j = 0; j < n; j++) { - integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j)); } } //------------------------------------------------------------------------------ -void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // - Matrix9* H1, Matrix9* H2) { - PreintegrationBase::mergeWith(pim12, H1, H2); +#ifdef GTSAM_TANGENT_PREINTEGRATION +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationType::mergeWith(pim12, H1, H2); preintMeasCov_ = - *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); + *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } - +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PreintegratedImuMeasurements::PreintegratedImuMeasurements( @@ -174,16 +176,17 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, } //------------------------------------------------------------------------------ +#ifdef GTSAM_TANGENT_PREINTEGRATION PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { if (!pim01.matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with different params"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); if (pim01.params()->body_P_sensor) - throw std::domain_error( - "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); // the bias for the merged factor will be the bias from 01 PreintegratedImuMeasurements pim02 = pim01; @@ -196,26 +199,27 @@ PreintegratedImuMeasurements ImuFactor::Merge( //------------------------------------------------------------------------------ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, - const shared_ptr& f12) { + const shared_ptr& f12) { // IMU bias keys must be the same. if (f01->key5() != f12->key5()) - throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); + throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) - throw std::domain_error( - "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); + throw std::domain_error( + "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = - Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B - pim02); + Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); + return boost::make_shared(f01->key1(),// P0 + f01->key2(),// V0 + f12->key3(),// P2 + f12->key4(),// V2 + f01->key5(),// B + pim02); } +#endif //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -248,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, //------------------------------------------------------------------------------ // ImuFactor2 methods //------------------------------------------------------------------------------ -ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), - _PIM_(pim) {} +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, + bias), _PIM_(pim) { +} //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor2::clone() const { @@ -266,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { } //------------------------------------------------------------------------------ -void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) - << "," << keyFormatter(this->key3()) << ")\n"; +void ImuFactor2::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) + << ")\n"; cout << *this << endl; } @@ -281,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, - const imuBias::ConstantBias& bias_i, // - boost::optional H1, - boost::optional H2, - boost::optional H3) const { +Vector ImuFactor2::evaluateError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, boost::optional H2, + boost::optional H3) const { return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); } //------------------------------------------------------------------------------ } - // namespace gtsam +// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 010550eb1..532abdac0 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,11 +23,18 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { +#ifdef GTSAM_TANGENT_PREINTEGRATION +typedef TangentPreintegration PreintegrationType; +#else +typedef ManifoldPreintegration PreintegrationType; +#endif + /* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating @@ -61,7 +68,7 @@ namespace gtsam { * * @addtogroup SLAM */ -class PreintegratedImuMeasurements: public PreintegrationBase { +class PreintegratedImuMeasurements: public PreintegrationType { friend class ImuFactor; friend class ImuFactor2; @@ -85,29 +92,28 @@ public: */ PreintegratedImuMeasurements(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : - PreintegrationBase(p, biasHat) { + PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); } /** * Construct preintegrated directly from members: base class and preintMeasCov - * @param base PreintegrationBase instance + * @param base PreintegrationType instance * @param preintMeasCov Covariance matrix used in noise model. */ - PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) - : PreintegrationBase(base), + PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov) + : PreintegrationType(base), preintMeasCov_(preintMeasCov) { } /// print - void print(const std::string& s = "Preintegrated Measurements:") const; + void print(const std::string& s = "Preintegrated Measurements:") const override; /// equals - bool equals(const PreintegratedImuMeasurements& expected, - double tol = 1e-9) const; + bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const; /// Re-initialize PreintegratedIMUMeasurements - void resetIntegration(); + void resetIntegration() override; /** * Add a single IMU measurement to the preintegration. @@ -115,7 +121,8 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt) override; /// Add multiple measurements, in matrix columns void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, @@ -124,8 +131,10 @@ public: /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge in a different set of measurements and update bias derivatives accordingly void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor @@ -150,7 +159,7 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType); ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; @@ -230,6 +239,7 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; +#ifdef GTSAM_TANGENT_PREINTEGRATION /// Merge two pre-integrated measurement classes static PreintegratedImuMeasurements Merge( const PreintegratedImuMeasurements& pim01, @@ -237,6 +247,7 @@ public: /// Merge two factors static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); +#endif #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated typename diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp new file mode 100644 index 000000000..cc88d9101 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -0,0 +1,143 @@ +/* ---------------------------------------------------------------------------- + + * 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 ManifoldPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "ManifoldPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +ManifoldPreintegration::ManifoldPreintegration( + const boost::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool ManifoldPreintegration::equals(const ManifoldPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ManifoldPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +Vector9 ManifoldPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h new file mode 100644 index 000000000..8a9d44755 --- /dev/null +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 ManifoldPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * IMU pre-integration on NavSatet manifold. + * This corresponds to the original RSS paper (with one difference: V is rotated) + */ +class ManifoldPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + 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 + + /// Default constructor for serialization + ManifoldPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + ManifoldPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position().vector(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } + Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } + Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } + Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } + Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } + + /// @name Testable + /// @{ + bool equals(const ManifoldPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f01a55577..50949c761 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -24,6 +24,18 @@ namespace gtsam { #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; +//------------------------------------------------------------------------------ +NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, R.transpose(), Z_3x3; + if (H3) + *H3 << Z_3x3, Z_3x3, R.transpose(); + return NavState(R, t, v); +} //------------------------------------------------------------------------------ NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { @@ -94,138 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const { } //------------------------------------------------------------------------------ -NavState NavState::inverse() const { +NavState NavState::retract(const Vector9& xi, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { TIE(nRb, n_t, n_v, *this); - const Rot3 bRn = nRb.inverse(); - return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); -} - -//------------------------------------------------------------------------------ -NavState NavState::operator*(const NavState& bTc) const { - TIE(nRb, n_t, n_v, *this); - TIE(bRc, b_t, b_v, bTc); - return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); -} - -//------------------------------------------------------------------------------ -NavState::PositionAndVelocity // -NavState::operator*(const PositionAndVelocity& b_tv) const { - TIE(nRb, n_t, n_v, *this); - const Point3& b_t = b_tv.first; - const Velocity3& b_v = b_tv.second; - return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); -} - -//------------------------------------------------------------------------------ -Point3 NavState::operator*(const Point3& b_t) const { - return Point3(R_ * b_t + t_); -} - -//------------------------------------------------------------------------------ -NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, - OptionalJacobian<9, 9> H) { - Matrix3 D_R_xi; - const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); - const Point3 p = Point3(dP(xi)); - const Vector v = dV(xi); - const NavState result(R, p, v); - if (H) { - *H << D_R_xi, Z_3x3, Z_3x3, // - Z_3x3, R.transpose(), Z_3x3, // - Z_3x3, Z_3x3, R.transpose(); + Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; + const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); + const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); + const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0); + const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0); + if (H1) { + *H1 << D_R_nRb, Z_3x3, Z_3x3, // + // Note(frank): the derivative of n_t with respect to xi is nRb + // We pre-multiply with nRc' to account for NavState::Create + // Then we make use of the identity nRc' * nRb = bRc' + nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3, + // Similar reasoning for v: + nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose(); } - return result; + if (H2) { + *H2 << D_bRc_xi, Z_3x3, Z_3x3, // + Z_3x3, bRc.transpose(), Z_3x3, // + Z_3x3, Z_3x3, bRc.transpose(); + } + return NavState(nRc, t, v); } //------------------------------------------------------------------------------ -Vector9 NavState::ChartAtOrigin::Local(const NavState& x, - OptionalJacobian<9, 9> H) { +Vector9 NavState::localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + Vector9 xi; Matrix3 D_xi_R; - xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); - if (H) { - *H << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, x.R(), Z_3x3, // - Z_3x3, Z_3x3, x.R(); + xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + if (H1) { + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; + } + if (H2) { + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } return xi; } -//------------------------------------------------------------------------------ -NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Expmap derivative not implemented yet"); - - Eigen::Block n_omega_nb = dR(xi); - Eigen::Block v = dP(xi); - Eigen::Block a = dV(xi); - - // NOTE(frank): See Pose3::Expmap - Rot3 nRb = Rot3::Expmap(n_omega_nb); - double theta2 = n_omega_nb.dot(n_omega_nb); - if (theta2 > numeric_limits::epsilon()) { - // Expmap implements a "screw" motion in the direction of n_omega_nb - Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis - Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis - Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel) - / theta2; - Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis - Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis - Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2; - return NavState(nRb, n_t, n_v); - } else { - return NavState(nRb, Point3(v), a); - } -} - -//------------------------------------------------------------------------------ -Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { - if (H) - throw runtime_error("NavState::Logmap derivative not implemented yet"); - - TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p; - - // NOTE(frank): See Pose3::Logmap - Vector9 xi; - Vector3 n_omega_nb = Rot3::Logmap(nRb); - double theta = n_omega_nb.norm(); - if (theta * theta <= numeric_limits::epsilon()) { - xi << n_omega_nb, n_t, n_v; - } else { - Matrix3 W = skewSymmetric(n_omega_nb / theta); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in n_t to avoid matrix math - double factor = (1 - theta / (2. * tan(0.5 * theta))); - Vector3 n_x = W * n_t; - Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x); - Vector3 n_y = W * n_v; - Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y); - xi << n_omega_nb, v, a; - } - return xi; -} - -//------------------------------------------------------------------------------ -Matrix9 NavState::AdjointMap() const { - // NOTE(frank): See Pose3::AdjointMap - const Matrix3 nRb = R(); - Matrix3 pAr = skewSymmetric(t()) * nRb; - Matrix3 vAr = skewSymmetric(v()) * nRb; - Matrix9 adj; - // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV - adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; - return adj; -} - -//------------------------------------------------------------------------------ -Matrix7 NavState::wedge(const Vector9& xi) { - const Matrix3 Omega = skewSymmetric(dR(xi)); - Matrix7 T; - T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; - return T; -} - //------------------------------------------------------------------------------ // sugar for derivative blocks #define D_R_R(H) (H)->block<3,3>(0,0) @@ -239,7 +168,6 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { @@ -282,7 +210,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, } return newState; } -#endif //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index eabd1f39b..a1544735d 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -20,7 +20,7 @@ #include #include -#include +#include namespace gtsam { @@ -29,9 +29,9 @@ typedef Vector3 Velocity3; /** * Navigation state: Pose (rotation, translation) + velocity - * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold */ -class NavState: public LieGroup { +class NavState { private: // TODO(frank): @@ -42,6 +42,10 @@ private: public: + enum { + dimension = 9 + }; + typedef std::pair PositionAndVelocity; /// @name Constructors @@ -49,7 +53,7 @@ public: /// Default constructor NavState() : - t_(0,0,0), v_(Vector3::Zero()) { + t_(0, 0, 0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -59,15 +63,15 @@ public: NavState(const Pose3& pose, const Velocity3& v) : R_(pose.rotation()), t_(pose.translation()), v_(v) { } - /// Construct from Matrix group representation (no checking) - NavState(const Matrix7& T) : - R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { - } /// Construct from SO(3) and R^6 NavState(const Matrix3& R, const Vector9 tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives + static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, + OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 3> H3); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); @@ -116,7 +120,8 @@ public: /// @{ /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const NavState& state); /// print void print(const std::string& s = "") const; @@ -124,29 +129,6 @@ public: /// equals bool equals(const NavState& other, double tol = 1e-8) const; - /// @} - /// @name Group - /// @{ - - /// identity for group operation - static NavState identity() { - return NavState(); - } - - /// inverse transformation with derivatives - NavState inverse() const; - - /// Group compose is the semi-direct product as specified below: - /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) - NavState operator*(const NavState& bTc) const; - - /// Native group action is on position/velocity pairs *in the body frame* as follows: - /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) - PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; - - /// Act on position alone, n_t = nRb * b_t + n_t - Point3 operator*(const Point3& b_t) const; - /// @} /// @name Manifold /// @{ @@ -172,44 +154,25 @@ public: return v.segment<3>(6); } - // Chart at origin, constructs components separately (as opposed to Expmap) - struct ChartAtOrigin { - static NavState Retract(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - static Vector9 Local(const NavState& x, // - OptionalJacobian<9, 9> H = boost::none); - }; + /// retract with optional derivatives + NavState retract(const Vector9& v, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a NavState from canonical coordinates - static NavState Expmap(const Vector9& xi, // - OptionalJacobian<9, 9> H = boost::none); - - /// Log map at identity - return the canonical coordinates for this NavState - static Vector9 Logmap(const NavState& p, // - OptionalJacobian<9, 9> H = boost::none); - - /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms - /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); - Matrix9 AdjointMap() const; - - /// wedge creates Lie algebra element from tangent vector - static Matrix7 wedge(const Vector9& xi); + /// localCoordinates with optional derivatives + Vector9 localCoordinates(const NavState& g, // + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = + boost::none) const; /// @} /// @name Dynamics /// @{ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; -#endif /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -239,14 +202,7 @@ private: // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors template<> -struct traits : Testable, internal::LieGroupTraits { +struct traits : internal::Manifold { }; -// Partial specialization of wedge -// TODO: deprecate, make part of traits -template<> -inline Matrix wedge(const Vector& xi) { - return NavState::wedge(xi); -} - } // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c243ca860..0e8cf67c9 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -31,21 +31,12 @@ namespace gtsam { PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { - resetIntegration(); -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::resetIntegration() { - deltaTij_ = 0.0; - preintegrated_.setZero(); - preintegrated_H_biasAcc_.setZero(); - preintegrated_H_biasOmega_.setZero(); } //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const PreintegrationBase& pim) { os << " deltaTij " << pim.deltaTij_ << endl; - os << " deltaRij " << Point3(pim.theta()) << endl; + os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl; os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; @@ -59,14 +50,9 @@ void PreintegrationBase::print(const string& s) const { } //------------------------------------------------------------------------------ -bool PreintegrationBase::equals(const PreintegrationBase& other, - double tol) const { - return p_->equals(*other.p_, tol) - && fabs(deltaTij_ - other.deltaTij_) < tol - && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) - && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) - && equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol); +void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) { + biasHat_ = biasHat; + resetIntegration(); } //------------------------------------------------------------------------------ @@ -105,7 +91,8 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal(); + *correctedAcc_H_unbiasedOmega = -( diag_wdp + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } @@ -114,139 +101,38 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( return make_pair(correctedAcc, correctedOmega); } -//------------------------------------------------------------------------------ -// See extensive discussion in ImuFactor.lyx -Vector9 PreintegrationBase::UpdatePreintegrated( - const Vector3& a_body, const Vector3& w_body, double dt, - const Vector9& preintegrated, OptionalJacobian<9, 9> A, - OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) { - const auto theta = preintegrated.segment<3>(0); - const auto position = preintegrated.segment<3>(3); - const auto velocity = preintegrated.segment<3>(6); - - // This functor allows for saving computation when exponential map and its - // derivatives are needed at the same location in so<3> - so3::DexpFunctor local(theta); - - // Calculate exact mean propagation - Matrix3 w_tangent_H_theta, invH; - const Vector3 w_tangent = // angular velocity mapped back to tangent space - local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); - const SO3 R = local.expmap(); - const Vector3 a_nav = R * a_body; - const double dt22 = 0.5 * dt * dt; - - Vector9 preintegratedPlus; - preintegratedPlus << // new preintegrated vector: - theta + w_tangent* dt, // theta - position + velocity* dt + a_nav* dt22, // position - velocity + a_nav* dt; // velocity - - if (A) { - // Exact derivative of R*a with respect to theta: - const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); - - A->setIdentity(); - A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta - A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... - A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity - A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta - } - if (B) { - B->block<3, 3>(0, 0) = Z_3x3; - B->block<3, 3>(3, 0) = R * dt22; - B->block<3, 3>(6, 0) = R * dt; - } - if (C) { - C->block<3, 3>(0, 0) = invH * dt; - C->block<3, 3>(3, 0) = Z_3x3; - C->block<3, 3>(6, 0) = Z_3x3; - } - - return preintegratedPlus; -} - //------------------------------------------------------------------------------ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt, Matrix9* A, - Matrix93* B, Matrix93* C) { - // Correct for bias in the sensor frame - Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 omega = biasHat_.correctGyroscope(measuredOmega); - - // Possibly correct for sensor pose - Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); - - // Do update - deltaTij_ += dt; - preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); - - if (p().body_P_sensor) { - // More complicated derivatives in case of non-trivial sensor pose - *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().isZero()) - *C += *B* D_correctedAcc_omega; - *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last - } - - // D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias - preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); - - // D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias - preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); -} - -void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, - double dt) { - // NOTE(frank): integrateMeasuremtn always needs to compute the derivatives, + const Vector3& measuredOmega, double dt) { + // NOTE(frank): integrateMeasurement always needs to compute the derivatives, // even when not of interest to the caller. Provide scratch space here. Matrix9 A; Matrix93 B, C; - integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); -} -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::biasCorrectedDelta( - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { - // We correct for a change between bias_i and the biasHat_ used to integrate - // This is a simple linear correction with obvious derivatives - const imuBias::ConstantBias biasIncr = bias_i - biasHat_; - const Vector9 biasCorrected = - preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + - preintegrated_H_biasOmega_ * biasIncr.gyroscope(); - - if (H) { - (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; - } - return biasCorrected; + update(measuredAcc, measuredOmega, dt, &A, &B, &C); } //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { // TODO(frank): make sure this stuff is still correct Matrix96 D_biasCorrected_bias; - Vector9 biasCorrected = - biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, - H1 ? &D_delta_state : 0, - H2 ? &D_delta_biasCorrected : 0); + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); - if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; - if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; return state_j; } @@ -306,94 +192,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } -//------------------------------------------------------------------------------ -// sugar for derivative blocks -#define D_R_R(H) (H)->block<3,3>(0,0) -#define D_R_t(H) (H)->block<3,3>(0,3) -#define D_R_v(H) (H)->block<3,3>(0,6) -#define D_t_R(H) (H)->block<3,3>(3,0) -#define D_t_t(H) (H)->block<3,3>(3,3) -#define D_t_v(H) (H)->block<3,3>(3,6) -#define D_v_R(H) (H)->block<3,3>(6,0) -#define D_v_t(H) (H)->block<3,3>(6,3) -#define D_v_v(H) (H)->block<3,3>(6,6) - -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::Compose(const Vector9& zeta01, - const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) { - const auto t01 = zeta01.segment<3>(0); - const auto p01 = zeta01.segment<3>(3); - const auto v01 = zeta01.segment<3>(6); - - const auto t12 = zeta12.segment<3>(0); - const auto p12 = zeta12.segment<3>(3); - const auto v12 = zeta12.segment<3>(6); - - Matrix3 R01_H_t01, R12_H_t12; - const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); - const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); - - Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity - const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); - - Matrix3 t02_H_R02; - Vector9 zeta02; - const Matrix3 R = R01.matrix(); - zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta - p01 + v01 * deltaT12 + R * p12, // position - v01 + R * v12; // velocity - - if (H1) { - H1->setIdentity(); - D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; - D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; - D_t_v(H1) = I_3x3 * deltaT12; - D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; - } - - if (H2) { - H2->setZero(); - D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; - D_t_t(H2) = R; - D_v_v(H2) = R; - } - - return zeta02; -} - -//------------------------------------------------------------------------------ -void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, - Matrix9* H2) { - if (!matchesParamsWith(pim12)) { - throw std::domain_error("Cannot merge pre-integrated measurements with different params"); - } - - if (params()->body_P_sensor) { - throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); - } - - const double t01 = deltaTij(); - const double t12 = pim12.deltaTij(); - deltaTij_ = t01 + t12; - - const Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); // will be modified. - - const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); - zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() - + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - - preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); - - preintegrated_H_biasAcc_ = - (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; - - preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + - (*H2) * pim12.preintegrated_H_biasOmega_; -} - //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, @@ -408,6 +206,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, p_ = q; return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); } + #endif //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9d751e92d..06be4642d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -75,29 +75,13 @@ class PreintegrationBase { /// Time interval from i to j double deltaTij_; - /** - * Preintegrated navigation state, from frame i to frame j - * Order is: theta, position, velocity - * Note: relative position does not take into account velocity at time i, see deltap+, in [2] - * Note: velocity is now also in frame i, as opposed to deltaVij in [2] - */ - Vector9 preintegrated_; - - Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias - Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias - /// Default constructor for serialization - PreintegrationBase() { - resetIntegration(); - } + PreintegrationBase() {} public: /// @name Constructors /// @{ - /// @name Constructors - /// @{ - /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -111,7 +95,12 @@ public: /// @name Basic utilities /// @{ /// Re-initialize PreintegratedMeasurements - void resetIntegration(); + virtual void resetIntegration()=0; + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements and set new bias + void resetIntegrationAndSetBias(const Bias& biasHat); /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { @@ -140,17 +129,10 @@ public: const imuBias::ConstantBias& biasHat() const { return biasHat_; } double deltaTij() const { return deltaTij_; } - const Vector9& preintegrated() const { return preintegrated_; } - - Vector3 theta() const { return preintegrated_.head<3>(); } - Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } - Vector3 deltaVij() const { return preintegrated_.tail<3>(); } - - Rot3 deltaRij() const { return Rot3::Expmap(theta()); } - NavState deltaXij() const { return NavState::Retract(preintegrated_); } - - const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } - const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + virtual Vector3 deltaPij() const=0; + virtual Vector3 deltaVij() const=0; + virtual Rot3 deltaRij() const=0; + virtual NavState deltaXij() const=0; // Exposed for MATLAB Vector6 biasHatVector() const { return biasHat_.vector(); } @@ -159,8 +141,7 @@ public: /// @name Testable /// @{ GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); - void print(const std::string& s) const; - bool equals(const PreintegrationBase& other, double tol) const; + virtual void print(const std::string& s) const; /// @} /// @name Main functionality @@ -175,30 +156,20 @@ public: OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; - // Update integrated vector on tangent manifold preintegrated with acceleration - // Static, functional version. - static Vector9 UpdatePreintegrated(const Vector3& a_body, - const Vector3& w_body, double dt, - const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); - /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame - /// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT, - Matrix9* A, Matrix93* B, Matrix93* C); + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0; - // Version without derivatives - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, const double deltaT); + /// Version without derivatives + virtual void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt); /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far - Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const; + virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const=0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, @@ -219,23 +190,6 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives - static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, - double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); - - /// Merge in a different set of measurements and update bias derivatives accordingly - /// The derivatives apply to the preintegrated Vector9 - void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); - /// @} - - /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); - } - - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ @@ -249,20 +203,6 @@ public: #endif /// @} - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); - ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); - ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); - } }; } /// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp new file mode 100644 index 000000000..0ac57d05c --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -0,0 +1,249 @@ +/* ---------------------------------------------------------------------------- + + * 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 TangentPreintegration.cpp + * @author Frank Dellaert + * @author Adam Bry + **/ + +#include "TangentPreintegration.h" +#include +#include + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, + const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::resetIntegration() { + deltaTij_ = 0.0; + preintegrated_.setZero(); + preintegrated_H_biasAcc_.setZero(); + preintegrated_H_biasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool TangentPreintegration::equals(const TangentPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) + && equal_with_abs_tol(preintegrated_H_biasAcc_, + other.preintegrated_H_biasAcc_, tol) + && equal_with_abs_tol(preintegrated_H_biasOmega_, + other.preintegrated_H_biasOmega_, tol); +} + +//------------------------------------------------------------------------------ +// See extensive discussion in ImuFactor.lyx +Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, double dt, const Vector9& preintegrated, + OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B, + OptionalJacobian<9, 3> C) { + const auto theta = preintegrated.segment<3>(0); + const auto position = preintegrated.segment<3>(3); + const auto velocity = preintegrated.segment<3>(6); + + // This functor allows for saving computation when exponential map and its + // derivatives are needed at the same location in so<3> + so3::DexpFunctor local(theta); + + // Calculate exact mean propagation + Matrix3 w_tangent_H_theta, invH; + const Vector3 w_tangent = // angular velocity mapped back to tangent space + local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0); + const SO3 R = local.expmap(); + const Vector3 a_nav = R * a_body; + const double dt22 = 0.5 * dt * dt; + + Vector9 preintegratedPlus; + preintegratedPlus << // new preintegrated vector: + theta + w_tangent * dt, // theta + position + velocity * dt + a_nav * dt22, // position + velocity + a_nav * dt; // velocity + + if (A) { + // Exact derivative of R*a with respect to theta: + const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp(); + + A->setIdentity(); + A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta + A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta... + A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity + A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta + } + if (B) { + B->block<3, 3>(0, 0) = Z_3x3; + B->block<3, 3>(3, 0) = R * dt22; + B->block<3, 3>(6, 0) = R * dt; + } + if (C) { + C->block<3, 3>(0, 0) = invH * dt; + C->block<3, 3>(3, 0) = Z_3x3; + C->block<3, 3>(6, 0) = Z_3x3; + } + + return preintegratedPlus; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Do update + deltaTij_ += dt; + preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C); + + if (p().body_P_sensor) { + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc + // where acc_H_biasAcc = -I_3x3, hence + // new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc + preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B); + + // new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega + // where omega_H_biasOmega = -I_3x3, hence + // new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega + preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C); +} + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // We correct for a change between bias_i and the biasHat_ used to integrate + // This is a simple linear correction with obvious derivatives + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + const Vector9 biasCorrected = preintegrated() + + preintegrated_H_biasAcc_ * biasIncr.accelerometer() + + preintegrated_H_biasOmega_ * biasIncr.gyroscope(); + + if (H) { + (*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_; + } + return biasCorrected; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +Vector9 TangentPreintegration::Compose(const Vector9& zeta01, + const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) { + const auto t01 = zeta01.segment<3>(0); + const auto p01 = zeta01.segment<3>(3); + const auto v01 = zeta01.segment<3>(6); + + const auto t12 = zeta12.segment<3>(0); + const auto p12 = zeta12.segment<3>(3); + const auto v12 = zeta12.segment<3>(6); + + Matrix3 R01_H_t01, R12_H_t12; + const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01); + const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12); + + Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 t02_H_R02; + Vector9 zeta02; + const Matrix3 R = R01.matrix(); + zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta + p01 + v01 * deltaT12 + R * p12, // position + v01 + R * v12; // velocity + + if (H1) { + H1->setIdentity(); + D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01; + D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01; + D_t_v(H1) = I_3x3 * deltaT12; + D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01; + } + + if (H2) { + H2->setZero(); + D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12; + D_t_t(H2) = R; + D_v_v(H2) = R; + } + + return zeta02; +} + +//------------------------------------------------------------------------------ +void TangentPreintegration::mergeWith(const TangentPreintegration& pim12, + Matrix9* H1, Matrix9* H2) { + if (!matchesParamsWith(pim12)) { + throw std::domain_error( + "Cannot merge pre-integrated measurements with different params"); + } + + if (params()->body_P_sensor) { + throw std::domain_error( + "Cannot merge pre-integrated measurements with sensor pose yet"); + } + + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); + deltaTij_ = t01 + t12; + + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. + + const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); + zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); + + preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2); + + preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h new file mode 100644 index 000000000..dddafad7a --- /dev/null +++ b/gtsam/navigation/TangentPreintegration.h @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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 TangentPreintegration.h + * @author Frank Dellaert + * @author Adam Bry + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * Integrate on the 9D tangent space of the NavState manifold. + * See extensive discussion in ImuFactor.lyx + */ +class TangentPreintegration : public PreintegrationBase { + protected: + + /** + * Preintegrated navigation state, as a 9D vector on tangent space at frame i + * Order is: theta, position, velocity + */ + Vector9 preintegrated_; + Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated_ w.r.t. acceleration bias + Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated_ w.r.t. angular rate bias + + /// Default constructor for serialization + TangentPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + TangentPreintegration(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } + Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } + Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } + NavState deltaXij() const override { return NavState().retract(preintegrated_); } + + const Vector9& preintegrated() const { return preintegrated_; } + Vector3 theta() const { return preintegrated_.head<3>(); } + const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; } + const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; } + + /// @name Testable + /// @{ + bool equals(const TangentPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + // Update integrated vector on tangent manifold preintegrated with acceleration + // Static, functional version. + static Vector9 UpdatePreintegrated(const Vector3& a_body, + const Vector3& w_body, const double dt, + const Vector9& preintegrated, + OptionalJacobian<9, 9> A = boost::none, + OptionalJacobian<9, 3> B = boost::none, + OptionalJacobian<9, 3> C = boost::none); + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + /// NOTE(frank): implementation is different in two versions + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, + const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + /// NOTE(frank): implementation is different in two versions + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const override; + + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives + static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, + double deltaT12, + OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none); + + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); + /// @} + + /** Dummy clone for MATLAB */ + virtual boost::shared_ptr clone() const { + return boost::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size())); + ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); + ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fa2e9d832..c1c17a6d4 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { } /* ************************************************************************* */ +#ifdef GTSAM_TANGENT_PREINTEGRATION TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; @@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } +#endif /* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity) { diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 54ca50797..d1dc6316d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -57,6 +57,41 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, } // namespace +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsConstruction) { + // Actual pre-integrated values + PreintegratedImuMeasurements actual(testing::Params()); + EXPECT(assert_equal(Rot3(), actual.deltaRij())); + EXPECT(assert_equal(kZero, actual.deltaPij())); + EXPECT(assert_equal(kZero, actual.deltaVij())); + DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); +} + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegratedMeasurementsReset) { + + auto p = testing::Params(); + // Create a preintegrated measurement struct and integrate + PreintegratedImuMeasurements pimActual(p); + Vector3 measuredAcc(0.5, 1.0, 0.5); + Vector3 measuredOmega(0.1, 0.3, 0.1); + double deltaT = 1.0; + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // reset and make sure that it is the same as a fresh one + pimActual.resetIntegration(); + CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p))); + + // Now create one with a different bias .. + Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); + PreintegratedImuMeasurements pimExpected(p, nonZeroBias); + + // integrate again, then reset to a new bias + pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pimActual.resetIntegrationAndSetBias(nonZeroBias); + CHECK(assert_equal(pimActual, pimExpected)); +} + /* ************************************************************************* */ TEST(ImuFactor, Accelerating) { const double a = 0.2, v = 50; @@ -83,24 +118,20 @@ TEST(ImuFactor, Accelerating) { /* ************************************************************************* */ TEST(ImuFactor, PreintegratedMeasurements) { // Measurements - Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + const double a = 0.1, w = M_PI / 100.0; + Vector3 measuredAcc(a, 0.0, 0.0); + Vector3 measuredOmega(w, 0.0, 0.0); double deltaT = 0.5; // Expected pre-integrated values - Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); - Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); + Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0); + Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0); // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(kZero, actual.theta())); - EXPECT(assert_equal(kZero, actual.deltaPij())); - EXPECT(assert_equal(kZero, actual.deltaVij())); - DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); - actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR1, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); @@ -129,7 +160,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(expectedDeltaR2, actual.theta())); + EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij())); EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); @@ -438,27 +469,6 @@ TEST(ImuFactor, fistOrderExponential) { EXPECT(assert_equal(expectedRot, actualRot)); } -/* ************************************************************************* */ -TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { - testing::SomeMeasurements measurements; - - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; - - // Actual pre-integrated values - PreintegratedImuMeasurements pim(testing::Params()); - testing::integrateMeasurements(measurements, &pim); - - EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), - pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), - pim.preintegrated_H_biasOmega(), 1e-3)); -} - /* ************************************************************************* */ Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { @@ -789,6 +799,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { } /* ************************************************************************* */ +#ifdef GTSAM_TANGENT_PREINTEGRATION static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { @@ -883,6 +894,7 @@ TEST(ImuFactor, MergeWithCoriolis) { mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); } +#endif /* ************************************************************************* */ // Same values as pre-integration test but now testing covariance diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp new file mode 100644 index 000000000..a3f688dda --- /dev/null +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * 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 testManifoldPreintegration.cpp + * @brief Unit test for the ManifoldPreintegration + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "imuFactorTesting.h" + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static boost::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; + + boost::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + boost::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + ManifoldPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + ManifoldPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT( + assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT( + assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT( + assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(ManifoldPreintegration, computeError) { + ManifoldPreintegration pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + boost::function f = + boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3, + boost::none, boost::none, boost::none); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index fb6045d33..8ea8ce9b5 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero(); /* ************************************************************************* */ TEST(NavState, Constructor) { + boost::function create = + boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none, + boost::none); + Matrix aH1, aH2, aH3; + EXPECT( + assert_equal(kState1, + NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); + EXPECT( + assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + EXPECT( + assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +} + +/* ************************************************************************* */ +TEST(NavState, Constructor2) { boost::function construct = boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::none); @@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) { EXPECT(assert_equal((Matrix )eH, aH)); } -/* ************************************************************************* */ -TEST( NavState, MatrixGroup ) { - // check roundtrip conversion to 7*7 matrix representation - Matrix7 T = kState1.matrix(); - EXPECT(assert_equal(kState1, NavState(T))); - - // check group product agrees with matrix product - NavState state2 = kState1 * kState1; - Matrix T2 = T * T; - EXPECT(assert_equal(state2, NavState(T2))); - EXPECT(assert_equal(T2, state2.matrix())); -} - /* ************************************************************************* */ TEST( NavState, Manifold ) { // Check zero xi @@ -114,7 +121,9 @@ TEST( NavState, Manifold ) { Rot3 drot = Rot3::Expmap(xi.head<3>()); Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); - NavState state2 = kState1 * NavState(drot, dt, dvel); + NavState state2 = NavState(kState1.attitude() * drot, + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -122,27 +131,6 @@ TEST( NavState, Manifold ) { NavState state3 = state2.retract(xi); EXPECT(assert_equal(xi, state2.localCoordinates(state3))); - // Check derivatives for ChartAtOrigin::Retract - Matrix9 aH; - // For zero xi - boost::function Retract = boost::bind( - NavState::ChartAtOrigin::Retract, _1, boost::none); - NavState::ChartAtOrigin::Retract(kZeroXi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Retract(xi, aH); - EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); - - // Check derivatives for ChartAtOrigin::Local - // For zero xi - boost::function Local = boost::bind( - NavState::ChartAtOrigin::Local, _1, boost::none); - NavState::ChartAtOrigin::Local(kIdentity, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); - // For non-zero xi - NavState::ChartAtOrigin::Local(kState1, aH); - EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); - // Check retract derivatives Matrix9 aH1, aH2; kState1.retract(xi, aH1, aH2); @@ -151,6 +139,12 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); + // Check retract derivatives on state 2 + const Vector9 xi2 = -3.0*xi; + state2.retract(xi2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1)); + EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2)); + // Check localCoordinates derivatives boost::function local = boost::bind(&NavState::localCoordinates, _1, _2, boost::none, @@ -169,29 +163,6 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } -/* ************************************************************************* */ -TEST( NavState, Lie ) { - // Check zero xi - EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); - EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); - EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); - - // Expmap/Logmap roundtrip - Vector xi(9); - xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; - NavState state2 = NavState::Expmap(xi); - EXPECT(assert_equal(xi, NavState::Logmap(state2))); - - // roundtrip from state2 to state3 and back - NavState state3 = state2.expmap(xi); - EXPECT(assert_equal(xi, state2.logmap(state3))); - - // For the expmap/logmap (not necessarily expmap/local) -xi goes other way - EXPECT(assert_equal(state2, state3.expmap(-xi))); - EXPECT(assert_equal(xi, -state3.logmap(state2))); -} - /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 TEST(NavState, Update) { @@ -201,8 +172,8 @@ TEST(NavState, Update) { Matrix9 aF; Matrix93 aG1, aG2; boost::function update = - boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, - boost::none, boost::none); + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, + boost::none, boost::none); Vector3 b_acc = kAttitude * acc; NavState expected(kAttitude.expmap(dt * omega), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp similarity index 72% rename from gtsam/navigation/tests/testPreintegrationBase.cpp rename to gtsam/navigation/tests/testTangentPreintegration.cpp index 527a6da7b..65fd55fa3 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -10,12 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file testPreintegrationBase.cpp + * @file testTangentPreintegration.cpp * @brief Unit test for the InertialNavFactor * @author Frank Dellaert */ -#include +#include #include #include #include @@ -29,7 +29,7 @@ static const double kDt = 0.1; Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { - return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta); + return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta); } namespace testing { @@ -44,8 +44,8 @@ static boost::shared_ptr Params() { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate1) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate1) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta.setZero(); @@ -58,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) { } /* ************************************************************************* */ -TEST(PreintegrationBase, UpdateEstimate2) { - PreintegrationBase pim(testing::Params()); +TEST(TangentPreintegration, UpdateEstimate2) { + TangentPreintegration pim(testing::Params()); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); Vector9 zeta; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; @@ -73,8 +73,31 @@ TEST(PreintegrationBase, UpdateEstimate2) { } /* ************************************************************************* */ -TEST(PreintegrationBase, computeError) { - PreintegrationBase pim(testing::Params()); +TEST(ImuFactor, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + boost::function preintegrated = + [=](const Vector3& a, const Vector3& w) { + TangentPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; + + // Actual pre-integrated values + TangentPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT( + assert_equal(numericalDerivative21(preintegrated, kZero, kZero), + pim.preintegrated_H_biasAcc())); + EXPECT( + assert_equal(numericalDerivative22(preintegrated, kZero, kZero), + pim.preintegrated_H_biasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(TangentPreintegration, computeError) { + TangentPreintegration pim(testing::Params()); NavState x1, x2; imuBias::ConstantBias bias; Matrix9 aH1, aH2; @@ -82,7 +105,7 @@ TEST(PreintegrationBase, computeError) { pim.computeError(x1, x2, bias, aH1, aH2, aH3); boost::function f = - boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, + boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3, boost::none, boost::none, boost::none); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -91,47 +114,47 @@ TEST(PreintegrationBase, computeError) { } /* ************************************************************************* */ -TEST(PreintegrationBase, Compose) { +TEST(TangentPreintegration, Compose) { testing::SomeMeasurements measurements; - PreintegrationBase pim(testing::Params()); + TangentPreintegration pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); boost::function f = [pim](const Vector9& zeta01, const Vector9& zeta12) { - return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); + return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij()); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); // Actual result Matrix9 H1, H2; - PreintegrationBase actual_pim02 = pim; + TangentPreintegration actual_pim02 = pim; actual_pim02.mergeWith(pim, &H1, &H2); const Vector9 zeta = pim.preintegrated(); const Vector9 actual_zeta = - PreintegrationBase::Compose(zeta, zeta, pim.deltaTij()); + TangentPreintegration::Compose(zeta, zeta, pim.deltaTij()); EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7)); EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); } /* ************************************************************************* */ -TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(TangentPreintegration, MergedBiasDerivatives) { testing::SomeMeasurements measurements; auto f = [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); + TangentPreintegration pim02(testing::Params(), Bias(a, w)); testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02); return pim02.preintegrated(); }; // Expected merge result - PreintegrationBase expected_pim02(testing::Params()); + TangentPreintegration expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index 2d7e36f47..afc6f331d 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -80,15 +80,19 @@ void exportImuFactor() { // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html register_ptr_to_python< boost::shared_ptr >(); - class_( - "PreintegrationBase", + class_( +#ifdef GTSAM_TANGENT_PREINTEGRATION + "TangentPreintegration", +#else + "ManifoldPreintegration", +#endif init&, const imuBias::ConstantBias&>()) - .def("predict", &PreintegrationBase::predict, predict_overloads()) - .def("computeError", &PreintegrationBase::computeError) - .def("resetIntegration", &PreintegrationBase::resetIntegration) - .def("deltaTij", &PreintegrationBase::deltaTij); + .def("predict", &PreintegrationType::predict, predict_overloads()) + .def("computeError", &PreintegrationType::computeError) + .def("resetIntegration", &PreintegrationType::resetIntegration) + .def("deltaTij", &PreintegrationType::deltaTij); - class_>( + class_>( "PreintegratedImuMeasurements", init&, const imuBias::ConstantBias&>()) .def(repr(self))