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

Merged in feature/restoreOldImuFactor (pull request #249) 

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

2
CMakeLists.txt
View File

@ -68,6 +68,7 @@ option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
# Options relating to MATLAB wrapper # Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries # TODO: Check for matlab mex binary before handling building of binaries
@ -492,6 +493,7 @@ print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full Ex
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags ") message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

18
README.md
View File

@ -45,6 +45,24 @@ GTSAM 4 will introduce several new features, most notably Expressions and a pyth
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
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 Additional Information
---------------------- ----------------------

40
gtsam.h
View File

@ -2506,30 +2506,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const; bool getUse2ndOrderCoriolis() const;
}; };
#include <gtsam/navigation/PreintegrationBase.h>
virtual class PreintegrationBase {
// Constructors
PreintegrationBase(const gtsam::PreintegrationParams* params);
PreintegrationBase(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegrationBase& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
// Standard Interface
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase { class PreintegratedImuMeasurements {
// Constructors // Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
@ -2544,6 +2522,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
Matrix preintMeasCov() const; Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
}; };
virtual class ImuFactor: gtsam::NonlinearFactor { virtual class ImuFactor: gtsam::NonlinearFactor {
@ -2559,7 +2544,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase { class PreintegratedCombinedMeasurements {
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@ -2570,6 +2555,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
Matrix preintMeasCov() const; Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
}; };
virtual class CombinedImuFactor: gtsam::NonlinearFactor { virtual class CombinedImuFactor: gtsam::NonlinearFactor {

3
gtsam/config.h.in
View File

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

50
gtsam/navigation/CombinedImuFactor.cpp
View File

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

23
gtsam/navigation/CombinedImuFactor.h
View File

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

44
gtsam/navigation/ImuFactor.cpp
View File

@ -32,20 +32,20 @@ using namespace std;
// Inner class PreintegratedImuMeasurements // Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const { void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s); PreintegrationType::print(s);
cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegratedImuMeasurements::equals( bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const { const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::resetIntegration() { void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationType::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -55,7 +55,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// Update preintegrated measurements (also get Jacobian) // Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C; Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation: // first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a // as in [2] we consider a first order propagation that can be seen as a
@ -77,10 +77,11 @@ void PreintegratedImuMeasurements::integrateMeasurement(
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, void PreintegratedImuMeasurements::integrateMeasurements(
const Matrix& measuredOmegas, const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts) { const Matrix& dts) {
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); assert(
measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
assert(dts.cols() >= 1); assert(dts.cols() >= 1);
assert(measuredAccs.cols() == dts.cols()); assert(measuredAccs.cols() == dts.cols());
assert(measuredOmegas.cols() == dts.cols()); assert(measuredOmegas.cols() == dts.cols());
@ -91,13 +92,14 @@ void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredA
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_TANGENT_PREINTEGRATION
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) { Matrix9* H1, Matrix9* H2) {
PreintegrationBase::mergeWith(pim12, H1, H2); PreintegrationType::mergeWith(pim12, H1, H2);
preintMeasCov_ = preintMeasCov_ =
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedImuMeasurements::PreintegratedImuMeasurements( PreintegratedImuMeasurements::PreintegratedImuMeasurements(
@ -174,6 +176,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_TANGENT_PREINTEGRATION
PreintegratedImuMeasurements ImuFactor::Merge( PreintegratedImuMeasurements ImuFactor::Merge(
const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12) { const PreintegratedImuMeasurements& pim12) {
@ -216,6 +219,7 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
f01->key5(),// B f01->key5(),// B
pim02); pim02);
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@ -248,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// ImuFactor2 methods // ImuFactor2 methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), const PreintegratedImuMeasurements& pim) :
_PIM_(pim) {} Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
bias), _PIM_(pim) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NonlinearFactor::shared_ptr ImuFactor2::clone() const { NonlinearFactor::shared_ptr ImuFactor2::clone() const {
@ -266,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { void ImuFactor2::print(const string& s,
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) const KeyFormatter& keyFormatter) const {
<< "," << keyFormatter(this->key3()) << ")\n"; cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ")\n";
cout << *this << endl; cout << *this << endl;
} }
@ -281,10 +289,10 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, Vector ImuFactor2::evaluateError(const NavState& state_i,
const NavState& state_j,
const imuBias::ConstantBias& bias_i, // const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const { boost::optional<Matrix&> H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
} }

35
gtsam/navigation/ImuFactor.h
View File

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

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

@ -0,0 +1,143 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ManifoldPreintegration.cpp
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#include "ManifoldPreintegration.h"
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
ManifoldPreintegration::ManifoldPreintegration(
const boost::shared_ptr<Params>& p, const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_.setZero();
delPdelBiasAcc_.setZero();
delPdelBiasOmega_.setZero();
delVdelBiasAcc_.setZero();
delVdelBiasOmega_.setZero();
}
//------------------------------------------------------------------------------
bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& deltaXij_.equals(other.deltaXij_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// Update Jacobians
// TODO(frank): Try same simplification as in new approach
Matrix3 D_acc_R;
oldRij.rotate(acc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = omega * dt;
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
//------------------------------------------------------------------------------
Vector9 ManifoldPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
H ? &D_correctedRij_bias : 0);
if (H)
D_correctedRij_bias *= delRdelBiasOmega_;
Vector9 xi;
Matrix3 D_dR_correctedRij;
// TODO(frank): could line below be simplified? It is equivalent to
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
}
return xi;
}
//------------------------------------------------------------------------------
}// namespace gtsam

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

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ManifoldPreintegration.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
/**
* IMU pre-integration on NavSatet manifold.
* This corresponds to the original RSS paper (with one difference: V is rotated)
*/
class ManifoldPreintegration : public PreintegrationBase {
protected:
/**
* Pre-integrated navigation state, from frame i to frame j
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
NavState deltaXij_;
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
/// Default constructor for serialization
ManifoldPreintegration() {
resetIntegration();
}
public:
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application
* @param bias Current estimate of acceleration and rotation rate biases
*/
ManifoldPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
/// @}
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration() override;
/// @}
/// @name Instance variables access
/// @{
NavState deltaXij() const override { return deltaXij_; }
Rot3 deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position().vector(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); }
Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
/// @name Testable
/// @{
bool equals(const ManifoldPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override;
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
return boost::shared_ptr<ManifoldPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
}
};
} /// namespace gtsam

173
gtsam/navigation/NavState.cpp
View File

@ -24,6 +24,18 @@ namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
//------------------------------------------------------------------------------
NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3) {
if (H1)
*H1 << I_3x3, Z_3x3, Z_3x3;
if (H2)
*H2 << Z_3x3, R.transpose(), Z_3x3;
if (H3)
*H3 << Z_3x3, Z_3x3, R.transpose();
return NavState(R, t, v);
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
@ -94,138 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::inverse() const { NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
TIE(nRb, n_t, n_v, *this); TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse(); Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
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();
}
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);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::operator*(const NavState& bTc) const { Vector9 NavState::localCoordinates(const NavState& g, //
TIE(nRb, n_t, n_v, *this); OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
TIE(bRc, b_t, b_v, bTc); Matrix3 D_dR_R, D_dt_R, D_dv_R;
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); 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);
//------------------------------------------------------------------------------
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();
}
return result;
}
//------------------------------------------------------------------------------
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
OptionalJacobian<9, 9> H) {
Vector9 xi; Vector9 xi;
Matrix3 D_xi_R; Matrix3 D_xi_R;
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
if (H) { if (H1) {
*H << D_xi_R, Z_3x3, Z_3x3, // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
Z_3x3, x.R(), Z_3x3, // D_dt_R, -I_3x3, Z_3x3, //
Z_3x3, Z_3x3, x.R(); D_dv_R, Z_3x3, -I_3x3;
}
if (H2) {
*H2 << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, dR.matrix(), Z_3x3, //
Z_3x3, Z_3x3, dR.matrix();
} }
return xi; return xi;
} }
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Expmap derivative not implemented yet");
Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
Eigen::Block<const Vector9, 3, 1> a = dV(xi);
// NOTE(frank): See Pose3::Expmap
Rot3 nRb = Rot3::Expmap(n_omega_nb);
double theta2 = n_omega_nb.dot(n_omega_nb);
if (theta2 > numeric_limits<double>::epsilon()) {
// Expmap implements a "screw" motion in the direction of n_omega_nb
Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
/ theta2;
Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
return NavState(nRb, n_t, n_v);
} else {
return NavState(nRb, Point3(v), a);
}
}
//------------------------------------------------------------------------------
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p;
// NOTE(frank): See Pose3::Logmap
Vector9 xi;
Vector3 n_omega_nb = Rot3::Logmap(nRb);
double theta = n_omega_nb.norm();
if (theta * theta <= numeric_limits<double>::epsilon()) {
xi << n_omega_nb, n_t, n_v;
} else {
Matrix3 W = skewSymmetric(n_omega_nb / theta);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in n_t to avoid matrix math
double factor = (1 - theta / (2. * tan(0.5 * theta)));
Vector3 n_x = W * n_t;
Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
Vector3 n_y = W * n_v;
Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
xi << n_omega_nb, v, a;
}
return xi;
}
//------------------------------------------------------------------------------
Matrix9 NavState::AdjointMap() const {
// NOTE(frank): See Pose3::AdjointMap
const Matrix3 nRb = R();
Matrix3 pAr = skewSymmetric(t()) * nRb;
Matrix3 vAr = skewSymmetric(v()) * nRb;
Matrix9 adj;
// nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
return adj;
}
//------------------------------------------------------------------------------
Matrix7 NavState::wedge(const Vector9& xi) {
const Matrix3 Omega = skewSymmetric(dR(xi));
Matrix7 T;
T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
return T;
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// sugar for derivative blocks // sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0) #define D_R_R(H) (H)->block<3,3>(0,0)
@ -239,7 +168,6 @@ Matrix7 NavState::wedge(const Vector9& xi) {
#define D_v_v(H) (H)->block<3,3>(6,6) #define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const { OptionalJacobian<9, 3> G2) const {
@ -282,7 +210,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
} }
return newState; return newState;
} }
#endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,

88
gtsam/navigation/NavState.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/ProductLieGroup.h> #include <gtsam/base/Manifold.h>
namespace gtsam { namespace gtsam {
@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
/** /**
* Navigation state: Pose (rotation, translation) + velocity * Navigation state: Pose (rotation, translation) + velocity
* Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
*/ */
class NavState: public LieGroup<NavState, 9> { class NavState {
private: private:
// TODO(frank): // TODO(frank):
@ -42,6 +42,10 @@ private:
public: public:
enum {
dimension = 9
};
typedef std::pair<Point3, Velocity3> PositionAndVelocity; typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors /// @name Constructors
@ -59,15 +63,15 @@ public:
NavState(const Pose3& pose, const Velocity3& v) : NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) { R_(pose.rotation()), t_(pose.translation()), v_(v) {
} }
/// Construct from Matrix group representation (no checking)
NavState(const Matrix7& T) :
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
}
/// Construct from SO(3) and R^6 /// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) : NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
} }
/// Named constructor with derivatives /// Named constructor with derivatives
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3);
/// Named constructor with derivatives
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
@ -116,7 +120,8 @@ public:
/// @{ /// @{
/// Output stream operator /// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state); GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const NavState& state);
/// print /// print
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
@ -124,29 +129,6 @@ public:
/// equals /// equals
bool equals(const NavState& other, double tol = 1e-8) const; bool equals(const NavState& other, double tol = 1e-8) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
static NavState identity() {
return NavState();
}
/// inverse transformation with derivatives
NavState inverse() const;
/// Group compose is the semi-direct product as specified below:
/// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
NavState operator*(const NavState& bTc) const;
/// Native group action is on position/velocity pairs *in the body frame* as follows:
/// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
/// Act on position alone, n_t = nRb * b_t + n_t
Point3 operator*(const Point3& b_t) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
@ -172,44 +154,25 @@ public:
return v.segment<3>(6); return v.segment<3>(6);
} }
// Chart at origin, constructs components separately (as opposed to Expmap) /// retract with optional derivatives
struct ChartAtOrigin { NavState retract(const Vector9& v, //
static NavState Retract(const Vector9& xi, // OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
OptionalJacobian<9, 9> H = boost::none); boost::none) const;
static Vector9 Local(const NavState& x, //
OptionalJacobian<9, 9> H = boost::none);
};
/// @} /// localCoordinates with optional derivatives
/// @name Lie Group Vector9 localCoordinates(const NavState& g, //
/// @{ OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
/// Exponential map at identity - create a NavState from canonical coordinates
static NavState Expmap(const Vector9& xi, //
OptionalJacobian<9, 9> H = boost::none);
/// Log map at identity - return the canonical coordinates for this NavState
static Vector9 Logmap(const NavState& p, //
OptionalJacobian<9, 9> H = boost::none);
/// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
/// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
Matrix9 AdjointMap() const;
/// wedge creates Lie algebra element from tangent vector
static Matrix7 wedge(const Vector9& xi);
/// @} /// @}
/// @name Dynamics /// @name Dynamics
/// @{ /// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Integrate forward in time given angular velocity and acceleration in body frame /// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt. /// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega, NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const; OptionalJacobian<9, 3> G2) const;
#endif
/// Compute tangent space contribution due to Coriolis forces /// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
@ -239,14 +202,7 @@ private:
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<> template<>
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> { struct traits<NavState> : internal::Manifold<NavState> {
}; };
// Partial specialization of wedge
// TODO: deprecate, make part of traits
template<>
inline Matrix wedge<NavState>(const Vector& xi) {
return NavState::wedge(xi);
}
} // namespace gtsam } // namespace gtsam

237
gtsam/navigation/PreintegrationBase.cpp
View File

@ -31,21 +31,12 @@ namespace gtsam {
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p, PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
const Bias& biasHat) const Bias& biasHat)
: p_(p), biasHat_(biasHat), deltaTij_(0.0) { : p_(p), biasHat_(biasHat), deltaTij_(0.0) {
resetIntegration();
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
preintegrated_.setZero();
preintegrated_H_biasAcc_.setZero();
preintegrated_H_biasOmega_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const PreintegrationBase& pim) { ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
os << " deltaTij " << pim.deltaTij_ << endl; os << " deltaTij " << pim.deltaTij_ << endl;
os << " deltaRij " << Point3(pim.theta()) << endl; os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
os << " deltaPij " << Point3(pim.deltaPij()) << endl; os << " deltaPij " << Point3(pim.deltaPij()) << endl;
os << " deltaVij " << Point3(pim.deltaVij()) << endl; os << " deltaVij " << Point3(pim.deltaVij()) << endl;
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl; os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
@ -59,14 +50,9 @@ void PreintegrationBase::print(const string& s) const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other, void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
double tol) const { biasHat_ = biasHat;
return p_->equals(*other.p_, tol) resetIntegration();
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -105,7 +91,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
// Update derivative: centrifugal causes the correlation between acc and omega!!! // Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) { if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm); double wdp = correctedOmega.dot(b_arm);
*correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
*correctedAcc_H_unbiasedOmega = -( diag_wdp
+ correctedOmega * b_arm.transpose()) * bRs.matrix() + correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose(); + 2 * b_arm * unbiasedOmega.transpose();
} }
@ -114,139 +101,38 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
return make_pair(correctedAcc, correctedOmega); return make_pair(correctedAcc, correctedOmega);
} }
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 PreintegrationBase::UpdatePreintegrated(
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
return preintegratedPlus;
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const Vector3& measuredOmega, double dt) {
double dt, Matrix9* A, // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
Matrix93* B, Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Do update
deltaTij_ += dt;
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt) {
// NOTE(frank): integrateMeasuremtn always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here. // even when not of interest to the caller. Provide scratch space here.
Matrix9 A; Matrix9 A;
Matrix93 B, C; Matrix93 B, C;
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C); update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// We correct for a change between bias_i and the biasHat_ used to integrate
// This is a simple linear correction with obvious derivatives
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Vector9 biasCorrected =
preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
}
return biasCorrected;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i, NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const { OptionalJacobian<9, 6> H2) const {
// TODO(frank): make sure this stuff is still correct // TODO(frank): make sure this stuff is still correct
Matrix96 D_biasCorrected_bias; Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = Vector9 biasCorrected = biasCorrectedDelta(bias_i,
biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0); H2 ? &D_biasCorrected_bias : 0);
// Correct for initial velocity and gravity // Correct for initial velocity and gravity
Matrix9 D_delta_state, D_delta_biasCorrected; Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0); H2 ? &D_delta_biasCorrected : 0);
// Use retract to get back to NavState manifold // Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta; Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state; if (H1)
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias; *H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
return state_j; return state_j;
} }
@ -306,94 +192,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
return error; return error;
} }
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
const auto t12 = zeta12.segment<3>(0);
const auto p12 = zeta12.segment<3>(3);
const auto v12 = zeta12.segment<3>(6);
Matrix3 R01_H_t01, R12_H_t12;
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setIdentity();
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
D_t_v(H1) = I_3x3 * deltaT12;
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
}
if (H2) {
H2->setZero();
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
D_t_t(H2) = R;
D_v_v(H2) = R;
}
return zeta02;
}
//------------------------------------------------------------------------------
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) {
if (!matchesParamsWith(pim12)) {
throw std::domain_error("Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor) {
throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
}
const double t01 = deltaTij();
const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
const Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ =
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
(*H2) * pim12.preintegrated_H_biasOmega_;
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
@ -408,6 +206,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
p_ = q; p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
} }
#endif #endif
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

100
gtsam/navigation/PreintegrationBase.h
View File

@ -75,29 +75,13 @@ class PreintegrationBase {
/// Time interval from i to j /// Time interval from i to j
double deltaTij_; double deltaTij_;
/**
* Preintegrated navigation state, from frame i to frame j
* Order is: theta, position, velocity
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
Vector9 preintegrated_;
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
/// Default constructor for serialization /// Default constructor for serialization
PreintegrationBase() { PreintegrationBase() {}
resetIntegration();
}
public: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// @name Constructors
/// @{
/** /**
* Constructor, initializes the variables in the base class * Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application * @param p Parameters, typically fixed in a single application
@ -111,7 +95,12 @@ public:
/// @name Basic utilities /// @name Basic utilities
/// @{ /// @{
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(); virtual void resetIntegration()=0;
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements and set new bias
void resetIntegrationAndSetBias(const Bias& biasHat);
/// check parameters equality: checks whether shared pointer points to same Params object. /// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegrationBase& other) const { bool matchesParamsWith(const PreintegrationBase& other) const {
@ -140,17 +129,10 @@ public:
const imuBias::ConstantBias& biasHat() const { return biasHat_; } const imuBias::ConstantBias& biasHat() const { return biasHat_; }
double deltaTij() const { return deltaTij_; } double deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; } virtual Vector3 deltaPij() const=0;
virtual Vector3 deltaVij() const=0;
Vector3 theta() const { return preintegrated_.head<3>(); } virtual Rot3 deltaRij() const=0;
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); } virtual NavState deltaXij() const=0;
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
// Exposed for MATLAB // Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); } Vector6 biasHatVector() const { return biasHat_.vector(); }
@ -159,8 +141,7 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
void print(const std::string& s) const; virtual void print(const std::string& s) const;
bool equals(const PreintegrationBase& other, double tol) const;
/// @} /// @}
/// @name Main functionality /// @name Main functionality
@ -175,30 +156,20 @@ public:
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Update preintegrated measurements and get derivatives /// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame /// It takes measured quantities in the j frame
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
void integrateMeasurement(const Vector3& measuredAcc, virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const Vector3& measuredOmega, const double deltaT, const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
Matrix9* A, Matrix93* B, Matrix93* C);
// Version without derivatives /// Version without derivatives
void integrateMeasurement(const Vector3& measuredAcc, virtual void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT); const Vector3& measuredOmega, const double dt);
/// Given the estimate of the bias, return a NavState tangent vector /// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const; OptionalJacobian<9, 6> H = boost::none) const=0;
/// Predict state at time j /// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
@ -219,23 +190,6 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
/// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
/// @}
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<PreintegrationBase> clone() const {
return boost::shared_ptr<PreintegrationBase>();
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
@ -249,20 +203,6 @@ public:
#endif #endif
/// @} /// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
}
}; };
} /// namespace gtsam } /// namespace gtsam

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

@ -0,0 +1,249 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TangentPreintegration.cpp
* @author Frank Dellaert
* @author Adam Bry
**/
#include "TangentPreintegration.h"
#include <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
//------------------------------------------------------------------------------
void TangentPreintegration::resetIntegration() {
deltaTij_ = 0.0;
preintegrated_.setZero();
preintegrated_H_biasAcc_.setZero();
preintegrated_H_biasOmega_.setZero();
}
//------------------------------------------------------------------------------
bool TangentPreintegration::equals(const TangentPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_,
other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_,
other.preintegrated_H_biasOmega_, tol);
}
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt, const Vector9& preintegrated,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
return preintegratedPlus;
}
//------------------------------------------------------------------------------
void TangentPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Do update
deltaTij_ += dt;
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
// where acc_H_biasAcc = -I_3x3, hence
// new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
// where omega_H_biasOmega = -I_3x3, hence
// new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// We correct for a change between bias_i and the biasHat_ used to integrate
// This is a simple linear correction with obvious derivatives
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Vector9 biasCorrected = preintegrated()
+ preintegrated_H_biasAcc_ * biasIncr.accelerometer()
+ preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
}
return biasCorrected;
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
const auto t12 = zeta12.segment<3>(0);
const auto p12 = zeta12.segment<3>(3);
const auto v12 = zeta12.segment<3>(6);
Matrix3 R01_H_t01, R12_H_t12;
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setIdentity();
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
D_t_v(H1) = I_3x3 * deltaT12;
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
}
if (H2) {
H2->setZero();
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
D_t_t(H2) = R;
D_v_v(H2) = R;
}
return zeta02;
}
//------------------------------------------------------------------------------
void TangentPreintegration::mergeWith(const TangentPreintegration& pim12,
Matrix9* H1, Matrix9* H2) {
if (!matchesParamsWith(pim12)) {
throw std::domain_error(
"Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor) {
throw std::domain_error(
"Cannot merge pre-integrated measurements with sensor pose yet");
}
const double t01 = deltaTij();
const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
const Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_
+ (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_
+ (*H2) * pim12.preintegrated_H_biasOmega_;
}
//------------------------------------------------------------------------------
}// namespace gtsam

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

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TangentPreintegration.h
* @author Frank Dellaert
* @author Adam Bry
**/
#pragma once
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
/**
* Integrate on the 9D tangent space of the NavState manifold.
* See extensive discussion in ImuFactor.lyx
*/
class TangentPreintegration : public PreintegrationBase {
protected:
/**
* Preintegrated navigation state, as a 9D vector on tangent space at frame i
* Order is: theta, position, velocity
*/
Vector9 preintegrated_;
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated_ w.r.t. acceleration bias
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated_ w.r.t. angular rate bias
/// Default constructor for serialization
TangentPreintegration() {
resetIntegration();
}
public:
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application
* @param bias Current estimate of acceleration and rotation rate biases
*/
TangentPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
/// @}
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration() override;
/// @}
/// @name Instance variables access
/// @{
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
NavState deltaXij() const override { return NavState().retract(preintegrated_); }
const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
/// @name Testable
/// @{
bool equals(const TangentPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, const double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
/// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9
void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
/// @}
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<TangentPreintegration> clone() const {
return boost::shared_ptr<TangentPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
}
};
} /// namespace gtsam

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

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

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

@ -57,6 +57,41 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
} // namespace } // namespace
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsConstruction) {
// Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(Rot3(), actual.deltaRij()));
EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
}
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsReset) {
auto p = testing::Params();
// Create a preintegrated measurement struct and integrate
PreintegratedImuMeasurements pimActual(p);
Vector3 measuredAcc(0.5, 1.0, 0.5);
Vector3 measuredOmega(0.1, 0.3, 0.1);
double deltaT = 1.0;
pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// reset and make sure that it is the same as a fresh one
pimActual.resetIntegration();
CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p)));
// Now create one with a different bias ..
Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
PreintegratedImuMeasurements pimExpected(p, nonZeroBias);
// integrate again, then reset to a new bias
pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pimActual.resetIntegrationAndSetBias(nonZeroBias);
CHECK(assert_equal(pimActual, pimExpected));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, Accelerating) { TEST(ImuFactor, Accelerating) {
const double a = 0.2, v = 50; const double a = 0.2, v = 50;
@ -83,24 +118,20 @@ TEST(ImuFactor, Accelerating) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) { TEST(ImuFactor, PreintegratedMeasurements) {
// Measurements // Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0); const double a = 0.1, w = M_PI / 100.0;
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); Vector3 measuredAcc(a, 0.0, 0.0);
Vector3 measuredOmega(w, 0.0, 0.0);
double deltaT = 0.5; double deltaT = 0.5;
// Expected pre-integrated values // Expected pre-integrated values
Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0); Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0);
Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0); Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0);
Vector3 expectedDeltaV1(0.05, 0.0, 0.0); Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
// Actual pre-integrated values // Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params()); PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(kZero, actual.theta()));
EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, actual.theta())); EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij())); EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9); DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
@ -129,7 +160,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
// Actual pre-integrated values // Actual pre-integrated values
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, actual.theta())); EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij())); EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij())); EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9); DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
@ -438,27 +469,6 @@ TEST(ImuFactor, fistOrderExponential) {
EXPECT(assert_equal(expectedRot, actualRot)); EXPECT(assert_equal(expectedRot, actualRot));
} }
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
PreintegratedImuMeasurements pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
const Vector3& measuredAcc, const Vector3& measuredOmega) { const Vector3& measuredAcc, const Vector3& measuredOmega) {
@ -789,6 +799,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
struct ImuFactorMergeTest { struct ImuFactorMergeTest {
@ -883,6 +894,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1); mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
// Same values as pre-integration test but now testing covariance // Same values as pre-integration test but now testing covariance

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

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testManifoldPreintegration.cpp
* @brief Unit test for the ManifoldPreintegration
* @author Luca Carlone
*/
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
auto p = PreintegrationParams::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
return p;
}
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaVij();
};
// Actual pre-integrated values
ManifoldPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(
assert_equal(numericalDerivative21(deltaRij, kZero, kZero),
Matrix3(Z_3x3)));
EXPECT(
assert_equal(numericalDerivative22(deltaRij, kZero, kZero),
pim.delRdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaPij, kZero, kZero),
pim.delPdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaPij, kZero, kZero),
pim.delPdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaVij, kZero, kZero),
pim.delVdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaVij, kZero, kZero),
pim.delVdelBiasOmega(), 1e-3));
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, computeError) {
ManifoldPreintegration pim(testing::Params());
NavState x1, x2;
imuBias::ConstantBias bias;
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, Constructor) { TEST(NavState, Constructor) {
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
Matrix aH1, aH2, aH3;
EXPECT(
assert_equal(kState1,
NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
EXPECT(
assert_equal(
numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
}
/* ************************************************************************* */
TEST(NavState, Constructor2) {
boost::function<NavState(const Pose3&, const Vector3&)> construct = boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none); boost::none);
@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
EXPECT(assert_equal((Matrix )eH, aH)); EXPECT(assert_equal((Matrix )eH, aH));
} }
/* ************************************************************************* */
TEST( NavState, MatrixGroup ) {
// check roundtrip conversion to 7*7 matrix representation
Matrix7 T = kState1.matrix();
EXPECT(assert_equal(kState1, NavState(T)));
// check group product agrees with matrix product
NavState state2 = kState1 * kState1;
Matrix T2 = T * T;
EXPECT(assert_equal(state2, NavState(T2)));
EXPECT(assert_equal(T2, state2.matrix()));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NavState, Manifold ) { TEST( NavState, Manifold ) {
// Check zero xi // Check zero xi
@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
Rot3 drot = Rot3::Expmap(xi.head<3>()); Rot3 drot = Rot3::Expmap(xi.head<3>());
Point3 dt = Point3(xi.segment<3>(3)); Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
NavState state2 = kState1 * NavState(drot, dt, dvel); NavState state2 = NavState(kState1.attitude() * drot,
kState1.position() + kState1.attitude() * dt,
kState1.velocity() + kState1.attitude() * dvel);
EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
NavState state3 = state2.retract(xi); NavState state3 = state2.retract(xi);
EXPECT(assert_equal(xi, state2.localCoordinates(state3))); EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
// Check derivatives for ChartAtOrigin::Retract
Matrix9 aH;
// For zero xi
boost::function<NavState(const Vector9&)> Retract = boost::bind(
NavState::ChartAtOrigin::Retract, _1, boost::none);
NavState::ChartAtOrigin::Retract(kZeroXi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
// For non-zero xi
NavState::ChartAtOrigin::Retract(xi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
// Check derivatives for ChartAtOrigin::Local
// For zero xi
boost::function<Vector9(const NavState&)> Local = boost::bind(
NavState::ChartAtOrigin::Local, _1, boost::none);
NavState::ChartAtOrigin::Local(kIdentity, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
// For non-zero xi
NavState::ChartAtOrigin::Local(kState1, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
// Check retract derivatives // Check retract derivatives
Matrix9 aH1, aH2; Matrix9 aH1, aH2;
kState1.retract(xi, aH1, aH2); kState1.retract(xi, aH1, aH2);
@ -151,6 +139,12 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
// Check retract derivatives on state 2
const Vector9 xi2 = -3.0*xi;
state2.retract(xi2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
// Check localCoordinates derivatives // Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local = boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
} }
/* ************************************************************************* */
TEST( NavState, Lie ) {
// Check zero xi
EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
// Expmap/Logmap roundtrip
Vector xi(9);
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
NavState state2 = NavState::Expmap(xi);
EXPECT(assert_equal(xi, NavState::Logmap(state2)));
// roundtrip from state2 to state3 and back
NavState state3 = state2.expmap(xi);
EXPECT(assert_equal(xi, state2.logmap(state3)));
// For the expmap/logmap (not necessarily expmap/local) -xi goes other way
EXPECT(assert_equal(state2, state3.expmap(-xi)));
EXPECT(assert_equal(xi, -state3.logmap(state2)));
}
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) { TEST(NavState, Update) {

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

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

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

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