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

Brought some relevant changes from aspn-imu-factor branch (is correct this time)

release/4.3a0
Richard Roberts 2013-08-02 16:04:17 +00:00
parent 3545b114ab
commit 34dc67c1af
11 changed files with 2306 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
gtsam.h
View File

@ -2079,8 +2079,9 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree {
gtsam::Values getLinearizationPoint() const;
gtsam::Values calculateEstimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::Value calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::VectorValues getDelta() const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
gtsam::Ordering getOrdering() const;
@ -2122,8 +2123,8 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NonlinearFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2133,8 +2134,8 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NonlinearFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@ -2144,8 +2145,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NonlinearFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
// Constructor - allows inexact evaluation
@ -2284,6 +2285,47 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
//*************************************************************************
// Navigation
//*************************************************************************
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value {
// Standard Constructor
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
// Testable
void print(string s) const;
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
// Group
static gtsam::imuBias::ConstantBias identity();
gtsam::imuBias::ConstantBias inverse() const;
gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const;
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
// Lie Group
static gtsam::imuBias::ConstantBias Expmap(Vector v);
static Vector Logmap(const gtsam::imuBias::ConstantBias& b);
// Standard Interface
Vector vector() const;
Vector accelerometer() const;
Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const;
Vector correctGyroscope(Vector measurement) const;
};
}///\namespace imuBias
//*************************************************************************
// Utilities
//*************************************************************************

51
gtsam/navigation/ImuBias.h
View File

@ -70,21 +70,21 @@ namespace imuBias {
const Vector3& gyroscope() const { return biasGyro_; }
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector correctAccelerometer(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const {
Vector correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){
H->resize(3,6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
}
return measurment - biasAcc_;
return measurement - biasAcc_;
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector correctGyroscope(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const {
Vector correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){
H->resize(3,6);
(*H) << Matrix3::Zero(), -Matrix3::Identity();
}
return measurment - biasGyro_;
return measurement - biasGyro_;
}
// // H1: Jacobian w.r.t. IMUBias
@ -148,30 +148,14 @@ namespace imuBias {
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& t2) const { return t2.vector() - vector(); }
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/// @}
/// @name Group
/// @{
ConstantBias compose(const ConstantBias& b2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return ConstantBias(biasAcc_ + b2.biasAcc_, biasGyro_ + b2.biasGyro_);
}
/** between operation */
ConstantBias between(const ConstantBias& b2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return ConstantBias(b2.biasAcc_ - biasAcc_, b2.biasGyro_ - biasGyro_);
}
/** identity for group operation */
static ConstantBias identity() { return ConstantBias(); }
/** invert the object and yield a new one */
inline ConstantBias inverse(boost::optional<Matrix&> H=boost::none) const {
@ -180,6 +164,25 @@ namespace imuBias {
return ConstantBias(-biasAcc_, -biasGyro_);
}
ConstantBias compose(const ConstantBias& b,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
}
/** between operation */
ConstantBias between(const ConstantBias& b,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -gtsam::Matrix::Identity(6,6);
if(H2) *H2 = gtsam::Matrix::Identity(6,6);
return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_);
}
/// @}
/// @name Lie Group
/// @{
@ -188,7 +191,7 @@ namespace imuBias {
static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); }
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const ConstantBias& p) { return p.vector(); }
static inline Vector Logmap(const ConstantBias& b) { return b.vector(); }
/// @}

68
gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -677,6 +677,74 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
}
#include <gtsam/linear/GaussianFactorGraph.h>
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, LinearizeTiming)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4);
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
LieVector Vel2 = Vel1.compose( dv );
imuBias::ConstantBias Bias1;
Values values;
values.insert(PoseKey1, Pose1);
values.insert(PoseKey2, Pose2);
values.insert(VelKey1, Vel1);
values.insert(VelKey2, Vel2);
values.insert(BiasKey1, Bias1);
Ordering ordering;
ordering.push_back(PoseKey1);
ordering.push_back(VelKey1);
ordering.push_back(BiasKey1);
ordering.push_back(PoseKey2);
ordering.push_back(VelKey2);
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = f.linearize(values, ordering);
graph.push_back(g);
}
gttoc_(LinearizeTiming);
tictoc_finishedIteration_();
tictoc_print_();
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

15
gtsam/nonlinear/ISAM2.cpp
View File

@ -1019,10 +1019,10 @@ Values ISAM2::calculateEstimate() const {
}
/* ************************************************************************* */
Matrix ISAM2::marginalCovariance(Index key) const {
return marginalFactor(ordering_[key],
params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
->information().inverse();
const Value& ISAM2::calculateEstimate(Key key) const {
const Index index = getOrdering()[key];
const Vector& delta = getDelta()[index];
return *theta_.at(key).retract_(delta);
}
/* ************************************************************************* */
@ -1032,6 +1032,13 @@ Values ISAM2::calculateBestEstimate() const {
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
Matrix ISAM2::marginalCovariance(Index key) const {
return marginalFactor(ordering_[key],
params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
->information().inverse();
}
/* ************************************************************************* */
const VectorValues& ISAM2::getDelta() const {
if(!deltaUptodate_)

9
gtsam/nonlinear/ISAM2.h
View File

@ -585,6 +585,15 @@ public:
template<class VALUE>
VALUE calculateEstimate(Key key) const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables. This is a non-templated version
* that returns a Value base class for use with the MATLAB wrapper.
* @param key
* @return
*/
GTSAM_EXPORT const Value& calculateEstimate(Key key) const;
/** Return marginal on any variable as a covariance matrix */
GTSAM_EXPORT Matrix marginalCovariance(Index key) const;

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -93,7 +93,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
/* ************************************************************************* */
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5);
if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
return PoseRTV(Rt_.inverse(), v_.inverse());
}

69
gtsam_unstable/gtsam_unstable.h
View File

@ -13,6 +13,7 @@ virtual class gtsam::Point3;
virtual class gtsam::Rot3;
virtual class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::imuBias::ConstantBias;
virtual class gtsam::NonlinearFactor;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
@ -614,6 +615,74 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor {
InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
};
#include <gtsam_unstable/slam/ImuFactor.h>
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::noiseModel::Base* model);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::noiseModel::Base* model, const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam_unstable/slam/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::noiseModel::Base* model);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam_unstable/slam/Mechanization_bRn2.h>
class Mechanization_bRn2 {
Mechanization_bRn2();

646
gtsam_unstable/slam/CombinedImuFactor.h Normal file
View File

@ -0,0 +1,646 @@
/* ----------------------------------------------------------------------------
* 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 CombinedImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts
**/
#pragma once
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
}
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
}
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class CombinedPreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
double deltaTij; ///< Time interval from i to j
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
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
/** Default constructor, initialize with no IMU measurements */
CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
{
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
}
CombinedPreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
{
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
}
/** equals */
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const {
return biasHat.equals(expected.biasHat, tol)
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
&& equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
&& equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
&& deltaRij.equals(expected.deltaRij, tol)
&& std::fabs(deltaTij - expected.deltaTij) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
// delPdelBiasAcc += delVdelBiasAcc * deltaT;
// delPdelBiasOmega += delVdelBiasOmega * deltaT;
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
// Single Jacobians to propagate covariance
Matrix3 H_pos_pos = I_3x3;
Matrix3 H_pos_vel = I_3x3 * deltaT;
Matrix3 H_pos_angles = Z_3x3;
Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
Matrix3 H_angles_pos = Z_3x3;
Matrix3 H_angles_vel = Z_3x3;
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15,15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) *
(measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) *
(measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3);
// OFF BLOCK DIAGONAL TERMS
Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(3,9,3,3) = block24;
G_measCov_Gt.block(9,3,3,3) = block24.transpose();
Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3);
G_measCov_Gt.block(6,12,3,3) = block35;
G_measCov_Gt.block(12,6,3,3) = block35.transpose();
PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
ar & BOOST_SERIALIZATION_NVP(deltaPij);
ar & BOOST_SERIALIZATION_NVP(deltaVij);
ar & BOOST_SERIALIZATION_NVP(deltaRij);
ar & BOOST_SERIALIZATION_NVP(deltaTij);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
}
};
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
public:
/** Shorthand for a smart pointer to a factor */
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
/** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
/** Constructor */
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor) {
}
virtual ~CombinedImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "CombinedImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// We compute factor's Jacobians, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(15,6);
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero(),
//dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
}
if(H2) {
H2->resize(15,3);
(*H2) <<
// dfP/dVi
- Matrix3::Identity() * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
}
if(H3) {
H3->resize(15,6);
(*H3) <<
// dfP/dPosej
Matrix3::Zero(), Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(),
//dBiasAcc/dPosej
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPosej
Matrix3::Zero(), Matrix3::Zero();
}
if(H4) {
H4->resize(15,3);
(*H4) <<
// dfP/dVj
Matrix3::Zero(),
// dfV/dVj
Matrix3::Identity(),
// dfR/dVj
Matrix3::Zero(),
//dBiasAcc/dVj
Matrix3::Zero(),
//dBiasOmega/dVj
Matrix3::Zero();
}
if(H5) {
const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H5->resize(15,6);
(*H5) <<
// dfP/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
// dfV/dBias_i
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
// dfR/dBias_i
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
//dBiasAcc/dBias_i
-Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_i
Matrix3::Zero(), -Matrix3::Identity();
}
if(H6) {
H6->resize(15,6);
(*H6) <<
// dfP/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfV/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfR/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dBias_j
Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_j
Matrix3::Zero(), Matrix3::Identity();
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
return r;
}
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
bias_j = bias_i;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class CombinedImuFactor
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
} /// namespace gtsam

559
gtsam_unstable/slam/ImuFactor.h Normal file
View File

@ -0,0 +1,559 @@
/* ----------------------------------------------------------------------------
* 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 ImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts
**/
#pragma once
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h>
/* External or standard includes */
#include <ostream>
namespace gtsam {
/**
*
* @addtogroup SLAM
* * REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
}
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in [1] */
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
}
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
class PreintegratedMeasurements {
public:
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
double deltaTij; ///< Time interval from i to j
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
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
/** Default constructor, initialize with no IMU measurements */
PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc
) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
{
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance;
PreintMeasCov = Matrix::Zero(9,9);
}
PreintegratedMeasurements() :
biasHat(imuBias::ConstantBias()), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
{
measurementCovariance = Matrix::Zero(9,9);
PreintMeasCov = Matrix::Zero(9,9);
}
/** print */
void print(const std::string& s = "Preintegrated Measurements:") const {
std::cout << s << std::endl;
biasHat.print(" biasHat");
std::cout << " deltaTij " << deltaTij << std::endl;
std::cout << " deltaPij [ " << deltaPij.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij.transpose() << " ]" << std::endl;
deltaRij.print(" deltaRij ");
std::cout << " measurementCovariance [ " << measurementCovariance << " ]" << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov << " ]" << std::endl;
}
/** equals */
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
return biasHat.equals(expected.biasHat, tol)
&& equal_with_abs_tol(measurementCovariance, expected.measurementCovariance, tol)
&& equal_with_abs_tol(deltaPij, expected.deltaPij, tol)
&& equal_with_abs_tol(deltaVij, expected.deltaVij, tol)
&& deltaRij.equals(expected.deltaRij, tol)
&& std::fabs(deltaTij - expected.deltaTij) < tol
&& equal_with_abs_tol(delPdelBiasAcc, expected.delPdelBiasAcc, tol)
&& equal_with_abs_tol(delPdelBiasOmega, expected.delPdelBiasOmega, tol)
&& equal_with_abs_tol(delVdelBiasAcc, expected.delVdelBiasAcc, tol)
&& equal_with_abs_tol(delVdelBiasOmega, expected.delVdelBiasOmega, tol)
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
) {
// NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias
Vector3 correctedAcc = biasHat.correctAccelerometer(measuredAcc);
Vector3 correctedOmega = biasHat.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
// delPdelBiasAcc += delVdelBiasAcc * deltaT;
// delPdelBiasOmega += delVdelBiasOmega * deltaT;
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
// Update preintegrated mesurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
const Vector3 theta_i = Rot3::Logmap(deltaRij); // parametrization of so(3)
const Matrix3 Jr_theta_i = rightJacobianExpMapSO3(theta_i);
Rot3 Rot_j = deltaRij * Rincr;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
const Matrix3 Jrinv_theta_j = rightJacobianExpMapSO3inverse(theta_j);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework
Matrix H_pos_pos = I_3x3;
Matrix H_pos_vel = I_3x3 * deltaT;
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9);
F << H_pos_pos, H_pos_vel, H_pos_angles,
H_vel_pos, H_vel_vel, H_vel_angles,
H_angles_pos, H_angles_vel, H_angles_angles;
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise
PreintMeasCov = F * PreintMeasCov * F.transpose() + measurementCovariance * deltaT ;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
Vector body_t_a_body = msr_acc_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles){
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector body_t_omega_body= msr_gyro_t;
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(biasHat);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance);
ar & BOOST_SERIALIZATION_NVP(deltaPij);
ar & BOOST_SERIALIZATION_NVP(deltaVij);
ar & BOOST_SERIALIZATION_NVP(deltaRij);
ar & BOOST_SERIALIZATION_NVP(deltaTij);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega);
}
};
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
public:
/** Shorthand for a smart pointer to a factor */
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
/** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
/** Constructor */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor) {
}
virtual ~ImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const
{
const double& deltaTij = preintegratedMeasurements_.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat.gyroscope();
// we give some shorter name to rotations and translations
const Rot3 Rot_i = pose_i.rotation();
const Rot3 Rot_j = pose_j.rotation();
const Vector3 pos_i = pose_i.translation().vector();
const Vector3 pos_j = pose_j.translation().vector();
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij.retract(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
const Matrix3 Jr_theta_bcc = rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
const Matrix3 Jrinv_fRhat = rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(9,6);
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
}
if(H2) {
H2->resize(9,3);
(*H2) <<
// dfP/dVi
- Matrix3::Identity() * deltaTij
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero();
}
if(H3) {
H3->resize(9,6);
(*H3) <<
// dfP/dPosej
Matrix3::Zero(), Rot_j.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero();
}
if(H4) {
H4->resize(9,3);
(*H4) <<
// dfP/dVj
Matrix3::Zero(),
// dfV/dVj
Matrix3::Identity(),
// dfR/dVj
Matrix3::Zero();
}
if(H5) {
const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr);
const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega;
H5->resize(9,6);
(*H5) <<
// dfP/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega,
// dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega,
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
pos_j - pos_i
- Rot_i.matrix() * (preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr)
- vel_i * deltaTij
+ skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity_ * deltaTij*deltaTij;
const Vector3 fv =
vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr)
+ 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term
- gravity_ * deltaTij;
const Vector3 fR = Rot3::Logmap(fRhat);
Vector r(9); r << fp, fv, fR;
return r;
}
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected =
Rot3::Expmap( theta_biascorrected_corioliscorrected );
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
pose_j = Pose3( Rot_j, Point3(pos_j) );
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
} /// namespace gtsam

298
gtsam_unstable/slam/tests/testCombinedImuFactor.cpp Normal file
View File

@ -0,0 +1,298 @@
/* ----------------------------------------------------------------------------
* 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 testImuFactor.cpp
* @brief Unit test for ImuFactor
* @author Luca Carlone, Stephen Williams, Richard Roberts
*/
#include <gtsam_unstable/slam/ImuFactor.h>
#include <gtsam_unstable/slam/CombinedImuFactor.h>
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias)
{
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}
Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias)
{
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
)
{
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity());
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
}
return result;
}
Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij;
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij;
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij;
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
{
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
{
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}
}
/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements )
{
cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
// Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
double deltaT = 0.5;
double tol = 1e-6;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero());
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij), Vector(actual1.deltaPij), tol));
EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
}
/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases )
{
cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
double tol = 1e-6;
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
// Expected Jacobians
Matrix H1e, H2e, H3e, H4e, H5e;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
}
/* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
{
cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
}
#include <gtsam/linear/GaussianFactorGraph.h>
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

569
gtsam_unstable/slam/tests/testImuFactor.cpp Normal file
View File

@ -0,0 +1,569 @@
/* ----------------------------------------------------------------------------
* 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 testImuFactor.cpp
* @brief Unit test for ImuFactor
* @author Luca Carlone, Stephen Williams, Richard Roberts
*/
#include <gtsam_unstable/slam/ImuFactor.h>
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias)
{
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}
Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const imuBias::ConstantBias& bias)
{
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
}
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
)
{
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity());
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
}
return result;
}
Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij;
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij;
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij;
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
{
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
{
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}
}
/* ************************************************************************* */
TEST( ImuFactor, PreintegratedMeasurements )
{
// Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
double deltaT = 0.5;
// Expected preintegrated values
Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij, 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij, 1e-6);
// Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0);
double expectedDeltaT2(1);
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij, 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij, 1e-6);
}
/* ************************************************************************* */
TEST( ImuFactor, Error )
{
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
// Expected error
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
// positions and velocities
Matrix H1etop6 = H1e.topRows(6);
Matrix H1atop6 = H1a.topRows(6);
EXPECT(assert_equal(H1etop6, H1atop6));
// rotations
EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue
EXPECT(assert_equal(H2e, H2a));
// positions and velocities
Matrix H3etop6 = H3e.topRows(6);
Matrix H3atop6 = H3a.topRows(6);
EXPECT(assert_equal(H3etop6, H3atop6));
// rotations
EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue
EXPECT(assert_equal(H4e, H4a));
// EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiases )
{
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0);
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0);
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
SETDEBUG("ImuFactor evaluateError", false);
// Expected error
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H4e, H4a));
EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeExpmap )
{
// Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
double deltaT = 0.5;
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega));
}
/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeLogmap )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta; deltatheta << 0, 0, 0;
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), deltatheta);
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x);
const Matrix3 actualDelFdeltheta = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
// Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
}
/* ************************************************************************* */
TEST( ImuFactor, fistOrderExponential )
{
// Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
double deltaT = 1.0;
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot =
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// Compare Jacobians
EXPECT(assert_equal(expectedRot, actualRot));
}
/* ************************************************************************* */
TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
{
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
}
//#include <gtsam/linear/GaussianFactorGraph.h>
///* ************************************************************************* */
//TEST( ImuFactor, LinearizeTiming)
//{
// // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0);
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0);
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator
// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
// Vector3 gravity; gravity << 0, 0, 9.81;
// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
//
// // Pre-integrate Measurements
// Vector3 measuredAcc(0.1, 0.0, 0.0);
// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
// double deltaT = 0.5;
// for(size_t i = 0; i < 50; ++i) {
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// }
//
// // Create factor
// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.preintegratedMeasurementsCovariance());
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
//
// Values values;
// values.insert(X(1), x1);
// values.insert(X(2), x2);
// values.insert(V(1), v1);
// values.insert(V(2), v2);
// values.insert(B(1), bias);
//
// Ordering ordering;
// ordering.push_back(X(1));
// ordering.push_back(V(1));
// ordering.push_back(X(2));
// ordering.push_back(V(2));
// ordering.push_back(B(1));
//
// GaussianFactorGraph graph;
// gttic_(LinearizeTiming);
// for(size_t i = 0; i < 100000; ++i) {
// GaussianFactor::shared_ptr g = factor.linearize(values, ordering);
// graph.push_back(g);
// }
// gttoc_(LinearizeTiming);
// tictoc_finishedIteration_();
// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl;
// tictoc_print_();
//}
/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
{
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5));
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H4e, H4a));
EXPECT(assert_equal(H5e, H5a));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */