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

Merged in fix/imubias_minus_operator (pull request #120) 

fixed bug in operator - for ImuBias
release/4.3a0
Frank Dellaert 2015-03-10 22:05:26 -07:00
parent 8b63e2231d 9ae797a88c
commit e5098375e3
11 changed files with 1387 additions and 1013 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -33,19 +33,20 @@ using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit,
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, const bool use2ndOrderIntegration) :
integrationErrorCovariance, use2ndOrderIntegration), PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_(
biasAccOmegaInit_(biasAccOmegaInit) biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_(
{ biasAccOmegaInit) {
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(
const string& s) const {
PreintegrationBase::print(s); PreintegrationBase::print(s);
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
@ -54,31 +55,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) const CombinedPreintegratedMeasurements& expected, double tol) const {
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_,
&&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) tol)
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_,
tol)
&& equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol); && PreintegrationBase::equals(expected, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationBase::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
double deltaT, boost::optional<const Pose3&> body_P_sensor, boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) { boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // 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). // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
@ -86,7 +91,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Update Jacobians // Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // 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 // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
@ -98,11 +104,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = - R_i * deltaT; Matrix3 H_vel_biasacc = -R_i * deltaT;
Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15); Matrix F(15, 15);
// for documentation: // for documentation:
// F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
// Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3,
@ -110,45 +116,47 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
F.setZero(); F.setZero();
F.block<9,9>(0,0) = F_9x9; F.block<9, 9>(0, 0) = F_9x9;
F.block<6,6>(9,9) = I_6x6; F.block<6, 6>(9, 9) = I_6x6;
F.block<3,3>(3,9) = H_vel_biasacc; F.block<3, 3>(3, 9) = H_vel_biasacc;
F.block<3,3>(6,12) = H_angles_biasomega; F.block<3, 3>(6, 12) = H_angles_biasomega;
// first order uncertainty propagation // first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15,15); Matrix G_measCov_Gt = Matrix::Zero(15, 15);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance();
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
(accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0))
(H_vel_biasacc.transpose()); * (H_vel_biasacc.transpose());
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
(gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3))
(H_angles_biasomega.transpose()); * (H_angles_biasomega.transpose());
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0)
G_measCov_Gt.block<3,3>(3,6) = block23; * H_angles_biasomega.transpose();
G_measCov_Gt.block<3,3>(6,3) = block23.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; preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
// F_test and G_test are used for testing purposes and are not needed by the factor // F_test and G_test are used for testing purposes and are not needed by the factor
if(F_test){ if (F_test) {
F_test->resize(15,15); F_test->resize(15, 15);
(*F_test) << F; (*F_test) << F;
} }
if(G_test){ if (G_test) {
G_test->resize(15,21); G_test->resize(15, 21);
// This is for testing & documentation // This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, (*G_test) << //
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
} }
} }
@ -156,16 +164,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// CombinedImuFactor methods // CombinedImuFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() : CombinedImuFactor::CombinedImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_6x6) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) : 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), Base(
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), noiseModel::Gaussian::Covariance(
_PIM_(preintegratedMeasurements) {} preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis,
body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -174,13 +188,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void CombinedImuFactor::print(const string& s,
cout << s << "CombinedImuFactor(" const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key1()) << "," cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n"; << keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print(""); ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
@ -188,11 +200,11 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { bool CombinedImuFactor::equals(const NonlinearFactor& expected,
const This *e = dynamic_cast<const This*> (&expected); double tol) const {
return e != NULL && Base::equals(*e, tol) const This *e = dynamic_cast<const This*>(&expected);
&& _PIM_.equals(e->_PIM_, tol) return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol); && ImuFactorBase::equals(*e, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -220,39 +232,39 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
// if we need the jacobians // if we need the jacobians
if (H1) { if (H1) {
H1->resize(15, 6); H1->resize(15, 6);
H1->block < 9, 6 > (0, 0) = D_r_pose_i; H1->block<9, 6>(0, 0) = D_r_pose_i;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi] // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block < 6, 6 > (9, 0) = Z_6x6; H1->block<6, 6>(9, 0) = Z_6x6;
} }
if (H2) { if (H2) {
H2->resize(15, 3); H2->resize(15, 3);
H2->block < 9, 3 > (0, 0) = D_r_vel_i; H2->block<9, 3>(0, 0) = D_r_vel_i;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi] // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
} }
if (H3) { if (H3) {
H3->resize(15, 6); H3->resize(15, 6);
H3->block < 9, 6 > (0, 0) = D_r_pose_j; H3->block<9, 6>(0, 0) = D_r_pose_j;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj] // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block < 6, 6 > (9, 0) = Z_6x6; H3->block<6, 6>(9, 0) = Z_6x6;
} }
if (H4) { if (H4) {
H4->resize(15, 3); H4->resize(15, 3);
H4->block < 9, 3 > (0, 0) = D_r_vel_j; H4->block<9, 3>(0, 0) = D_r_vel_j;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi] // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
} }
if (H5) { if (H5) {
H5->resize(15, 6); H5->resize(15, 6);
H5->block < 9, 6 > (0, 0) = D_r_bias_i; H5->block<9, 6>(0, 0) = D_r_bias_i;
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
H5->block < 6, 6 > (9, 0) = Hbias_i; H5->block<6, 6>(9, 0) = Hbias_i;
} }
if (H6) { if (H6) {
H6->resize(15, 6); H6->resize(15, 6);
H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6); H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6);
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block < 6, 6 > (9, 0) = Hbias_j; H6->block<6, 6>(9, 0) = Hbias_j;
} }
// overall error // overall error

87
gtsam/navigation/CombinedImuFactor.h
View File

@ -65,7 +65,8 @@ namespace gtsam {
* the correlation between the bias uncertainty and the preintegrated * the correlation between the bias uncertainty and the preintegrated
* measurements uncertainty. * measurements uncertainty.
*/ */
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{ class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias>, public ImuFactorBase {
public: public:
/** /**
@ -82,11 +83,11 @@ public:
protected: protected:
Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements Eigen::Matrix<double, 15, 15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
///< between the preintegrated measurements and the biases ///< between the preintegrated measurements and the biases
@ -105,16 +106,20 @@ public:
* @param use2ndOrderIntegration Controls the order of integration * @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) * (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, CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration =
false);
/// print /// print
void print(const std::string& s = "Preintegrated Measurements:") const; void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals /// equals
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; bool equals(const CombinedPreintegratedMeasurements& expected, double tol =
1e-9) const;
/// Re-initialize CombinedPreintegratedMeasurements /// Re-initialize CombinedPreintegratedMeasurements
void resetIntegration(); void resetIntegration();
@ -126,12 +131,16 @@ public:
* @param deltaT Time interval between two consecutive IMU measurements * @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) * @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, void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none, boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none); boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables /// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_;} Matrix preintMeasCov() const {
return preintMeasCov_;
}
private: private:
@ -147,7 +156,8 @@ public:
private: private:
typedef CombinedImuFactor This; typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base; typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements _PIM_; CombinedPreintegratedMeasurements _PIM_;
@ -177,12 +187,15 @@ public:
* @param body_P_sensor Optional pose of the sensor frame in the body 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 * @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, CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~CombinedImuFactor() {} virtual ~CombinedImuFactor() {
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -190,54 +203,60 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals /// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** Access the preintegrated measurements. */ /** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return _PIM_; } return _PIM_;
}
/** implement functions needed to derive from Factor */ /** 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, 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, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::optional<Matrix&> H4 = boost::none, boost::none, boost::optional<Matrix&> H6 = boost::none) const;
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); PoseVelocityBias PVB(
pose_j = PVB.pose; PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
vel_j = PVB.velocity; use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6", ar
boost::serialization::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
} }
}; // class CombinedImuFactor };
// class CombinedImuFactor
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;

221
gtsam/navigation/ImuBias.h
View File

@ -36,57 +36,63 @@ namespace gtsam {
/// All bias models live in the imuBias namespace /// All bias models live in the imuBias namespace
namespace imuBias { namespace imuBias {
class ConstantBias { class ConstantBias {
private: private:
Vector3 biasAcc_; Vector3 biasAcc_;
Vector3 biasGyro_; Vector3 biasGyro_;
public: public:
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
static const size_t dimension = 6; static const size_t dimension = 6;
ConstantBias(): ConstantBias() :
biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
} }
ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro): ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
biasAcc_(biasAcc), biasGyro_(biasGyro) { biasAcc_(biasAcc), biasGyro_(biasGyro) {
} }
ConstantBias(const Vector6& v): ConstantBias(const Vector6& v) :
biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
}
/** return the accelerometer and gyro biases in a single vector */
Vector6 vector() const {
Vector6 v;
v << biasAcc_, biasGyro_;
return v;
}
/** get accelerometer bias */
const Vector3& accelerometer() const {
return biasAcc_;
}
/** get gyroscope bias */
const Vector3& gyroscope() const {
return biasGyro_;
}
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
} }
return measurement - biasAcc_;
}
/** return the accelerometer and gyro biases in a single vector */ /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector6 vector() const { Vector3 correctGyroscope(const Vector3& measurement,
Vector6 v; boost::optional<Matrix&> H = boost::none) const {
v << biasAcc_, biasGyro_; if (H) {
return v; H->resize(3, 6);
} (*H) << Matrix3::Zero(), -Matrix3::Identity();
/** get accelerometer bias */
const Vector3& accelerometer() const { return biasAcc_; }
/** get gyroscope bias */
const Vector3& gyroscope() const { return biasGyro_; }
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
}
return measurement - biasAcc_;
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << Matrix3::Zero(), -Matrix3::Identity();
}
return measurement - biasGyro_;
} }
return measurement - biasGyro_;
}
// // H1: Jacobian w.r.t. IMUBias // // H1: Jacobian w.r.t. IMUBias
// // H2: Jacobian w.r.t. pose // // H2: Jacobian w.r.t. pose
@ -118,77 +124,93 @@ namespace imuBias {
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
// } // }
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/// print with optional string /// print with optional string
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
// explicit printing for now. // explicit printing for now.
std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl;
std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl;
} }
/** equality up to tolerance */ /** equality up to tolerance */
inline bool equals(const ConstantBias& expected, double tol=1e-5) const { inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
&& equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
} }
/// @} /// @}
/// @name Group /// @name Group
/// @{ /// @{
/** identity for group operation */ /** identity for group operation */
static ConstantBias identity() { return ConstantBias(); } static ConstantBias identity() {
return ConstantBias();
}
/** inverse */ /** inverse */
inline ConstantBias operator-() const { inline ConstantBias operator-() const {
return ConstantBias(-biasAcc_, -biasGyro_); return ConstantBias(-biasAcc_, -biasGyro_);
} }
/** addition */ /** addition */
ConstantBias operator+(const ConstantBias& b) const { ConstantBias operator+(const ConstantBias& b) const {
return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
} }
/** subtraction */ /** subtraction */
ConstantBias operator-(const ConstantBias& b) const { ConstantBias operator-(const ConstantBias& b) const {
return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
} }
/// @} /// @}
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
ConstantBias inverse() { return -(*this);} ConstantBias inverse() {
ConstantBias compose(const ConstantBias& q) { return (*this)+q;} return -(*this);
ConstantBias between(const ConstantBias& q) { return q-(*this);} }
Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();} ConstantBias compose(const ConstantBias& q) {
ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));} return (*this) + q;
static Vector6 Logmap(const ConstantBias& p) {return p.vector();} }
static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);} ConstantBias between(const ConstantBias& q) {
/// @} return q - (*this);
}
Vector6 localCoordinates(const ConstantBias& q) {
return between(q).vector();
}
ConstantBias retract(const Vector6& v) {
return compose(ConstantBias(v));
}
static Vector6 Logmap(const ConstantBias& p) {
return p.vector();
}
static ConstantBias Expmap(const Vector6& v) {
return ConstantBias(v);
}
/// @}
private: private:
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version) {
{ ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_); }
}
/// @} /// @}
}; // ConstantBias class };
} // namespace imuBias // ConstantBias class
}// namespace imuBias
template<> template<>
struct traits<imuBias::ConstantBias> : public internal::VectorSpace< struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
@ -197,4 +219,3 @@ struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
} // namespace gtsam } // namespace gtsam

91
gtsam/navigation/ImuFactor.cpp
View File

@ -33,12 +33,11 @@ using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration) : const bool use2ndOrderIntegration) :
PreintegrationBase(bias, PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
measuredAccCovariance, measuredOmegaCovariance, integrationErrorCovariance, use2ndOrderIntegration) {
integrationErrorCovariance, use2ndOrderIntegration)
{
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -49,13 +48,14 @@ void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { bool ImuFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol); && PreintegrationBase::equals(expected, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration(){ void ImuFactor::PreintegratedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration(); PreintegrationBase::resetIntegration();
preintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
@ -63,18 +63,20 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement( void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor, boost::optional<const Pose3&> body_P_sensor, OptionalJacobian<9, 9> F_test,
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians // Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements (also get Jacobian) // Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
@ -90,31 +92,34 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
// as G and measurementCovariance are blockdiagonal matrices // as G and measurementCovariance are blockdiagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose(); preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance()
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; * R_i.transpose() * deltaT;
preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
* gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc; Matrix3 F_pos_noiseacc;
if(use2ndOrderIntegration()){ if (use2ndOrderIntegration()) {
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose(); preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
* accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
preintMeasCov_.block<3,3>(0,3) += temp; preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3,3>(3,0) += temp.transpose(); preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
} }
// F_test and G_test are given as output for testing purposes and are not needed by the factor // F_test and G_test are given as output for testing purposes and are not needed by the factor
if(F_test){ // This in only for testing if (F_test) { // This in only for testing
(*F_test) << F; (*F_test) << F;
} }
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
if(!use2ndOrderIntegration()) if (!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3; F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
} }
} }
@ -122,19 +127,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// ImuFactor methods // ImuFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::ImuFactor() : ImuFactor::ImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
ImuFactor::ImuFactor( ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
const bool use2ndOrderCoriolis) : Base(
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), noiseModel::Gaussian::Covariance(
pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor,
_PIM_(preintegratedMeasurements) {} use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -144,12 +150,10 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor(" cout << s << "ImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< keyFormatter(this->key3()) << "," << ")\n";
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
ImuFactorBase::print(""); ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
@ -157,10 +161,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& _PIM_.equals(e->_PIM_, tol) && ImuFactorBase::equals(*e, tol);
&& ImuFactorBase::equals(*e, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

82
gtsam/navigation/ImuFactor.h
View File

@ -58,7 +58,8 @@ namespace gtsam {
* (which are usually slowly varying quantities), which is up to the caller. * (which are usually slowly varying quantities), which is up to the caller.
* See also CombinedImuFactor for a class that does this for you. * See also CombinedImuFactor for a class that does this for you.
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase { class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias>, public ImuFactorBase {
public: public:
/** /**
@ -75,7 +76,7 @@ public:
protected: protected:
Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*). ///< (first-order propagation from *measurementCovariance*).
public: public:
@ -90,14 +91,17 @@ public:
* (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) * (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, PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& measuredAccCovariance,
const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false);
/// print /// print
void print(const std::string& s = "Preintegrated Measurements:") const; void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals /// equals
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(); void resetIntegration();
@ -110,12 +114,16 @@ public:
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing) * @param Fout, Gout Jacobians used internally (only needed for testing)
*/ */
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none, boost::optional<const Pose3&> body_P_sensor = boost::none,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout =
boost::none);
/// methods to access class variables /// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_;} Matrix preintMeasCov() const {
return preintMeasCov_;
}
private: private:
@ -128,14 +136,15 @@ public:
} }
}; };
private: private:
typedef ImuFactor This; typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base; typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_; PreintegratedMeasurements _PIM_;
public: public:
/** Shorthand for a smart pointer to a factor */ /** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
@ -163,9 +172,11 @@ public:
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~ImuFactor() {} virtual ~ImuFactor() {
}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const; virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -173,52 +184,59 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/// print /// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals /// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** Access the preintegrated measurements. */ /** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const { const PreintegratedMeasurements& preintegratedMeasurements() const {
return _PIM_; } return _PIM_;
}
/** implement functions needed to derive from Factor */ /** 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, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const Pose3& pose_j, const Vector3& vel_j,
boost::optional<Matrix&> H1 = boost::none, const imuBias::ConstantBias& bias, boost::optional<Matrix&> H1 =
boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::optional<Matrix&> H4 = boost::none, boost::none, boost::optional<Matrix&> H5 = boost::none) const;
boost::optional<Matrix&> H5 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const PreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); PoseVelocityBias PVB(
pose_j = PVB.pose; PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
vel_j = PVB.velocity; use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5", ar
boost::serialization::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
} }
}; // class ImuFactor };
// class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;

40
gtsam/navigation/ImuFactorBase.h
View File

@ -33,15 +33,16 @@ protected:
Vector3 gravity_; Vector3 gravity_;
Vector3 omegaCoriolis_; Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame 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 bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public: public:
/** Default constructor - only use for serialization */ /** Default constructor - only use for serialization */
ImuFactorBase() : ImuFactorBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} boost::none), use2ndOrderCoriolis_(false) {
}
/** /**
* Default constructor, stores basic quantities required by the Imu factors * Default constructor, stores basic quantities required by the Imu factors
@ -51,21 +52,29 @@ public:
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/ */
ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : boost::optional<const Pose3&> body_P_sensor = boost::none,
gravity_(gravity), omegaCoriolis_(omegaCoriolis), const bool use2ndOrderCoriolis = false) :
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
/// Methods to access class variables /// Methods to access class variables
const Vector3& gravity() const { return gravity_; } const Vector3& gravity() const {
const Vector3& omegaCoriolis() const { return omegaCoriolis_; } return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/// Needed for testable /// Needed for testable
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; << std::endl;
if(this->body_P_sensor_) std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]"
<< std::endl;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: "); this->body_P_sensor_->print(" sensor pose in body frame: ");
} }
@ -73,10 +82,11 @@ public:
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool equals(const ImuFactorBase& expected, double tol) const { bool equals(const ImuFactorBase& expected, double tol) const {
return equal_with_abs_tol(gravity_, expected.gravity_, tol) return equal_with_abs_tol(gravity_, expected.gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
&& (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
&& ((!body_P_sensor_ && !expected.body_P_sensor_) || && ((!body_P_sensor_ && !expected.body_P_sensor_)
(body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); || (body_P_sensor_ && expected.body_P_sensor_
&& body_P_sensor_->equals(*expected.body_P_sensor_)));
} }
}; };

60
gtsam/navigation/PreintegratedRotation.h
View File

@ -32,12 +32,12 @@ namespace gtsam {
*/ */
class PreintegratedRotation { class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
/// Jacobian of preintegrated rotation w.r.t. angular rate bias /// Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_; Matrix3 delRdelBiasOmega_;
Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
public: public:
@ -45,35 +45,49 @@ public:
* Default constructor, initializes the variables in the base class * Default constructor, initializes the variables in the base class
*/ */
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
deltaRij_(Rot3()), deltaTij_(0.0), deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} measuredOmegaCovariance) {
}
/// methods to access class variables /// methods to access class variables
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive Matrix3 deltaRij() const {
Vector3 thetaRij(OptionalJacobian<3,3> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive return deltaRij_.matrix();
const double& deltaTij() const{return deltaTij_;} } // expensive
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} return Rot3::Logmap(deltaRij_, H);
} // super-expensive
const double& deltaTij() const {
return deltaTij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& gyroscopeCovariance() const {
return gyroscopeCovariance_;
}
/// Needed for testable /// Needed for testable
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << s << std::endl; std::cout << s << std::endl;
std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl;
std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]"
<< std::endl;
} }
/// Needed for testable /// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const { bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol) return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol && fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
&& equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); tol)
&& equal_with_abs_tol(gyroscopeCovariance_,
expected.gyroscopeCovariance_, tol);
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(){ void resetIntegration() {
deltaRij_ = Rot3(); deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3; delRdelBiasOmega_ = Z_3x3;
@ -81,7 +95,7 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
OptionalJacobian<3, 3> H = boost::none){ OptionalJacobian<3, 3> H = boost::none) {
deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT; deltaTij_ += deltaT;
} }
@ -90,15 +104,16 @@ public:
* Update Jacobians to be used during preintegration * Update Jacobians to be used during preintegration
* TODO: explain arguments * TODO: explain arguments
*/ */
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
double deltaT) { const Rot3& incrR, double deltaT) {
const Matrix3 incrRt = incrR.transpose(); const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_Rincr_integratedOmega * deltaT;
} }
/// Return a bias corrected version of the integrated rotation - expensive /// Return a bias corrected version of the integrated rotation - expensive
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
} }
/// Get so<3> version of bias corrected rotation, with optional Jacobian /// Get so<3> version of bias corrected rotation, with optional Jacobian
@ -111,7 +126,8 @@ public:
if (H) { if (H) {
Matrix3 Jrinv_theta_bc; Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3> // This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = // const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;

359
gtsam/navigation/PreintegrationBase.h
View File

@ -36,7 +36,7 @@ struct PoseVelocityBias {
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) : const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) { pose(_pose), velocity(_velocity), bias(_bias) {
} }
}; };
@ -46,7 +46,7 @@ struct PoseVelocityBias {
* It includes the definitions of the preintegrated variables and the methods * It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them. * to access, print, and compare them.
*/ */
class PreintegrationBase : public PreintegratedRotation { class PreintegrationBase: public PreintegratedRotation {
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration bool use2ndOrderIntegration_; ///< Controls the order of integration
@ -54,13 +54,13 @@ class PreintegrationBase : public PreintegratedRotation {
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) 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) Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration 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 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration) /// (to compensate errors in Euler integration)
public: public:
@ -72,56 +72,84 @@ public:
* (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) * (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)
*/ */
PreintegrationBase(const imuBias::ConstantBias& bias, PreintegrationBase(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& measuredAccCovariance,
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : const Matrix3& measuredOmegaCovariance,
PreintegratedRotation(measuredOmegaCovariance), const Matrix3&integrationErrorCovariance,
biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), const bool use2ndOrderIntegration) :
deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias), use2ndOrderIntegration_(
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), Vector3::Zero()), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delVdelBiasAcc_(
accelerometerCovariance_(measuredAccCovariance), Z_3x3), delVdelBiasOmega_(Z_3x3), accelerometerCovariance_(
integrationCovariance_(integrationErrorCovariance) {} measuredAccCovariance), integrationCovariance_(
integrationErrorCovariance) {
}
/// methods to access class variables /// methods to access class variables
bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} bool use2ndOrderIntegration() const {
const Vector3& deltaPij() const {return deltaPij_;} return use2ndOrderIntegration_;
const Vector3& deltaVij() const {return deltaVij_;} }
const imuBias::ConstantBias& biasHat() const { return biasHat_;} const Vector3& deltaPij() const {
Vector6 biasHatVector() const { return biasHat_.vector();} // expensive return deltaPij_;
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} }
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} const Vector3& deltaVij() const {
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} return deltaVij_;
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} }
const imuBias::ConstantBias& biasHat() const {
return biasHat_;
}
Vector6 biasHatVector() const {
return biasHat_.vector();
} // expensive
const Matrix3& delPdelBiasAcc() const {
return delPdelBiasAcc_;
}
const Matrix3& delPdelBiasOmega() const {
return delPdelBiasOmega_;
}
const Matrix3& delVdelBiasAcc() const {
return delVdelBiasAcc_;
}
const Matrix3& delVdelBiasOmega() const {
return delVdelBiasOmega_;
}
const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} const Matrix3& accelerometerCovariance() const {
const Matrix3& integrationCovariance() const { return integrationCovariance_;} return accelerometerCovariance_;
}
const Matrix3& integrationCovariance() const {
return integrationCovariance_;
}
/// Needed for testable /// Needed for testable
void print(const std::string& s) const { void print(const std::string& s) const {
PreintegratedRotation::print(s); PreintegratedRotation::print(s);
std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; std::cout << " accelerometerCovariance [ " << accelerometerCovariance_
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; << " ]" << std::endl;
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]"
<< std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
biasHat_.print(" biasHat"); biasHat_.print(" biasHat");
} }
/// Needed for testable /// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const { bool equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(expected, tol) return PreintegratedRotation::equals(other, tol)
&& biasHat_.equals(expected.biasHat_, tol) && biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) && equal_with_abs_tol(accelerometerCovariance_,
&& equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); other.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_,
other.integrationCovariance_, tol);
} }
/// Re-initialize PreintegratedMeasurements /// Re-initialize PreintegratedMeasurements
void resetIntegration(){ void resetIntegration() {
PreintegratedRotation::resetIntegration(); PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero(); deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero();
@ -133,51 +161,56 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) {
Matrix3 dRij = deltaRij(); // expensive Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){ if (!use2ndOrderIntegration_) {
deltaPij_ += deltaVij_ * deltaT; deltaPij_ += deltaVij_ * deltaT;
}else{ } else {
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
} }
deltaVij_ += temp; deltaVij_ += temp;
Matrix3 R_i, F_angles_angles; Matrix3 R_i, F_angles_angles;
if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij if (F)
updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if(F){ if (F) {
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles; Matrix3 F_pos_angles;
if(use2ndOrderIntegration_) if (use2ndOrderIntegration_)
F_pos_angles = 0.5 * F_vel_angles * deltaT; F_pos_angles = 0.5 * F_vel_angles * deltaT;
else else
F_pos_angles = Z_3x3; F_pos_angles = Z_3x3;
// pos vel angle // pos vel angle
*F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos *F << //
Z_3x3, I_3x3, F_vel_angles, // vel I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, Z_3x3, F_angles_angles;// angle Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
} }
} }
/// Update Jacobians to be used during preintegration /// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc, void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT) {
Matrix3 dRij = deltaRij(); // expensive Matrix3 dRij = deltaRij(); // expensive
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega();
if (!use2ndOrderIntegration_) { if (!use2ndOrderIntegration_) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else { } else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT
delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
} }
delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp; delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
} }
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
@ -188,11 +221,13 @@ public:
// Then compensate for sensor-body displacement: we express the quantities // Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
if(body_P_sensor){ if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); 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(); 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 // linear acceleration vector in the body frame
} }
} }
@ -205,47 +240,51 @@ public:
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const { boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); const imuBias::ConstantBias biasIncr = bias_i - biasHat();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); const Vector3& biasAccIncr = biasIncr.accelerometer();
const Vector3& biasOmegaIncr = biasIncr.gyroscope();
const Rot3& Rot_i = pose_i.rotation(); const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
if(deltaPij_biascorrected_out)// if desired, store this + delPdelBiasOmega() * biasOmegaIncr;
if (deltaPij_biascorrected_out) // if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected; *deltaPij_biascorrected_out = deltaPij_biascorrected;
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
+ vel_i * deltaTij() + vel_i * deltaTij()
- omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - omegaCoriolis.cross(vel_i) * deltaTij() * deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij(); + 0.5 * gravity * deltaTij() * deltaTij();
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
if(deltaVij_biascorrected_out)// if desired, store this + delVdelBiasOmega() * biasOmegaIncr;
if (deltaVij_biascorrected_out) // if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected; *deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected Vector3 vel_j = Vector3(
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term vel_i + Rot_i.matrix() * deltaVij_biascorrected
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * deltaTij()); + gravity * deltaTij());
if(use2ndOrderCoriolis){ if (use2ndOrderCoriolis) {
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i))
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity * deltaTij() * deltaTij(); // 2nd order coriolis term for position
vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega - Vector3 correctedOmega = biascorrectedOmega
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij = const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
Rot3::Expmap( correctedOmega ); const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
} }
@ -255,14 +294,12 @@ public:
const Pose3& pose_j, const Vector3& vel_j, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const Vector3& gravity, const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
OptionalJacobian<9, 6> H1 = boost::none, OptionalJacobian<9, 6> H1 = boost::none, OptionalJacobian<9, 3> H2 =
OptionalJacobian<9, 3> H2 = boost::none, boost::none, OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 =
OptionalJacobian<9, 3> H4 = boost::none, boost::none) const {
OptionalJacobian<9, 6> H5 = boost::none) const {
// We need the mismatch w.r.t. the biases used for preintegration // We need the mismatch w.r.t. the biases used for preintegration
// const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
// we give some shorter name to rotations and translations // we give some shorter name to rotations and translations
@ -274,117 +311,124 @@ public:
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected; Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected,
deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); const Vector3 fp = Ri.transpose()
* (pos_j - predictedState_j.pose.translation().vector());
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity);
// fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
// Jacobian computation
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation // Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5 // If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect // H5 will then be corrected below to take into account the Coriolis effect
Matrix3 D_cThetaRij_biasOmegaIncr; Matrix3 D_cThetaRij_biasOmegaIncr;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 correctedOmega = biascorrectedOmega - coriolis; Vector3 correctedOmega = biascorrectedOmega - coriolis;
Rot3 correctedDeltaRij, fRrot; // Calculate Jacobians, matrices below needed only for some Jacobians...
Vector3 fR; Vector3 fR;
Rot3 correctedDeltaRij, fRrot;
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot,
Ritranspose_omegaCoriolisHat;
// Accessory matrix, used to build the jacobians if (H1 || H2)
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; Ritranspose_omegaCoriolisHat = Ri.transpose()
* skewSymmetric(omegaCoriolis);
// This is done to save computation: we ask for the jacobians only when they are needed // This is done to save computation: we ask for the jacobians only when they are needed
if(H1 || H2 || H3 || H4 || H5){ if (H1 || H2 || H3 || H4 || H5) {
correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
// Residual rotation error // Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj)); fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot, D_fR_fRrot); fR = Rot3::Logmap(fRrot, D_fR_fRrot);
D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
}else{ } else {
correctedDeltaRij = Rot3::Expmap( correctedOmega); correctedDeltaRij = Rot3::Expmap(correctedOmega);
// Residual rotation error // Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj)); fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot); fR = Rot3::Logmap(fRrot);
} }
if(H1) { if (H1) {
H1->resize(9,6); H1->resize(9, 6);
Matrix3 dfPdPi = -I_3x3; Matrix3 dfPdPi = -I_3x3;
Matrix3 dfVdPi = Z_3x3; Matrix3 dfVdPi = Z_3x3;
if(use2ndOrderCoriolis){ if (use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); Matrix3 temp = Ritranspose_omegaCoriolisHat
dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); * (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * deltaTij() * deltaTij();
dfVdPi += temp * deltaTij(); dfVdPi += temp * deltaTij();
} }
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
skewSymmetric(fp + deltaPij_biascorrected), skewSymmetric(fp + deltaPij_biascorrected),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
skewSymmetric(fv + deltaVij_biascorrected), skewSymmetric(fv + deltaVij_biascorrected),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), D_fR_fRrot
// dfR/dPi * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
Z_3x3; // dfR/dPi
Z_3x3;
} }
if(H2) { if (H2) {
H2->resize(9,3); H2->resize(9, 3);
(*H2) << (*H2) <<
// dfP/dVi // dfP/dVi
- Ri.transpose() * deltaTij() -Ri.transpose() * deltaTij()
+ Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi // dfV/dVi
- Ri.transpose() -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
+ 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
// dfR/dVi // dfR/dVi
Z_3x3; Z_3x3;
} }
if(H3) { if (H3) {
H3->resize(9,6); H3->resize(9, 6);
(*H3) << (*H3) <<
// dfP/dPosej // dfP/dPosej
Z_3x3, Ri.transpose() * Rj.matrix(), Z_3x3, Ri.transpose() * Rj.matrix(),
// dfV/dPosej // dfV/dPosej
Matrix::Zero(3,6), Matrix::Zero(3, 6),
// dfR/dPosej // dfR/dPosej
D_fR_fRrot, Z_3x3; D_fR_fRrot, Z_3x3;
} }
if(H4) { if (H4) {
H4->resize(9,3); H4->resize(9, 3);
(*H4) << (*H4) <<
// dfP/dVj // dfP/dVj
Z_3x3, Z_3x3,
// dfV/dVj // dfV/dVj
Ri.transpose(), Ri.transpose(),
// dfR/dVj // dfR/dVj
Z_3x3; Z_3x3;
} }
if(H5) { if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
H5->resize(9,6); H5->resize(9, 6);
(*H5) << (*H5) <<
// dfP/dBias // dfP/dBias
- delPdelBiasAcc(), - delPdelBiasOmega(), -delPdelBiasAcc(), -delPdelBiasOmega(),
// dfV/dBias // dfV/dBias
- delVdelBiasAcc(), - delVdelBiasOmega(), -delVdelBiasAcc(), -delVdelBiasOmega(),
// dfR/dBias // dfR/dBias
Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
} }
Vector9 r; r << fp, fv, fR; Vector9 r;
r << fp, fv, fR;
return r; return r;
} }
@ -410,22 +454,29 @@ protected:
Vector3 gravity_; Vector3 gravity_;
Vector3 omegaCoriolis_; Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame 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 bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public: public:
ImuBase() : ImuBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} boost::none), use2ndOrderCoriolis_(false) {
}
ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : boost::optional<const Pose3&> body_P_sensor = boost::none,
gravity_(gravity), omegaCoriolis_(omegaCoriolis), const bool use2ndOrderCoriolis = false) :
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
const Vector3& gravity() const { return gravity_; } const Vector3& gravity() const {
const Vector3& omegaCoriolis() const { return omegaCoriolis_; } return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
}; };

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

@ -44,36 +44,39 @@ namespace {
// Auxiliary functions to test Jacobians F and G used for // Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration // covariance propagation during preintegration
/* ************************************************************************* */ /* ************************************************************************* */
Vector updatePreintegratedMeasurementsTest( Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) { const bool use2ndOrderIntegration) {
Matrix3 dRij = deltaRij_old.matrix(); Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new; Vector3 deltaPij_new;
if(!use2ndOrderIntegration){ if (!use2ndOrderIntegration) {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{ } else {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
} }
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); Rot3 deltaRij_new = deltaRij_old
* Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old); imuBias::ConstantBias bias_new(bias_old);
Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); Vector result(15);
result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
return result; return result;
} }
Rot3 updatePreintegratedMeasurementsRot( Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration){ const bool use2ndOrderIntegration) {
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
deltaT, use2ndOrderIntegration);
return Rot3::Expmap(result.segment<3>(6)); return Rot3::Expmap(result.segment<3>(6));
} }
@ -82,60 +85,53 @@ Rot3 updatePreintegratedMeasurementsRot(
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */ /* ************************************************************************* */
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<Vector3>& measuredOmegas, CombinedImuFactor::CombinedPreintegratedMeasurements result(bias,
const list<double>& deltaTs){ Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false);
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false);
list<Vector3>::const_iterator itAcc = measuredAccs.begin(); list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin(); list<double>::const_iterator itDeltaT = deltaTs.begin();
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
} }
return result; return result;
} }
Vector3 evaluatePreintegratedMeasurementsPosition( Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<Vector3>& measuredOmegas, return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
const list<double>& deltaTs){ deltaTs).deltaPij();
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij();
} }
Vector3 evaluatePreintegratedMeasurementsVelocity( Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<Vector3>& measuredOmegas, return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
const list<double>& deltaTs){ deltaTs).deltaVij();
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij();
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredAccs, const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
const list<Vector3>& measuredOmegas, return Rot3(
const list<double>& deltaTs){ evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
return Rot3(evaluatePreintegratedMeasurements(bias, deltaTs).deltaRij());
measuredAccs, measuredOmegas, deltaTs).deltaRij());
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements ) TEST( CombinedImuFactor, PreintegratedMeasurements ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
// Measurements // Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5; double deltaT = 0.5;
double tol = 1e-6; double tol = 1e-6;
@ -145,58 +141,75 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); Matrix3::Zero(), Matrix::Zero(6, 6));
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); EXPECT(
EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()),
EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); tol));
EXPECT(
assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()),
tol));
EXPECT(
assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()),
tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases ) TEST( CombinedImuFactor, ErrorWithBiases ) {
{
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1(0.5, 0.0, 0.0); Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(0.5, 0.0, 0.0); Vector3 v2(0.5, 0.0, 0.0);
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; Vector3 omegaCoriolis;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0; double deltaT = 1.0;
double tol = 1e-6; double tol = 1e-6;
Matrix I6x6(6,6); Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6,6); I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6);
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity,
omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); noiseModel::Gaussian::shared_ptr Combinedmodel =
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias,
bias2);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
@ -206,7 +219,8 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// Actual Jacobians // Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a; Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9)));
@ -216,101 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
// Measurements // Measurements
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs; list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
for(int i=1;i<100;i++) for (int i = 1; i < 100; i++) {
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
// Actual preintegrated values // Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); imuBias::ConstantBias>(
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); imuBias::ConstantBias>(
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>( Matrix expectedDelRdelBias =
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); numericalDerivative11<Rot3, imuBias::ConstantBias>(
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); EXPECT(
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity){ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; Vector3 omegaCoriolis;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 1.1, -9.81;
double deltaT = 0.001; double deltaT = 0.001;
Matrix I6x6(6,6); Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6,6); I6x6 = Matrix::Identity(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); for (int i = 0; i < 1000; ++i)
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
// Create factor // Create factor
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); noiseModel::Gaussian::shared_ptr Combinedmodel =
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x1; Pose3 x1;
Vector3 v1(0, 0.0, 0.0); Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1,
bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0; Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); EXPECT(
assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) { TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Matrix I6x6(6,6); Matrix I6x6(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
Vector3 gravity; gravity<<0,0,9.81; Vector3 measuredAcc;
Vector3 omegaCoriolis; omegaCoriolis << 0,0,0; measuredAcc << 0, 0, -9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001; double deltaT = 0.001;
double tol = 1e-4; double tol = 1e-4;
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)
@ -320,16 +361,16 @@ TEST(CombinedImuFactor, PredictRotation) {
Combined_pre_int_data, gravity, omegaCoriolis); Combined_pre_int_data, gravity, omegaCoriolis);
// Predict // Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2; Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0,0,0), v2; Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); CombinedImuFactor::Predict(x, v, x2, v2, bias,
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol)); EXPECT(assert_equal(expectedPose, x2, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
{
// Linearization point // Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); Pose3 body_P_sensor = Pose3();
@ -338,35 +379,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
list<Vector3> measuredAccs, measuredOmegas; list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs; list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
for(int i=1;i<100;i++) for (int i = 1; i < 100; i++) {
{
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01); deltaTs.push_back(0.01);
} }
// Actual preintegrated values // Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas,
deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01; const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov(); Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
body_P_sensor, Factual, Gactual); newDeltaT, body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;
@ -374,44 +416,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
// COMPUTE NUMERICAL DERIVATIVES FOR F // COMPUTE NUMERICAL DERIVATIVES FOR F
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected F wrt positions (15,3) // Compute expected F wrt positions (15,3)
Matrix df_dpos = Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
_1, deltaVij_old, deltaRij_old, bias_old, deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); use2ndOrderIntegration), deltaPij_old);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_dpos.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
_1, deltaVij_old, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
// Compute expected F wrt velocities (15,3) // Compute expected F wrt velocities (15,3)
Matrix df_dvel = Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
deltaPij_old, _1, deltaRij_old, bias_old, deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); use2ndOrderIntegration), deltaVij_old);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_dvel.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
deltaPij_old, _1, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
// Compute expected F wrt angles (15,3) // Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedMeasurementsTest, Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
deltaPij_old, deltaVij_old, _1, bias_old, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_dangle.block<3,3>(6,0) = numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedMeasurementsRot, df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
deltaPij_old, deltaVij_old, _1, bias_old, boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// Compute expected F wrt biases (15,6) // Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest, Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
deltaPij_old, deltaVij_old, deltaRij_old, _1, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_dbias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot, df_dbias.block<3, 6>(6, 0) =
deltaPij_old, deltaVij_old, deltaRij_old, _1, numericalDerivative11<Rot3, imuBias::ConstantBias>(
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
Matrix Fexpected(15,15); Matrix Fexpected(15, 15);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual)); EXPECT(assert_equal(Fexpected, Factual));
@ -419,55 +468,65 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
// COMPUTE NUMERICAL DERIVATIVES FOR G // COMPUTE NUMERICAL DERIVATIVES FOR G
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected G wrt integration noise // Compute expected G wrt integration noise
Matrix df_dintNoise(15,3); Matrix df_dintNoise(15, 3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected G wrt acc noise (15,3) // Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
deltaPij_old, deltaVij_old, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_daccNoise.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
deltaPij_old, deltaVij_old, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3) // Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
deltaPij_old, deltaVij_old, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
deltaPij_old, deltaVij_old, deltaRij_old, bias_old, boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6) // Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero(); df_rwBias.setZero();
df_rwBias.block<6,6>(9,0) = eye(6); df_rwBias.block<6, 6>(9, 0) = eye(6);
// Compute expected G wrt gyro noise (15,6) // Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest, Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
deltaPij_old, deltaVij_old, deltaRij_old, _1, boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold // rotation part has to be done properly, on manifold
df_dinitBias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot, df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
deltaPij_old, deltaVij_old, deltaRij_old, _1, imuBias::ConstantBias>(
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows
Matrix Gexpected(15,21); Matrix Gexpected(15, 21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual)); EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation // Check covariance propagation
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
(1/newDeltaT) * Gactual * Gactual.transpose(); * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

106
gtsam/navigation/tests/testImuBias.cpp
View File

@ -21,23 +21,111 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
Vector biasAcc1(Vector3(0.2, -0.1, 0));
Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ImuBias, Constructor) TEST( ImuBias, Constructor) {
{
Vector bias_acc(Vector3(0.1,0.2,0.4));
Vector bias_gyro(Vector3(-0.2, 0.5, 0.03));
// Default Constructor // Default Constructor
gtsam::imuBias::ConstantBias bias1; imuBias::ConstantBias bias1;
// Acc + Gyro Constructor // Acc + Gyro Constructor
gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro); imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
// Copy Constructor // Copy Constructor
gtsam::imuBias::ConstantBias bias3(bias2); imuBias::ConstantBias bias3(bias2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} TEST( ImuBias, inverse) {
imuBias::ConstantBias biasActual = bias1.inverse();
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1,
-biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, compose) {
imuBias::ConstantBias biasActual = bias2.compose(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, between) {
// p.between(q) == q - p
imuBias::ConstantBias biasActual = bias2.between(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2,
biasGyro1 - biasGyro2)).vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, retract) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.retract(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0));
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, Expmap) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.Expmap(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSub) {
imuBias::ConstantBias biasActual = -bias1;
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorAdd) {
imuBias::ConstantBias biasActual = bias2 + bias1;
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1,
biasGyro2 + biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSubB) {
imuBias::ConstantBias biasActual = bias2 - bias1;
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1,
biasGyro2 - biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

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

File diff suppressed because it is too large Load Diff