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>(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<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),
gravity_(gravity),
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
///< [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]
///< (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_);
}
};

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>(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<const Pose3&> 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),

6
gtsam/navigation/ImuFactor.h
View File

@ -82,7 +82,7 @@ public:
Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of
///< 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*).
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_);
}
};