Merge branch 'imuFixed'
Luca
commit
beb2c4f97a
|
|
@ -63,6 +63,10 @@ typedef Eigen::Matrix<double,3,9> Matrix39;
|
|||
typedef Eigen::Block<Matrix> SubMatrix;
|
||||
typedef Eigen::Block<const Matrix> ConstSubMatrix;
|
||||
|
||||
// Two very commonly used matrices:
|
||||
const Matrix3 Z_3x3 = Matrix3::Zero();
|
||||
const Matrix3 I_3x3 = Matrix3::Identity();
|
||||
|
||||
// Matlab-like syntax
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -0,0 +1,497 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class CombinedPreintegratedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
|
||||
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
|
||||
deltaRij_(Rot3()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
|
||||
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
|
||||
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
|
||||
measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
|
||||
measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
|
||||
cout << s << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
cout << " deltaTij " << deltaTij_ << endl;
|
||||
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) 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)
|
||||
&& 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);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Z_3x3;
|
||||
delPdelBiasOmega_ = Z_3x3;
|
||||
delVdelBiasAcc_ = Z_3x3;
|
||||
delVdelBiasOmega_ = Z_3x3;
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega,
|
||||
double deltaT, boost::optional<const Pose3&> body_P_sensor) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
|
||||
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
// 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 = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
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
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::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 = Rot3::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<Vector3, Vector3>(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<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
|
||||
|
||||
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) *
|
||||
(measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) *
|
||||
(H_vel_biasacc.transpose());
|
||||
|
||||
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) *
|
||||
(measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) *
|
||||
(H_angles_biasomega.transpose());
|
||||
|
||||
G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
|
||||
|
||||
G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
|
||||
|
||||
// NEW OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose();
|
||||
G_measCov_Gt.block<3,3>(3,6) = block23;
|
||||
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
|
||||
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// CombinedImuFactor methods
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::CombinedImuFactor() :
|
||||
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
CombinedImuFactor::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,
|
||||
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
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:");
|
||||
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
|
||||
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& 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_)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) 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 = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(15,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3,
|
||||
//dBiasAcc/dPi
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasOmega/dPi
|
||||
Z_3x3, Z_3x3;
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(15,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3,
|
||||
//dBiasAcc/dVi
|
||||
Z_3x3,
|
||||
//dBiasOmega/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
H3->resize(15,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3,
|
||||
//dBiasAcc/dPosej
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasOmega/dPosej
|
||||
Z_3x3, Z_3x3;
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(15,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3,
|
||||
//dBiasAcc/dVj
|
||||
Z_3x3,
|
||||
//dBiasOmega/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::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
|
||||
-I_3x3, Z_3x3,
|
||||
//dBiasOmega/dBias_i
|
||||
Z_3x3, -I_3x3;
|
||||
}
|
||||
|
||||
if(H6) {
|
||||
H6->resize(15,6);
|
||||
(*H6) <<
|
||||
// dfP/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
// dfV/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
// dfR/dBias_j
|
||||
Z_3x3, Z_3x3,
|
||||
//dBiasAcc/dBias_j
|
||||
I_3x3, Z_3x3,
|
||||
//dBiasOmega/dBias_j
|
||||
Z_3x3, I_3x3;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
|
||||
|
||||
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
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
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;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
|
||||
return PoseVelocityBias(pose_j, vel_j, bias_i);
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -11,714 +11,291 @@
|
|||
|
||||
/**
|
||||
* @file CombinedImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Struct to hold all state variables of CombinedImuFactor
|
||||
* returned by predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
/**
|
||||
* Struct to hold all state variables of CombinedImuFactor returned by Predict function
|
||||
*/
|
||||
struct PoseVelocityBias {
|
||||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
imuBias::ConstantBias bias;
|
||||
|
||||
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
|
||||
const imuBias::ConstantBias _bias) :
|
||||
pose(_pose), velocity(_velocity), bias(_bias) {
|
||||
}
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
* 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.
|
||||
/**
|
||||
* CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias
|
||||
* at previous time step), and current state (pose, velocity, bias at current time step). According to the
|
||||
* preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are
|
||||
* "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor:
|
||||
* 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step).
|
||||
* Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices
|
||||
* "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution.
|
||||
* 2) The preintegration covariance takes into account the noise in the bias estimate used for integration.
|
||||
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
|
||||
* and the preintegrated measurements uncertainty.
|
||||
*/
|
||||
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
|
||||
public:
|
||||
|
||||
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor).
|
||||
* Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
|
||||
* from the IMU) so as to avoid costly integration at time of factor construction.
|
||||
*/
|
||||
class CombinedPreintegratedMeasurements {
|
||||
|
||||
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
|
||||
friend class CombinedImuFactor;
|
||||
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Eigen::Matrix<double,21,21> 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
|
||||
|
||||
Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
|
||||
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
|
||||
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
|
||||
///< between the preintegrated measurements and the biases
|
||||
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
|
||||
public:
|
||||
|
||||
/** Struct to store results of preintegrating IMU measurements. Can be build
|
||||
* incrementally so as to avoid costly integration at time of factor construction. */
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param measuredAccCovariance Covariance matrix of measuredAcc
|
||||
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
|
||||
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
|
||||
* @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution)
|
||||
* @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution)
|
||||
* @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements
|
||||
* @param use2ndOrderIntegration Controls the order of integration
|
||||
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
|
||||
*/
|
||||
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
|
||||
|
||||
/** 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 {
|
||||
friend class CombinedImuFactor;
|
||||
protected:
|
||||
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)
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
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
|
||||
/// equals
|
||||
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const;
|
||||
|
||||
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*)
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
// public:
|
||||
///< 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 */
|
||||
public:
|
||||
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
|
||||
const bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
///< (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)),
|
||||
use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
// 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);
|
||||
/// Re-initialize CombinedPreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
}
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time interval between two consecutive IMU measurements
|
||||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none);
|
||||
|
||||
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)),
|
||||
use2ndOrderIntegration_(false) ///< Controls the order of integration
|
||||
{
|
||||
}
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
Vector deltaPij() const {return deltaPij_;}
|
||||
Vector deltaVij() const {return deltaVij_;}
|
||||
Vector biasHat() const { return biasHat_.vector();}
|
||||
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
|
||||
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
|
||||
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
|
||||
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
|
||||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() const { return PreintMeasCov_;}
|
||||
|
||||
/** 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;
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// 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;
|
||||
}
|
||||
|
||||
/** 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);
|
||||
}
|
||||
|
||||
void resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(15,15);
|
||||
}
|
||||
|
||||
/** 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 = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
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 = Rot3::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 = Rot3::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<Vector3, Vector3>(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(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(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);
|
||||
|
||||
// NEW OFF BLOCK DIAGONAL TERMS
|
||||
Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose();
|
||||
G_measCov_Gt.block(3,6,3,3) = block23;
|
||||
G_measCov_Gt.block(6,3,3,3) = block23.transpose();
|
||||
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
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);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
Vector deltaPij() const {return deltaPij_;}
|
||||
Vector deltaVij() const {return deltaVij_;}
|
||||
Vector biasHat() const { return biasHat_.vector();}
|
||||
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
|
||||
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
|
||||
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
|
||||
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
|
||||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() const { return PreintMeasCov_;}
|
||||
|
||||
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_);
|
||||
}
|
||||
};
|
||||
// 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:
|
||||
|
||||
typedef CombinedImuFactor This;
|
||||
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,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
|
||||
|
||||
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
|
||||
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/** 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, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias_i, ///< previous bias key
|
||||
Key bias_j, ///< current bias key
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
|
||||
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
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_, tol)
|
||||
&& 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_; }
|
||||
|
||||
const Vector3& gravity() const { return gravity_; }
|
||||
|
||||
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& 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 = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
/*
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
- Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
|
||||
// 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(H1) {
|
||||
H1->resize(15,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// 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 = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::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 PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
|
||||
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
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
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;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
|
||||
return PoseVelocityBias(pose_j, vel_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_);
|
||||
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_);
|
||||
}
|
||||
}; // \class CombinedImuFactor
|
||||
};
|
||||
|
||||
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
|
||||
private:
|
||||
|
||||
typedef CombinedImuFactor This;
|
||||
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,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
|
||||
|
||||
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
|
||||
|
||||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
|
||||
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
CombinedImuFactor();
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param pose_i Previous pose key
|
||||
* @param vel_i Previous velocity key
|
||||
* @param pose_j Current pose key
|
||||
* @param vel_j Current velocity key
|
||||
* @param bias_i Previous bias key
|
||||
* @param bias_j Current bias key
|
||||
* @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements
|
||||
* @param gravity Gravity vector expressed in the global frame
|
||||
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
|
||||
* @param body_P_sensor Optional pose of the sensor frame in the body frame
|
||||
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
*/
|
||||
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,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
virtual ~CombinedImuFactor() {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
|
||||
return preintegratedMeasurements_; }
|
||||
|
||||
const Vector3& gravity() const { return gravity_; }
|
||||
|
||||
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& 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;
|
||||
|
||||
/// predicted states from IMU
|
||||
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i,
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,438 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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.cpp
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class PreintegratedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
|
||||
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
|
||||
const bool use2ndOrderIntegration) :
|
||||
biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
|
||||
deltaRij_(Rot3()), deltaTij_(0.0),
|
||||
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
|
||||
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
|
||||
delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
measurementCovariance_.setZero();
|
||||
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
|
||||
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
|
||||
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
|
||||
PreintMeasCov_.setZero(9,9);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
cout << " deltaTij " << deltaTij_ << endl;
|
||||
cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
|
||||
deltaRij_.print(" deltaRij ");
|
||||
cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
|
||||
cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) 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)
|
||||
&& 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);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Z_3x3;
|
||||
delPdelBiasOmega_ = Z_3x3;
|
||||
delVdelBiasAcc_ = Z_3x3;
|
||||
delVdelBiasOmega_ = Z_3x3;
|
||||
delRdelBiasOmega_ = Z_3x3;
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor) {
|
||||
|
||||
// NOTE: order is important here because each update uses old values (i.e., we have to update
|
||||
// jacobians and covariances before updating preintegrated measurements).
|
||||
|
||||
// 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 = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
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
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::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 = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
||||
|
||||
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<Vector3, Vector3>(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<Vector3, Vector3>(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
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
|
||||
|
||||
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
|
||||
// This in only kept for documentation.
|
||||
//
|
||||
// Matrix G(9,9);
|
||||
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
|
||||
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
|
||||
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
//
|
||||
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
|
||||
|
||||
// Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
|
||||
}
|
||||
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// ImuFactor methods
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor() :
|
||||
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ImuFactor::ImuFactor(
|
||||
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor,
|
||||
const bool use2ndOrderCoriolis) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
|
||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
cout << s << "ImuFactor("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << ")\n";
|
||||
preintegratedMeasurements_.print(" preintegrated measurements:");
|
||||
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
|
||||
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
|
||||
this->noiseModel_->print(" noise model: ");
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol)
|
||||
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
|
||||
&& 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_)));
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
|
||||
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
|
||||
boost::optional<Matrix&> H5) 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 = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Z_3x3;
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
H2->resize(9,3);
|
||||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- I_3x3 * deltaTij
|
||||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- I_3x3
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
H3->resize(9,6);
|
||||
(*H3) <<
|
||||
// dfP/dPosej
|
||||
Z_3x3, Rot_j.matrix(),
|
||||
// dfV/dPosej
|
||||
Matrix::Zero(3,6),
|
||||
// dfR/dPosej
|
||||
Jrinv_fRhat * ( I_3x3 ), Z_3x3;
|
||||
}
|
||||
|
||||
if(H4) {
|
||||
H4->resize(9,3);
|
||||
(*H4) <<
|
||||
// dfP/dVj
|
||||
Z_3x3,
|
||||
// dfV/dVj
|
||||
I_3x3,
|
||||
// dfR/dVj
|
||||
Z_3x3;
|
||||
}
|
||||
|
||||
if(H5) {
|
||||
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::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;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
|
||||
{
|
||||
|
||||
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
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
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;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocity(pose_j, vel_j);
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -11,23 +11,39 @@
|
|||
|
||||
/**
|
||||
* @file ImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
* @author Vadim Indelman
|
||||
* @author David Jensen
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#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/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
** 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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Struct to hold return variables by the Predict Function
|
||||
*/
|
||||
|
|
@ -35,265 +51,121 @@ struct PoseVelocity {
|
|||
Pose3 pose;
|
||||
Vector3 velocity;
|
||||
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
|
||||
pose(_pose), velocity(_velocity) {
|
||||
pose(_pose), velocity(_velocity) {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step),
|
||||
* current state (pose and velocity at current time step), and the bias estimate. According to the
|
||||
* preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are
|
||||
* "summarized" using the PreintegratedMeasurements class.
|
||||
* Note that this factor does not force "temporal consistency" of the biases (which are usually
|
||||
* slowly varying quantities), see also CombinedImuFactor for more details.
|
||||
*/
|
||||
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
|
||||
public:
|
||||
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
** 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.
|
||||
* PreintegratedMeasurements 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 (ImuFactor).
|
||||
* Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
|
||||
* from the IMU) so as to avoid costly integration at time of factor construction.
|
||||
*/
|
||||
class PreintegratedMeasurements {
|
||||
|
||||
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
|
||||
public:
|
||||
friend class ImuFactor;
|
||||
|
||||
/** Struct to store results of preintegrating IMU measurements. Can be build
|
||||
* incrementally so as to avoid costly integration at time of factor construction. */
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
|
||||
|
||||
/** 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 {
|
||||
friend class ImuFactor;
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
|
||||
Matrix measurementCovariance_; ///< (continuous-time 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
|
||||
|
||||
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
|
||||
|
||||
Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
|
||||
///< (first-order propagation from *measurementCovariance*).
|
||||
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
|
||||
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*)
|
||||
bool use2ndOrderIntegration_; ///< Controls the order of integration
|
||||
public:
|
||||
/** 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 measured Angular Rate
|
||||
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
|
||||
const bool use2ndOrderIntegration = false ///< Controls the order of integration
|
||||
) : 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), use2ndOrderIntegration_(use2ndOrderIntegration)
|
||||
{
|
||||
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), use2ndOrderIntegration_(false)
|
||||
{
|
||||
measurementCovariance_ = Matrix::Zero(9,9);
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
/**
|
||||
* Default constructor, initializes the class with no measurements
|
||||
* @param bias Current estimate of acceleration and rotation rate biases
|
||||
* @param measuredAccCovariance Covariance matrix of measuredAcc
|
||||
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
|
||||
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
|
||||
* @param use2ndOrderIntegration Controls the order of integration
|
||||
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
|
||||
*/
|
||||
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
|
||||
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
|
||||
const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false);
|
||||
|
||||
/** 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;
|
||||
}
|
||||
/// print
|
||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||
|
||||
/** 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);
|
||||
}
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
Vector deltaPij() const {return deltaPij_;}
|
||||
Vector deltaVij() const {return deltaVij_;}
|
||||
Vector biasHat() const { return biasHat_.vector();}
|
||||
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
|
||||
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
|
||||
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
|
||||
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
|
||||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix preintMeasCov() const { return PreintMeasCov_;}
|
||||
/// equals
|
||||
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const;
|
||||
|
||||
/// Re-initialize PreintegratedMeasurements
|
||||
void resetIntegration();
|
||||
|
||||
void resetIntegration(){
|
||||
deltaPij_ = Vector3::Zero();
|
||||
deltaVij_ = Vector3::Zero();
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delPdelBiasAcc_ = Matrix3::Zero();
|
||||
delPdelBiasOmega_ = Matrix3::Zero();
|
||||
delVdelBiasAcc_ = Matrix3::Zero();
|
||||
delVdelBiasOmega_ = Matrix3::Zero();
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(9,9);
|
||||
}
|
||||
/**
|
||||
* Add a single IMU measurement to the preintegration.
|
||||
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
|
||||
* @param measuredOmega Measured angular velocity (as given by the sensor)
|
||||
* @param deltaT Time interval between two consecutive IMU measurements
|
||||
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none);
|
||||
|
||||
/** 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
|
||||
) {
|
||||
/// methods to access class variables
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const{return deltaTij_;}
|
||||
Vector deltaPij() const {return deltaPij_;}
|
||||
Vector deltaVij() const {return deltaVij_;}
|
||||
Vector biasHat() const { return biasHat_.vector();}
|
||||
Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
|
||||
Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
|
||||
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
|
||||
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
|
||||
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
|
||||
Matrix preintMeasCov() const { return PreintMeasCov_;}
|
||||
|
||||
// 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);
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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 = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
|
||||
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
|
||||
}else{
|
||||
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
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
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 = Rot3::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 = Rot3::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<Vector3, Vector3>(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<Vector3, Vector3>(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
|
||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
||||
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
|
||||
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
|
||||
|
||||
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
|
||||
//
|
||||
// Matrix G(9,9);
|
||||
// G << I_3x3 * deltaT, Z_3x3, Z_3x3,
|
||||
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
|
||||
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
|
||||
//
|
||||
// PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
if(!use2ndOrderIntegration_){
|
||||
deltaPij_ += deltaVij_ * deltaT;
|
||||
}else{
|
||||
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);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// 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 */
|
||||
|
|
@ -336,65 +208,41 @@ struct PoseVelocity {
|
|||
#endif
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){}
|
||||
ImuFactor();
|
||||
|
||||
/** Constructor */
|
||||
ImuFactor(
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias, ///< previous bias key
|
||||
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
|
||||
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
) :
|
||||
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
|
||||
preintegratedMeasurements_(preintegratedMeasurements),
|
||||
gravity_(gravity),
|
||||
omegaCoriolis_(omegaCoriolis),
|
||||
body_P_sensor_(body_P_sensor),
|
||||
use2ndOrderCoriolis_(use2ndOrderCoriolis){
|
||||
}
|
||||
/**
|
||||
* Constructor
|
||||
* @param pose_i Previous pose key
|
||||
* @param vel_i Previous velocity key
|
||||
* @param pose_j Current pose key
|
||||
* @param vel_j Current velocity key
|
||||
* @param bias Previous bias key
|
||||
* @param preintegratedMeasurements Preintegrated IMU measurements
|
||||
* @param gravity Gravity vector expressed in the global frame
|
||||
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
|
||||
* @param body_P_sensor Optional pose of the sensor frame in the body frame
|
||||
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
|
||||
*/
|
||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis,
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
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))); }
|
||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
||||
|
||||
/** 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: ");
|
||||
}
|
||||
/// print
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** 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_, tol)
|
||||
&& 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_)));
|
||||
}
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
|
||||
|
||||
/** Access the preintegrated measurements. */
|
||||
|
||||
const PreintegratedMeasurements& preintegratedMeasurements() const {
|
||||
return preintegratedMeasurements_; }
|
||||
|
||||
|
|
@ -404,205 +252,19 @@ struct PoseVelocity {
|
|||
|
||||
/** implement functions needed to derive from Factor */
|
||||
|
||||
/** vector of errors */
|
||||
/// vector of errors
|
||||
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& 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
|
||||
{
|
||||
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 = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
|
||||
|
||||
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
|
||||
|
||||
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
|
||||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// 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 = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
|
||||
const Matrix3 Jr_JbiasOmegaIncr = Rot3::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 */
|
||||
/// predicted states from IMU
|
||||
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
|
||||
const bool use2ndOrderCoriolis = false)
|
||||
{
|
||||
|
||||
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
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
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;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
|
||||
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
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 );
|
||||
|
||||
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
|
||||
return PoseVelocity(pose_j, vel_j);
|
||||
}
|
||||
|
||||
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -617,7 +279,7 @@ struct PoseVelocity {
|
|||
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
|
||||
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
|
||||
}
|
||||
}; // \class ImuFactor
|
||||
}; // class ImuFactor
|
||||
|
||||
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
|
||||
|
||||
|
|
|
|||
|
|
@ -10,19 +10,22 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testImuFactor.cpp
|
||||
* @brief Unit test for ImuFactor
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts
|
||||
* @file testCombinedImuFactor.cpp
|
||||
* @brief Unit test for Lupton-style combined IMU factor
|
||||
* @author Luca Carlone
|
||||
* @author Stephen Williams
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
|
|
|||
|
|
@ -39,15 +39,13 @@ using symbol_shorthand::B;
|
|||
namespace {
|
||||
Vector callEvaluateError(const ImuFactor& factor,
|
||||
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
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 Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
const imuBias::ConstantBias& bias)
|
||||
{
|
||||
const imuBias::ConstantBias& bias){
|
||||
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
|
||||
}
|
||||
|
||||
|
|
@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
|
||||
)
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
|
||||
Matrix3::Identity(), Matrix3::Identity());
|
||||
|
||||
|
|
@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|||
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
return evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs).deltaPij();
|
||||
}
|
||||
|
|
@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
|
|||
const list<Vector3>& measuredAccs,
|
||||
const list<Vector3>& measuredOmegas,
|
||||
const list<double>& deltaTs,
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
|
||||
{
|
||||
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
|
||||
return Rot3(evaluatePreintegratedMeasurements(bias,
|
||||
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
|
||||
}
|
||||
|
||||
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
|
||||
{
|
||||
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)
|
||||
{
|
||||
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
|
||||
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
|
||||
}
|
||||
|
||||
|
|
@ -212,7 +202,6 @@ TEST( ImuFactor, Error )
|
|||
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);
|
||||
|
|
@ -230,7 +219,7 @@ TEST( ImuFactor, Error )
|
|||
EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
|
||||
|
||||
EXPECT(assert_equal(H4e, H4a));
|
||||
// EXPECT(assert_equal(H5e, H5a));
|
||||
// EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
// 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));
|
||||
// Vector3 v2(Vector3(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));
|
||||
Vector3 v1(Vector3(0.5, 0.0, 0.0));
|
||||
|
|
@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
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);
|
||||
// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
|
||||
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
|
||||
|
|
@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases )
|
|||
|
||||
// Expected error
|
||||
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector,Pose3>(
|
||||
|
|
@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
|
|||
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 0.5;
|
||||
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
|
||||
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
|
||||
|
|
@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
|
|||
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap )
|
|||
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));
|
||||
|
||||
|
|
@ -361,30 +344,30 @@ TEST( ImuFactor, PartialDerivativeLogmap )
|
|||
TEST( ImuFactor, fistOrderExponential )
|
||||
{
|
||||
// Linearization point
|
||||
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
|
||||
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
|
||||
|
||||
// Measurements
|
||||
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
|
||||
double deltaT = 1.0;
|
||||
// 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;
|
||||
// change w.r.t. linearization point
|
||||
double alpha = 0.0;
|
||||
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
|
||||
|
||||
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
|
||||
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
|
||||
|
||||
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
|
||||
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
|
||||
|
||||
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
|
||||
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));
|
||||
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));
|
||||
// Compare Jacobians
|
||||
EXPECT(assert_equal(expectedRot, actualRot));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
|
|||
// tictoc_print_();
|
||||
//}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
||||
{
|
||||
|
|
@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
|||
|
||||
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
|
||||
|
|
@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
|
|||
EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictPositionAndVelocity){
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
|
|
@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ImuFactor, PredictRotation) {
|
||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue