diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 090b0c980..7d6fdcc11 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -148,7 +148,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Matrix3::Zero()) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -156,7 +156,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const Vector3& omegaCoriolis, boost::optional body_P_sensor) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } @@ -172,7 +172,7 @@ void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) @@ -183,7 +183,7 @@ void AHRSFactor::print(const string& s, bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -197,11 +197,11 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, // Do bias correction, if (H3) will contain 3*3 derivative used below const Vector3 theta_biascorrected = // - preintegratedMeasurements_.predict(bias, H3); + _PIM_.predict(bias, H3); // Coriolis term const Vector3 coriolis = // - preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); + _PIM_.integrateCoriolis(rot_i, omegaCoriolis_); const Vector3 theta_corrected = theta_biascorrected - coriolis; // Prediction diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 8b065dd7e..e3eb3d51a 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -119,7 +119,7 @@ private: typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; Vector3 gravity_; Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -165,7 +165,7 @@ public: /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -195,7 +195,7 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index b3e8d4b11..edd57d17f 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -158,7 +158,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), _PIM_(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, @@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ 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), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - preintegratedMeasurements_(preintegratedMeasurements) {} + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -185,7 +185,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; ImuFactorBase::print(""); - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } @@ -193,7 +193,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) bool CombinedImuFactor::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) + && _PIM_.equals(e->_PIM_, tol) && ImuFactorBase::equals(*e, tol); } @@ -209,7 +209,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); // error wrt bias evolution model (random walk) @@ -256,7 +256,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ } // else, only compute the error vector: // error wrt preintegrated measurements - Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, + Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, boost::none, boost::none, boost::none, boost::none, boost::none); // error wrt bias evolution model (random walk) Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 98a248024..03a0dd824 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -149,7 +149,7 @@ private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements preintegratedMeasurements_; + CombinedPreintegratedMeasurements _PIM_; public: @@ -198,7 +198,7 @@ public: /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -228,7 +228,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 640e02641..1b055bef2 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -139,7 +139,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -150,7 +150,7 @@ ImuFactor::ImuFactor( const bool use2ndOrderCoriolis) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), - preintegratedMeasurements_(preintegratedMeasurements) {} + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -167,7 +167,7 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; ImuFactorBase::print(""); - preintegratedMeasurements_.print(" preintegrated measurements:"); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } @@ -175,7 +175,7 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { 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) + && _PIM_.equals(e->_PIM_, tol) && ImuFactorBase::equals(*e, tol); } @@ -186,7 +186,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const boost::optional H3, boost::optional H4, boost::optional H5) const{ - return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); + return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 89344f1a1..01a245f8b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -136,7 +136,7 @@ public: typedef ImuFactor This; typedef NoiseModelFactor5 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; public: @@ -184,7 +184,7 @@ public: /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -212,7 +212,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 7dbe6c9ec..2d637d117 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -95,14 +95,14 @@ public: /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j //------------------------------------------------------------------------------ - Vector computeErrorAndJacobians(const PreintegrationBase& preintegratedMeasurements_, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const{ - const double& deltaTij = preintegratedMeasurements_.deltaTij(); + const double& deltaTij = _PIM.deltaTij(); // We need the mistmatch w.r.t. the biases used for preintegration - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); // we give some shorter name to rotations and translations const Rot3& Rot_i = pose_i.rotation(); @@ -115,7 +115,7 @@ public: // 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 Vector3 theta_biascorrected = - preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5); + _PIM.biascorrectedThetaRij(biasOmegaIncr, H5); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term @@ -144,13 +144,13 @@ public: } (*H1) << // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + Rot_i.matrix() * skewSymmetric(_PIM.deltaPij() + + _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr), // dfP/dPi dfPdPi, // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + Rot_i.matrix() * skewSymmetric(_PIM.deltaVij() + + _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr), // dfV/dPi dfVdPi, // dfR/dRi @@ -196,11 +196,11 @@ public: H5->resize(9,6); (*H5) << // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + - Rot_i.matrix() * _PIM.delPdelBiasAcc(), + - Rot_i.matrix() * _PIM.delPdelBiasOmega(), // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + - Rot_i.matrix() * _PIM.delVdelBiasAcc(), + - Rot_i.matrix() * _PIM.delVdelBiasOmega(), // dfR/dBias Matrix::Zero(3,3), Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); @@ -208,7 +208,7 @@ public: // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ - PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements_, + PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM, gravity_, omegaCoriolis_, use2ndOrderCoriolis_); const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); @@ -226,28 +226,28 @@ public: //------------------------------------------------------------------------------ static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const PreintegrationBase& preintegratedMeasurements, + const PreintegrationBase& _PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - const double& deltaTij = preintegratedMeasurements.deltaTij(); - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + const double& deltaTij = _PIM.deltaTij(); + const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope(); const Rot3& Rot_i = pose_i.rotation(); const Vector3& pos_i = pose_i.translation().vector(); // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij() + + _PIM.delPdelBiasAcc() * biasAccIncr + + _PIM.delPdelBiasOmega() * biasOmegaIncr) + vel_i * deltaTij - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + 0.5 * gravity * deltaTij*deltaTij; - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij() + + _PIM.delVdelBiasAcc() * biasAccIncr + + _PIM.delVdelBiasOmega() * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + gravity * deltaTij); @@ -256,7 +256,7 @@ public: vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.biascorrectedDeltaRij(biasOmegaIncr); + const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr); // TODO Frank says comment below does not reflect what was in code // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 173228c61..fd5597aff 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -34,11 +34,6 @@ namespace gtsam { */ class PreintegrationBase : public PreintegratedRotation { - friend class ImuFactorBase; - friend class ImuFactor; - friend class CombinedImuFactor; - -protected: imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration bool use2ndOrderIntegration_; ///< Controls the order of integration @@ -67,7 +62,8 @@ public: /// methods to access class variables const Vector3& deltaPij() const {return deltaPij_;} const Vector3& deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} // TODO expensive + const imuBias::ConstantBias& biasHat() const { return biasHat_;} + Vector biasHatVector() const { return biasHat_.vector();} // expensive const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}