12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/restoreOldImuFactor (pull request #249) 

Enable both Tangent and Manifold IMU pre-integration
release/4.3a0
Frank Dellaert 2016-06-05 11:04:50 -07:00
parent 6f24ac8de9 05cc8d8567
commit 89f7a331bb
22 changed files with 1253 additions and 799 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
CMakeLists.txt
View File

@ -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_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_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) 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 # Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries # 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 " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ") message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") 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_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 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_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_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") 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 ") message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

134
README.md
View File

@ -1,59 +1,77 @@
README - Georgia Tech Smoothing and Mapping library README - Georgia Tech Smoothing and Mapping library
=================================================== ===================================================
What is GTSAM? What is GTSAM?
-------------- --------------
GTSAM is a library of C++ classes that implement smoothing and GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse networks as the underlying computing paradigm rather than sparse
matrices. matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development. is under development.
Quickstart Quickstart
---------- ----------
In the root library folder execute: In the root library folder execute:
``` ```
#!bash #!bash
$ mkdir build $ mkdir build
$ cd build $ cd build
$ cmake .. $ cmake ..
$ make check (optional, runs unit tests) $ make check (optional, runs unit tests)
$ make install $ make install
``` ```
Prerequisites: Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [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`) - [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. - A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake: 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 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) - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility 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. 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. 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 The Preintegrated IMU Factor
---------------------- ----------------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. GTSAM includes a state of the art IMU handling scheme based on
See the [`INSTALL`](INSTALL) file for more detailed installation instructions. - 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.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. Our implementation improves on this using integration on the manifold, as detailed in
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - 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). 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).

40
gtsam.h
View File

@ -2506,30 +2506,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const; bool getUse2ndOrderCoriolis() const;
}; };
#include <gtsam/navigation/PreintegrationBase.h>
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 <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { class PreintegratedImuMeasurements {
// Constructors // Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
@ -2544,6 +2522,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
Matrix preintMeasCov() const; 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 { virtual class ImuFactor: gtsam::NonlinearFactor {
@ -2559,7 +2544,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { class PreintegratedCombinedMeasurements {
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@ -2570,6 +2555,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
Matrix preintMeasCov() const; 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 { virtual class CombinedImuFactor: gtsam::NonlinearFactor {

3
gtsam/config.h.in
View File

@ -68,3 +68,6 @@
// Support Metis-based nested dissection // Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION
// Support Metis-based nested dissection
#cmakedefine GTSAM_TANGENT_PREINTEGRATION

76
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -31,22 +31,21 @@ using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements // Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::print( void PreintegratedCombinedMeasurements::print(const string& s) const {
const string& s) const { PreintegrationType::print(s);
PreintegrationBase::print(s);
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegratedCombinedMeasurements::equals( bool PreintegratedCombinedMeasurements::equals(
const PreintegratedCombinedMeasurements& other, double tol) const { const PreintegratedCombinedMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) && return PreintegrationType::equals(other, tol)
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() { void PreintegratedCombinedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationType::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
void PreintegratedCombinedMeasurements::integrateMeasurement( void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements. // Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C; 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 // Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF // order propagation that can be seen as a prediction phase in an EKF
@ -79,8 +78,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements // and preintegrated measurements
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
// TODO(frank): should we not also accout for bias on position? // TODO(frank): should we not also account for bias on position?
Matrix3 theta_H_biasOmega = - C.topRows<3>(); Matrix3 theta_H_biasOmega = -C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>(); Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov; D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc * D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) * * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
(vel_H_biasAcc.transpose()); * (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega * D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) * * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
(theta_H_biasOmega.transpose()); * (theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) * Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
theta_H_biasOmega.transpose(); * theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp; D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose(); D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt, const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) { const bool use2ndOrderIntegration) {
if (!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; biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD(); boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance; p->gyroscopeCovariance = measuredOmegaCovariance;
@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// CombinedImuFactor methods // CombinedImuFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor( CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim) const PreintegratedCombinedMeasurements& pim) :
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j), pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
_PIM_(pim) {} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { 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; Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements // error wrt preintegrated measurements
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, 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); H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
// if we need the jacobians // if we need the jacobians
@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor(
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor, const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis) const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j), pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) { _PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p = boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p()); boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->n_gravity = n_gravity; p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis; p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor; p->body_P_sensor = body_P_sensor;
@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor(
} }
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim, CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) { const bool use2ndOrderCoriolis) {
// use deprecated predict // use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis); omegaCoriolis, use2ndOrderCoriolis);
@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
} }
#endif #endif
} /// namespace gtsam }
/// namespace gtsam

23
gtsam/navigation/CombinedImuFactor.h
View File

@ -22,12 +22,19 @@
#pragma once #pragma once
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
namespace gtsam { namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/* /*
* If you are using the factor, please cite: * If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@ -57,7 +64,7 @@ namespace gtsam {
* *
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class PreintegratedCombinedMeasurements : public PreintegrationBase { class PreintegratedCombinedMeasurements : public PreintegrationType {
public: public:
@ -123,7 +130,7 @@ public:
PreintegratedCombinedMeasurements( PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p, const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) { : PreintegrationType(p, biasHat) {
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -133,10 +140,10 @@ public:
/// @{ /// @{
/// Re-initialize PreintegratedCombinedMeasurements /// Re-initialize PreintegratedCombinedMeasurements
void resetIntegration(); void resetIntegration() override;
/// const reference to params, shadows definition in base class /// const reference to params, shadows definition in base class
Params& p() const { return *boost::static_pointer_cast<Params>(p_);} Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
/// @} /// @}
/// @name Access instance variables /// @name Access instance variables
@ -146,7 +153,7 @@ public:
/// @name Testable /// @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; bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
/// @} /// @}
@ -163,7 +170,7 @@ public:
* frame) * frame)
*/ */
void integrateMeasurement(const Vector3& measuredAcc, 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; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { 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_); ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
} }
}; };

94
gtsam/navigation/ImuFactor.cpp
View File

@ -32,20 +32,20 @@ using namespace std;
// Inner class PreintegratedImuMeasurements // Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const { void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s); PreintegrationType::print(s);
cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegratedImuMeasurements::equals( bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const { const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationType::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() {
void PreintegratedImuMeasurements::integrateMeasurement( void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements (also get Jacobian) // 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; Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation: // first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a // 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(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 // 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, void PreintegratedImuMeasurements::integrateMeasurements(
const Matrix& measuredOmegas, const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts) { const Matrix& dts) {
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(
measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
assert(dts.cols() >= 1); assert(dts.cols() >= 1);
assert(measuredAccs.cols() == dts.cols()); assert(measuredAccs.cols() == dts.cols());
assert(measuredOmegas.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols());
size_t n = static_cast<size_t>(dts.cols()); size_t n = static_cast<size_t>(dts.cols());
for (size_t j = 0; j < n; j++) { 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, // #ifdef GTSAM_TANGENT_PREINTEGRATION
Matrix9* H1, Matrix9* H2) { void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
PreintegrationBase::mergeWith(pim12, H1, H2); Matrix9* H1, Matrix9* H2) {
PreintegrationType::mergeWith(pim12, H1, H2);
preintMeasCov_ = 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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedImuMeasurements::PreintegratedImuMeasurements( PreintegratedImuMeasurements::PreintegratedImuMeasurements(
@ -174,16 +176,17 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_TANGENT_PREINTEGRATION
PreintegratedImuMeasurements ImuFactor::Merge( PreintegratedImuMeasurements ImuFactor::Merge(
const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12) { const PreintegratedImuMeasurements& pim12) {
if (!pim01.matchesParamsWith(pim12)) if (!pim01.matchesParamsWith(pim12))
throw std::domain_error( throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with different params"); "Cannot merge PreintegratedImuMeasurements with different params");
if (pim01.params()->body_P_sensor) if (pim01.params()->body_P_sensor)
throw std::domain_error( throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with sensor pose yet"); "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
// the bias for the merged factor will be the bias from 01 // the bias for the merged factor will be the bias from 01
PreintegratedImuMeasurements pim02 = pim01; PreintegratedImuMeasurements pim02 = pim01;
@ -196,26 +199,27 @@ PreintegratedImuMeasurements ImuFactor::Merge(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) { const shared_ptr& f12) {
// IMU bias keys must be the same. // IMU bias keys must be the same.
if (f01->key5() != f12->key5()) 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. // expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
throw std::domain_error( throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up"); "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor // return new factor
auto pim02 = auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0 return boost::make_shared<ImuFactor>(f01->key1(),// P0
f01->key2(), // V0 f01->key2(),// V0
f12->key3(), // P2 f12->key3(),// P2
f12->key4(), // V2 f12->key4(),// V2
f01->key5(), // B f01->key5(),// B
pim02); pim02);
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@ -248,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// ImuFactor2 methods // ImuFactor2 methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), const PreintegratedImuMeasurements& pim) :
_PIM_(pim) {} Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
bias), _PIM_(pim) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NonlinearFactor::shared_ptr ImuFactor2::clone() const { 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 { void ImuFactor2::print(const string& s,
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) const KeyFormatter& keyFormatter) const {
<< "," << keyFormatter(this->key3()) << ")\n"; cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ")\n";
cout << *this << endl; 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, Vector ImuFactor2::evaluateError(const NavState& state_i,
const imuBias::ConstantBias& bias_i, // const NavState& state_j,
boost::optional<Matrix&> H1, const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const { boost::optional<Matrix&> H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
} }
// namespace gtsam // namespace gtsam

35
gtsam/navigation/ImuFactor.h
View File

@ -23,11 +23,18 @@
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h> #include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
namespace gtsam { namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/* /*
* If you are using the factor, please cite: * If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
@ -61,7 +68,7 @@ namespace gtsam {
* *
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class PreintegratedImuMeasurements: public PreintegrationBase { class PreintegratedImuMeasurements: public PreintegrationType {
friend class ImuFactor; friend class ImuFactor;
friend class ImuFactor2; friend class ImuFactor2;
@ -85,29 +92,28 @@ public:
*/ */
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p, PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationBase(p, biasHat) { PreintegrationType(p, biasHat) {
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
/** /**
* Construct preintegrated directly from members: base class and preintMeasCov * 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. * @param preintMeasCov Covariance matrix used in noise model.
*/ */
PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov) PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
: PreintegrationBase(base), : PreintegrationType(base),
preintMeasCov_(preintMeasCov) { preintMeasCov_(preintMeasCov) {
} }
/// print /// print
void print(const std::string& s = "Preintegrated Measurements:") const; void print(const std::string& s = "Preintegrated Measurements:") const override;
/// equals /// equals
bool equals(const PreintegratedImuMeasurements& expected, bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
double tol = 1e-9) const;
/// Re-initialize PreintegratedIMUMeasurements /// Re-initialize PreintegratedIMUMeasurements
void resetIntegration(); void resetIntegration() override;
/** /**
* Add a single IMU measurement to the preintegration. * Add a single IMU measurement to the preintegration.
@ -115,7 +121,8 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement * @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 /// Add multiple measurements, in matrix columns
void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
@ -124,8 +131,10 @@ public:
/// Return pre-integrated measurement covariance /// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge in a different set of measurements and update bias derivatives accordingly /// Merge in a different set of measurements and update bias derivatives accordingly
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated constructor /// @deprecated constructor
@ -150,7 +159,7 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization; 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())); ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
} }
}; };
@ -230,6 +239,7 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const; boost::none, boost::optional<Matrix&> H5 = boost::none) const;
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge two pre-integrated measurement classes /// Merge two pre-integrated measurement classes
static PreintegratedImuMeasurements Merge( static PreintegratedImuMeasurements Merge(
const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim01,
@ -237,6 +247,7 @@ public:
/// Merge two factors /// Merge two factors
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename /// @deprecated typename

143
gtsam/navigation/ManifoldPreintegration.cpp Normal file
View File

@ -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<Params>& 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

133
gtsam/navigation/ManifoldPreintegration.h Normal file
View File

@ -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 <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
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<Params>& 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<ManifoldPreintegration> clone() const {
return boost::shared_ptr<ManifoldPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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

173
gtsam/navigation/NavState.cpp
View File

@ -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_; #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, NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { 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); TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse(); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); 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);
NavState NavState::operator*(const NavState& bTc) const { if (H1) {
TIE(nRb, n_t, n_v, *this); *H1 << D_R_nRb, Z_3x3, Z_3x3, //
TIE(bRc, b_t, b_v, bTc); // Note(frank): the derivative of n_t with respect to xi is nRb
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); // 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:
NavState::PositionAndVelocity // nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
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();
} }
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, Vector9 NavState::localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H) { 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; Vector9 xi;
Matrix3 D_xi_R; Matrix3 D_xi_R;
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
if (H) { if (H1) {
*H << D_xi_R, Z_3x3, Z_3x3, // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
Z_3x3, x.R(), Z_3x3, // D_dt_R, -I_3x3, Z_3x3, //
Z_3x3, Z_3x3, x.R(); 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; 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<const Vector9, 3, 1> n_omega_nb = dR(xi);
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
Eigen::Block<const Vector9, 3, 1> 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<double>::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<double>::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 // sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0) #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) #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, NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const { OptionalJacobian<9, 3> G2) const {
@ -282,7 +210,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
} }
return newState; return newState;
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,

90
gtsam/navigation/NavState.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/ProductLieGroup.h> #include <gtsam/base/Manifold.h>
namespace gtsam { namespace gtsam {
@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
/** /**
* Navigation state: Pose (rotation, translation) + velocity * 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<NavState, 9> { class NavState {
private: private:
// TODO(frank): // TODO(frank):
@ -42,6 +42,10 @@ private:
public: public:
enum {
dimension = 9
};
typedef std::pair<Point3, Velocity3> PositionAndVelocity; typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors /// @name Constructors
@ -49,7 +53,7 @@ public:
/// Default constructor /// Default constructor
NavState() : NavState() :
t_(0,0,0), v_(Vector3::Zero()) { t_(0, 0, 0), v_(Vector3::Zero()) {
} }
/// Construct from attitude, position, velocity /// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) : NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@ -59,15 +63,15 @@ public:
NavState(const Pose3& pose, const Velocity3& v) : NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(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 /// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) : NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
} }
/// Named constructor with derivatives /// 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, static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
@ -116,7 +120,8 @@ public:
/// @{ /// @{
/// Output stream operator /// 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 /// print
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
@ -124,29 +129,6 @@ public:
/// equals /// equals
bool equals(const NavState& other, double tol = 1e-8) const; 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 /// @name Manifold
/// @{ /// @{
@ -172,44 +154,25 @@ public:
return v.segment<3>(6); return v.segment<3>(6);
} }
// Chart at origin, constructs components separately (as opposed to Expmap) /// retract with optional derivatives
struct ChartAtOrigin { NavState retract(const Vector9& v, //
static NavState Retract(const Vector9& xi, // OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
OptionalJacobian<9, 9> H = boost::none); boost::none) const;
static Vector9 Local(const NavState& x, //
OptionalJacobian<9, 9> H = boost::none);
};
/// @} /// localCoordinates with optional derivatives
/// @name Lie Group Vector9 localCoordinates(const NavState& g, //
/// @{ OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
/// 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);
/// @} /// @}
/// @name Dynamics /// @name Dynamics
/// @{ /// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Integrate forward in time given angular velocity and acceleration in body frame /// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt. /// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega, NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const; OptionalJacobian<9, 3> G2) const;
#endif
/// Compute tangent space contribution due to Coriolis forces /// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, 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 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<> template<>
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> { struct traits<NavState> : internal::Manifold<NavState> {
}; };
// Partial specialization of wedge
// TODO: deprecate, make part of traits
template<>
inline Matrix wedge<NavState>(const Vector& xi) {
return NavState::wedge(xi);
}
} // namespace gtsam } // namespace gtsam

241
gtsam/navigation/PreintegrationBase.cpp
View File

@ -31,21 +31,12 @@ namespace gtsam {
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p, PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
const Bias& biasHat) const Bias& biasHat)
: p_(p), biasHat_(biasHat), deltaTij_(0.0) { : 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) { ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
os << " deltaTij " << pim.deltaTij_ << endl; 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 << " deltaPij " << Point3(pim.deltaPij()) << endl;
os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl;
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << 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, void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
double tol) const { biasHat_ = biasHat;
return p_->equals(*other.p_, tol) resetIntegration();
&& 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);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -105,7 +91,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
// Update derivative: centrifugal causes the correlation between acc and omega!!! // Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) { if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm); 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() + correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose(); + 2 * b_arm * unbiasedOmega.transpose();
} }
@ -114,139 +101,38 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
return make_pair(correctedAcc, correctedOmega); 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, void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const Vector3& measuredOmega, double dt) {
double dt, Matrix9* A, // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
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,
// even when not of interest to the caller. Provide scratch space here. // even when not of interest to the caller. Provide scratch space here.
Matrix9 A; Matrix9 A;
Matrix93 B, C; Matrix93 B, C;
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); update(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;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i, NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2) const {
OptionalJacobian<9, 6> H2) const {
// TODO(frank): make sure this stuff is still correct // TODO(frank): make sure this stuff is still correct
Matrix96 D_biasCorrected_bias; Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = Vector9 biasCorrected = biasCorrectedDelta(bias_i,
biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); H2 ? &D_biasCorrected_bias : 0);
// Correct for initial velocity and gravity // Correct for initial velocity and gravity
Matrix9 D_delta_state, D_delta_biasCorrected; Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H1 ? &D_delta_state : 0, H2 ? &D_delta_biasCorrected : 0);
H2 ? &D_delta_biasCorrected : 0);
// Use retract to get back to NavState manifold // Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta; Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, 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 (H1)
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; *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; return state_j;
} }
@ -306,94 +192,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
return error; 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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
@ -408,6 +206,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
p_ = q; p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
} }
#endif #endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

100
gtsam/navigation/PreintegrationBase.h
View File

@ -75,29 +75,13 @@ class PreintegrationBase {
/// Time interval from i to j /// Time interval from i to j
double deltaTij_; 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 /// Default constructor for serialization
PreintegrationBase() { PreintegrationBase() {}
resetIntegration();
}
public: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// @name Constructors
/// @{
/** /**
* Constructor, initializes the variables in the base class * Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application * @param p Parameters, typically fixed in a single application
@ -111,7 +95,12 @@ public:
/// @name Basic utilities /// @name Basic utilities
/// @{ /// @{
/// Re-initialize PreintegratedMeasurements /// 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. /// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegrationBase& other) const { bool matchesParamsWith(const PreintegrationBase& other) const {
@ -140,17 +129,10 @@ public:
const imuBias::ConstantBias& biasHat() const { return biasHat_; } const imuBias::ConstantBias& biasHat() const { return biasHat_; }
double deltaTij() const { return deltaTij_; } double deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; } virtual Vector3 deltaPij() const=0;
virtual Vector3 deltaVij() const=0;
Vector3 theta() const { return preintegrated_.head<3>(); } virtual Rot3 deltaRij() const=0;
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } virtual NavState deltaXij() const=0;
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_; }
// Exposed for MATLAB // Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); } Vector6 biasHatVector() const { return biasHat_.vector(); }
@ -159,8 +141,7 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
void print(const std::string& s) const; virtual void print(const std::string& s) const;
bool equals(const PreintegrationBase& other, double tol) const;
/// @} /// @}
/// @name Main functionality /// @name Main functionality
@ -175,30 +156,20 @@ public:
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; 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 /// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame /// It takes measured quantities in the j frame
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
void integrateMeasurement(const Vector3& measuredAcc, virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const Vector3& measuredOmega, const double deltaT, const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
Matrix9* A, Matrix93* B, Matrix93* C);
// Version without derivatives /// Version without derivatives
void integrateMeasurement(const Vector3& measuredAcc, virtual void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT); const Vector3& measuredOmega, const double dt);
/// Given the estimate of the bias, return a NavState tangent vector /// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const; OptionalJacobian<9, 6> H = boost::none) const=0;
/// Predict state at time j /// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, 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 = OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; 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<PreintegrationBase> clone() const {
return boost::shared_ptr<PreintegrationBase>();
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
@ -249,20 +203,6 @@ public:
#endif #endif
/// @} /// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 } /// namespace gtsam

249
gtsam/navigation/TangentPreintegration.cpp Normal file
View File

@ -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 <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& 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

140
gtsam/navigation/TangentPreintegration.h Normal file
View File

@ -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 <gtsam/navigation/PreintegrationBase.h>
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<Params>& 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<TangentPreintegration> clone() const {
return boost::shared_ptr<TangentPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params(); auto p = testing::Params();
testing::SomeMeasurements measurements; testing::SomeMeasurements measurements;
@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1), EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasOmega(), 1e-3)); pim.preintegrated_H_biasOmega(), 1e-3));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity) { TEST(CombinedImuFactor, PredictPositionAndVelocity) {

76
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -57,6 +57,41 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
} // namespace } // 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) { TEST(ImuFactor, Accelerating) {
const double a = 0.2, v = 50; const double a = 0.2, v = 50;
@ -83,24 +118,20 @@ TEST(ImuFactor, Accelerating) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) { TEST(ImuFactor, PreintegratedMeasurements) {
// Measurements // Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0); const double a = 0.1, w = M_PI / 100.0;
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); Vector3 measuredAcc(a, 0.0, 0.0);
Vector3 measuredOmega(w, 0.0, 0.0);
double deltaT = 0.5; double deltaT = 0.5;
// Expected pre-integrated values // Expected pre-integrated values
Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0);
Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0);
Vector3 expectedDeltaV1(0.05, 0.0, 0.0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
// Actual pre-integrated values // Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params()); 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); 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(expectedDeltaP1, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
@ -129,7 +160,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
// Actual pre-integrated values // Actual pre-integrated values
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); 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(expectedDeltaP2, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
@ -438,27 +469,6 @@ TEST(ImuFactor, fistOrderExponential) {
EXPECT(assert_equal(expectedRot, actualRot)); EXPECT(assert_equal(expectedRot, actualRot));
} }
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> 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, Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
const Vector3& measuredAcc, const Vector3& measuredOmega) { 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; static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
struct ImuFactorMergeTest { struct ImuFactorMergeTest {
@ -883,6 +894,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
// Same values as pre-integration test but now testing covariance // Same values as pre-integration test but now testing covariance

114
gtsam/navigation/tests/testManifoldPreintegration.cpp Normal file
View File

@ -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 <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> 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<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> 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<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> 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);
}
/* ************************************************************************* */

91
gtsam/navigation/tests/testNavState.cpp
View File

@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, Constructor) { TEST(NavState, Constructor) {
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> 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<NavState(const Pose3&, const Vector3&)> construct = boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none); boost::none);
@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
EXPECT(assert_equal((Matrix )eH, aH)); 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 ) { TEST( NavState, Manifold ) {
// Check zero xi // Check zero xi
@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
Rot3 drot = Rot3::Expmap(xi.head<3>()); Rot3 drot = Rot3::Expmap(xi.head<3>());
Point3 dt = Point3(xi.segment<3>(3)); Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.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(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
NavState state3 = state2.retract(xi); NavState state3 = state2.retract(xi);
EXPECT(assert_equal(xi, state2.localCoordinates(state3))); EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
// Check derivatives for ChartAtOrigin::Retract
Matrix9 aH;
// For zero xi
boost::function<NavState(const Vector9&)> 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<Vector9(const NavState&)> 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 // Check retract derivatives
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
kState1.retract(xi, 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(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); 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 // Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local = boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); 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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) { TEST(NavState, Update) {
@ -201,8 +172,8 @@ TEST(NavState, Update) {
Matrix9 aF; Matrix9 aF;
Matrix93 aG1, aG2; Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update = boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none); boost::none, boost::none);
Vector3 b_acc = kAttitude * acc; Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(dt * omega), NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),

61
gtsam/navigation/tests/testPreintegrationBase.cpp → gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -10,12 +10,12 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testPreintegrationBase.cpp * @file testTangentPreintegration.cpp
* @brief Unit test for the InertialNavFactor * @brief Unit test for the InertialNavFactor
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/navigation/PreintegrationBase.h> #include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h> #include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h> #include <gtsam/nonlinear/ExpressionFactor.h>
@ -29,7 +29,7 @@
static const double kDt = 0.1; static const double kDt = 0.1;
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { 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 { namespace testing {
@ -44,8 +44,8 @@ static boost::shared_ptr<PreintegrationParams> Params() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, UpdateEstimate1) { TEST(TangentPreintegration, UpdateEstimate1) {
PreintegrationBase pim(testing::Params()); TangentPreintegration pim(testing::Params());
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta; Vector9 zeta;
zeta.setZero(); zeta.setZero();
@ -58,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, UpdateEstimate2) { TEST(TangentPreintegration, UpdateEstimate2) {
PreintegrationBase pim(testing::Params()); TangentPreintegration pim(testing::Params());
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3); const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta; Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3; zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
@ -73,8 +73,31 @@ TEST(PreintegrationBase, UpdateEstimate2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, computeError) { TEST(ImuFactor, BiasCorrectionJacobians) {
PreintegrationBase pim(testing::Params()); testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> 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; NavState x1, x2;
imuBias::ConstantBias bias; imuBias::ConstantBias bias;
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
@ -82,7 +105,7 @@ TEST(PreintegrationBase, computeError) {
pim.computeError(x1, x2, bias, aH1, aH2, aH3); pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&, boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f = const imuBias::ConstantBias&)> f =
boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3, boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none); boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 // 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(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
@ -91,47 +114,47 @@ TEST(PreintegrationBase, computeError) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, Compose) { TEST(TangentPreintegration, Compose) {
testing::SomeMeasurements measurements; testing::SomeMeasurements measurements;
PreintegrationBase pim(testing::Params()); TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim); testing::integrateMeasurements(measurements, &pim);
boost::function<Vector9(const Vector9&, const Vector9&)> f = boost::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) { [pim](const Vector9& zeta01, const Vector9& zeta12) {
return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij()); return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
}; };
// Expected merge result // Expected merge result
PreintegrationBase expected_pim02(testing::Params()); TangentPreintegration expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);
// Actual result // Actual result
Matrix9 H1, H2; Matrix9 H1, H2;
PreintegrationBase actual_pim02 = pim; TangentPreintegration actual_pim02 = pim;
actual_pim02.mergeWith(pim, &H1, &H2); actual_pim02.mergeWith(pim, &H1, &H2);
const Vector9 zeta = pim.preintegrated(); const Vector9 zeta = pim.preintegrated();
const Vector9 actual_zeta = 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(expected_pim02.preintegrated(), actual_zeta, 1e-7));
EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7)); EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7)); EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PreintegrationBase, MergedBiasDerivatives) { TEST(TangentPreintegration, MergedBiasDerivatives) {
testing::SomeMeasurements measurements; testing::SomeMeasurements measurements;
auto f = [=](const Vector3& a, const Vector3& w) { 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);
testing::integrateMeasurements(measurements, &pim02); testing::integrateMeasurements(measurements, &pim02);
return pim02.preintegrated(); return pim02.preintegrated();
}; };
// Expected merge result // Expected merge result
PreintegrationBase expected_pim02(testing::Params()); TangentPreintegration expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02);

18
python/handwritten/navigation/ImuFactor.cpp
View File

@ -80,15 +80,19 @@ void exportImuFactor() {
// NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html
register_ptr_to_python< boost::shared_ptr<PreintegrationParams> >(); register_ptr_to_python< boost::shared_ptr<PreintegrationParams> >();
class_<PreintegrationBase>( class_<PreintegrationType>(
"PreintegrationBase", #ifdef GTSAM_TANGENT_PREINTEGRATION
"TangentPreintegration",
#else
"ManifoldPreintegration",
#endif
init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>()) init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
.def("predict", &PreintegrationBase::predict, predict_overloads()) .def("predict", &PreintegrationType::predict, predict_overloads())
.def("computeError", &PreintegrationBase::computeError) .def("computeError", &PreintegrationType::computeError)
.def("resetIntegration", &PreintegrationBase::resetIntegration) .def("resetIntegration", &PreintegrationType::resetIntegration)
.def("deltaTij", &PreintegrationBase::deltaTij); .def("deltaTij", &PreintegrationType::deltaTij);
class_<PreintegratedImuMeasurements, bases<PreintegrationBase>>( class_<PreintegratedImuMeasurements, bases<PreintegrationType>>(
"PreintegratedImuMeasurements", "PreintegratedImuMeasurements",
init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>()) init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
.def(repr(self)) .def(repr(self))