diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 7e011b697..0c5b89d9f 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -45,27 +45,27 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ PreintegrationBase::print(s); cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; + cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ PreintegrationBase::resetIntegration(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -149,7 +149,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( 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; // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_ const CombinedPreintegratedMeasurements& preintegratedMeasurements, 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), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6536a49a8..856929c41 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -87,7 +87,7 @@ public: Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases @@ -132,7 +132,7 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} + Matrix PreintMeasCov() const { return preintMeasCov_;} private: @@ -142,7 +142,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9fc7559f9..d29620e97 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -41,27 +41,27 @@ 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(9,9); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; + cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(PreintMeasCov_, expected.PreintMeasCov_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ PreintegrationBase::resetIntegration(); - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ @@ -119,7 +119,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // the deltaT allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // This in only kept for documentation. @@ -129,7 +129,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) /* ----------------------------------------------------------------------------------------------------------------------- */ @@ -149,7 +149,7 @@ ImuFactor::ImuFactor( 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), + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 1d4b027fa..78291450e 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -82,7 +82,7 @@ public: Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). public: @@ -121,7 +121,7 @@ public: /// methods to access class variables Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} + Matrix preintMeasCov() const { return preintMeasCov_;} private: @@ -131,7 +131,7 @@ public: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } };