From 747342ffdae9126173dc53955555828afc96c315 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 2 Jul 2012 15:01:28 +0000 Subject: [PATCH] Changed some printing details, prompted by MATLAB display --- gtsam.h | 4 ++-- gtsam/geometry/Pose3.cpp | 5 +++-- gtsam/nonlinear/NonlinearEquality.h | 4 ++-- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/Values.cpp | 2 +- gtsam/slam/BearingFactor.h | 7 +++++++ gtsam/slam/BearingRangeFactor.h | 8 ++++---- gtsam/slam/ProjectionFactor.h | 7 ++++--- gtsam/slam/RangeFactor.h | 3 ++- 9 files changed, 26 insertions(+), 16 deletions(-) diff --git a/gtsam.h b/gtsam.h index 6858e4d65..2a1e5c0b3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1267,10 +1267,10 @@ class ISAM { class LevenbergMarquardtOptimizer { double lambda() const; void iterate(); - visualSLAM::Values optimize(); - visualSLAM::Values optimizeSafely(); double error() const; size_t iterations() const; + visualSLAM::Values optimize(); + visualSLAM::Values optimizeSafely(); visualSLAM::Values values() const; }; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 59e5ceef5..be9869030 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -55,8 +55,9 @@ namespace gtsam { /* ************************************************************************* */ void Pose3::print(const string& s) const { - R_.print(s + ".R"); - t_.print(s + ".t"); + cout << s; + R_.print("R:\n"); + t_.print("t: "); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e5f419ff4..e4a628b85 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -109,8 +109,8 @@ namespace gtsam { /// @{ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << "Constraint: " << s << " on [" << keyFormatter(this->key()) << "]\n"; - gtsam::print(feasible_,"Feasible Point"); + std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; + gtsam::print(feasible_,"Feasible Point:\n"); std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 21e3d17d7..ad10b3ee0 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -99,7 +99,7 @@ public: /** print */ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "keys = { "; + std::cout << s << " keys = { "; BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 58099d149..636c87c73 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -43,7 +43,7 @@ namespace gtsam { void Values::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str << "Values with " << size() << " values:" << endl; for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - cout << " " << keyFormatter(key_value->key) << ": "; + cout << "Value " << keyFormatter(key_value->key) << ": "; key_value->value.print(""); } } diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 251e37a37..c42c2798d 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -78,6 +78,13 @@ namespace gtsam { return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } + /** print contents */ + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "BearingFactor, bearing = "; + measured_.print(); + Base::print("", keyFormatter); + } + private: /** Serialization function */ diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 3ce671292..933333fdf 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -64,12 +64,12 @@ namespace gtsam { /** Print */ virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << ": BearingRangeFactor(" + std::cout << s << "BearingRangeFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measuredBearing_.print(" measured bearing"); - std::cout << " measured range: " << measuredRange_ << std::endl; - this->noiseModel_->print(" noise model: "); + measuredBearing_.print("measured bearing:\n"); + std::cout << "measured range: " << measuredRange_ << std::endl; + this->noiseModel_->print("noise model:\n"); } /** equals */ diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index e607809b0..bce7bcf84 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -78,9 +78,10 @@ namespace gtsam { * print * @param s optional string naming the factor */ - void print(const std::string& s = "ProjectionFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GenericProjectionFactor, z = "; + measured_.print(); + Base::print("", keyFormatter); } /// equals diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index dcc94476a..78727f08f 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -75,7 +75,8 @@ namespace gtsam { /** print contents */ void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s + std::string(" range: ") + boost::lexical_cast(measured_), keyFormatter); + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + Base::print("", keyFormatter); } private: