diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 45f067fcf..3cb4c8588 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, @@ -168,7 +168,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ @@ -186,12 +186,9 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; + ImuFactorBase::print(""); preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ @@ -199,9 +196,7 @@ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) cons const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -393,7 +388,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ //------------------------------------------------------------------------------ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ const double& deltaTij = preintegratedMeasurements.deltaTij_; @@ -433,8 +428,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant } } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index fb481c8d6..23e1a0c5d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include namespace gtsam { @@ -70,7 +71,7 @@ struct PoseVelocityBias { * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty * and the preintegrated measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuBase{ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) @@ -217,7 +218,7 @@ public: /// predicted states from IMU static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9f7021c3a..824b850e8 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -41,7 +41,7 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - preintMeasCov_.setZero(9,9); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -150,7 +150,7 @@ ImuFactor::ImuFactor( boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), preintegratedMeasurements_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ @@ -167,12 +167,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; + ImuFactorBase::print(""); preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ @@ -180,9 +177,7 @@ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -333,14 +328,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const } //------------------------------------------------------------------------------ -PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) -{ +PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const PreintegrationBase& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); const Rot3 Rot_i = pose_i.rotation(); const Vector3 pos_i = pose_i.translation().vector(); @@ -375,7 +370,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 429db33ec..5307e94a3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include namespace gtsam { @@ -47,11 +48,14 @@ namespace gtsam { /** * Struct to hold return variables by the Predict Function */ -struct PoseVelocity { +struct PoseVelocityBias { Pose3 pose; Vector3 velocity; - PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { } }; @@ -63,7 +67,7 @@ struct PoseVelocity { * Note that this factor does not force "temporal consistency" of the biases (which are usually * slowly varying quantities), see also CombinedImuFactor for more details. */ -class ImuFactor: public NoiseModelFactor5, public ImuBase { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** @@ -202,8 +206,8 @@ public: boost::optional H5 = boost::none) const; /// predicted states from IMU - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h new file mode 100644 index 000000000..f3e715f92 --- /dev/null +++ b/gtsam/navigation/ImuFactorBase.h @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +/* GTSAM includes */ +#include + +namespace gtsam { + +class ImuFactorBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + ImuFactorBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /// Needed for testable + //------------------------------------------------------------------------------ + void print(const std::string& s) const { + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /// Needed for testable + //------------------------------------------------------------------------------ + bool equals(const ImuFactorBase& expected, double tol) const { + return equal_with_abs_tol(gravity_, expected.gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) || + (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); + } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index edc391498..dc824a9d8 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -81,11 +81,11 @@ public: /// Needed for testable bool equals(const PreintegrationBase& expected, double tol) const { return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)