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

fixed naming convention

release/4.3a0
Luca 2014-12-03 19:09:04 -05:00
parent 218af7c889
commit 7dd359ce5d
4 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -45,27 +45,27 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu
measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; measurementCovariance_.block<6,6>(15,15) = 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 << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) 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); && PreintegrationBase::equals(expected, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
PreintegrationBase::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>(3,6) = block23;
G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); 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 // Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------------------------------------------------- */
@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_
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(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements), preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity), gravity_(gravity),
omegaCoriolis_(omegaCoriolis), omegaCoriolis_(omegaCoriolis),

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -87,7 +87,7 @@ public:
Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
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
@ -132,7 +132,7 @@ public:
/// methods to access class variables /// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix PreintMeasCov() const { return PreintMeasCov_;} Matrix PreintMeasCov() const { return preintMeasCov_;}
private: private:
@ -142,7 +142,7 @@ public:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
} }
}; };

14
gtsam/navigation/ImuFactor.cpp
View File

@ -41,27 +41,27 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
PreintMeasCov_.setZero(9,9); preintMeasCov_.setZero(9,9);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::print(const string& s) const { void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
PreintegrationBase::print(s); PreintegrationBase::print(s);
cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; 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 { bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) 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); && PreintegrationBase::equals(expected, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration(){ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
PreintegrationBase::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 // the deltaT allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * 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 // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only kept for documentation. // This in only kept for documentation.
@ -129,7 +129,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
// Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; // 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!) // 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, 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(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), preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity), gravity_(gravity),
omegaCoriolis_(omegaCoriolis), omegaCoriolis_(omegaCoriolis),

6
gtsam/navigation/ImuFactor.h
View File

@ -82,7 +82,7 @@ public:
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of
///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9) ///< the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
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:
@ -121,7 +121,7 @@ public:
/// methods to access class variables /// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix preintMeasCov() const { return PreintMeasCov_;} Matrix preintMeasCov() const { return preintMeasCov_;}
private: private:
@ -131,7 +131,7 @@ public:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
ar & BOOST_SERIALIZATION_NVP(PreintMeasCov_); ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
} }
}; };