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

default string value for printing Imu factors

release/4.3a0
Varun Agrawal 2020-08-13 14:17:51 -05:00
parent 9bcdbe8b78
commit 68a8320c68
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, void CombinedImuFactor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << "," cout << (s == "" ? s : s + "\n") << "CombinedImuFactor("
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << ","
<< keyFormatter(this->key6()) << ")\n"; << keyFormatter(this->key5()) << "," << keyFormatter(this->key6())
<< ")\n";
_PIM_.print(" preintegrated measurements:"); _PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: "); 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, GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&); const CombinedImuFactor&);
/// print /// print
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; 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 { void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," cout << (s == "" ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1())
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n"; << ")\n";
cout << *this << endl; cout << *this << endl;
} }
@ -226,9 +226,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void ImuFactor2::print(const string& s, void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," cout << (s == "" ? s : s + "\n") << "ImuFactor2("
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ","
<< ")\n"; << keyFormatter(this->key3()) << ")\n";
cout << *this << endl; cout << *this << endl;
} }

8
gtsam/navigation/ImuFactor.h
View File

@ -205,8 +205,8 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// @} /// @}
@ -283,8 +283,8 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&); GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) 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 { namespace gtsam {
void PreintegratedRotationParams::print(const string& s) const { void PreintegratedRotationParams::print(const string& s) const {
cout << s << endl; cout << (s == "" ? s : s + "\n") << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis) if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl; cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (body_P_sensor) if (body_P_sensor) body_P_sensor->print("body_P_sensor");
body_P_sensor->print("body_P_sensor");
} }
bool PreintegratedRotationParams::equals( 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 { 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 /// @name Testable
/// @{ /// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim); 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 /// @name Main functionality