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