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

Merge pull request #468 from borglab/feature/imu-default-string 

Default string value for printing Imu factors
release/4.3a0
Varun Agrawal 2020-08-13 18:25:21 -04:00 committed by GitHub
parent 9bcdbe8b78 68a8320c68
commit 5b48e4123f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -167,10 +167,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
//------------------------------------------------------------------------------
void CombinedImuFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
cout << (s == "" ? s : s + "\n") << "CombinedImuFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
<< ")\n";
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
}

4
gtsam/navigation/CombinedImuFactor.h
View File

@ -303,8 +303,8 @@ public:
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&);
/// print
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

14
gtsam/navigation/ImuFactor.cpp
View File

@ -130,10 +130,10 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
//------------------------------------------------------------------------------
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n";
cout << (s == "" ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
<< "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n";
cout << *this << endl;
}
@ -226,9 +226,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
//------------------------------------------------------------------------------
void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ")\n";
cout << (s == "" ? s : s + "\n") << "ImuFactor2("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ")\n";
cout << *this << endl;
}

8
gtsam/navigation/ImuFactor.h
View File

@ -205,8 +205,8 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// @}
@ -283,8 +283,8 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// @}

5
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -26,12 +26,11 @@ using namespace std;
namespace gtsam {
void PreintegratedRotationParams::print(const string& s) const {
cout << s << endl;
cout << (s == "" ? s : s + "\n") << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (body_P_sensor)
body_P_sensor->print("body_P_sensor");
if (body_P_sensor) body_P_sensor->print("body_P_sensor");
}
bool PreintegratedRotationParams::equals(

2
gtsam/navigation/PreintegrationBase.cpp
View File

@ -46,7 +46,7 @@ ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
//------------------------------------------------------------------------------
void PreintegrationBase::print(const string& s) const {
cout << s << *this << endl;
cout << (s == "" ? s : s + "\n") << *this << endl;
}
//------------------------------------------------------------------------------

2
gtsam/navigation/PreintegrationBase.h
View File

@ -116,7 +116,7 @@ class GTSAM_EXPORT PreintegrationBase {
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
virtual void print(const std::string& s) const;
virtual void print(const std::string& s="") const;
/// @}
/// @name Main functionality