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

removed useless parameters, included "using namespace std", deleted arguments in setZero() for fixed size matrix

release/4.3a0
Luca 2014-12-02 15:56:53 -05:00
parent 78f19d1aa3
commit 32f0bdb288
1 changed files with 26 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/navigation/ImuFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file ImuFactor.h * @file ImuFactor.cpp
* @author Luca Carlone * @author Luca Carlone
* @author Stephen Williams * @author Stephen Williams
* @author Richard Roberts * @author Richard Roberts
@ -26,6 +26,8 @@
namespace gtsam { namespace gtsam {
using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements // Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -47,15 +49,15 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::print(const std::string& s) const { void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
std::cout << s << std::endl; cout << s << endl;
biasHat_.print(" biasHat"); biasHat_.print(" biasHat");
std::cout << " deltaTij " << deltaTij_ << std::endl; cout << " deltaTij " << deltaTij_ << endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << std::endl; cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
std::cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << std::endl; cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -65,7 +67,7 @@ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurement
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol) && deltaRij_.equals(expected.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol && fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
@ -84,7 +86,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
delVdelBiasAcc_ = Z_3x3; delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3; delVdelBiasOmega_ = Z_3x3;
delRdelBiasOmega_ = Z_3x3; delRdelBiasOmega_ = Z_3x3;
PreintMeasCov_.setZero(9,9); PreintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -215,16 +217,16 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << "ImuFactor(" cout << s << "ImuFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n"; << keyFormatter(this->key5()) << ")\n";
preintegratedMeasurements_.print(" preintegrated measurements:"); preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_) if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: "); this->body_P_sensor_->print(" sensor pose in body frame: ");
@ -390,8 +392,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensore, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
const bool use2ndOrderCoriolis)
{ {
const double& deltaTij = preintegratedMeasurements.deltaTij_; const double& deltaTij = preintegratedMeasurements.deltaTij_;