diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index e24dfdf2a..f8b3fc0bb 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -74,13 +74,14 @@ public: /// @name Testable /// @{ - // equals + /// equals virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; - // print - virtual void print(const std::string& s = "DiscreteFactor\n", - const KeyFormatter& formatter = DefaultKeyFormatter) const { - Factor::print(s, formatter); + /// print + virtual void print( + const std::string& s = "DiscreteFactor\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); } /** Test whether the factor is empty */ diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 4c2607f1f..8df602af5 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -129,8 +129,9 @@ public: double operator()(const DiscreteFactor::Values & values) const; /// print - void print(const std::string& s = "DiscreteFactorGraph", - const KeyFormatter& formatter =DefaultKeyFormatter) const; + void print( + const std::string& s = "DiscreteFactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /** Solve the factor graph by performing variable elimination in COLAMD order using * the dense elimination function specified in \c function, diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index eff747eb5..97ebe8c7e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -144,7 +144,7 @@ public: bool equals(const PinholeBase &camera, double tol = 1e-9) const; /// print - void print(const std::string& s = "PinholeBase") const; + virtual void print(const std::string& s = "PinholeBase") const; /// @} /// @name Standard Interface @@ -324,6 +324,11 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; + /// print + void print(const std::string& s = "CalibratedCamera") const override { + PinholeBase::print(s); + } + /// @deprecated inline size_t dim() const { return dimension; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index ecff766e2..8ac67a5c3 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -148,7 +148,7 @@ public: } /// print - void print(const std::string& s = "PinholeCamera") const { + void print(const std::string& s = "PinholeCamera") const override { Base::print(s); K_.print(s + ".calibration"); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 1b5a06609..949caaa27 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -340,7 +340,7 @@ public: } /// print - void print(const std::string& s = "PinholePose") const { + void print(const std::string& s = "PinholePose") const override { Base::print(s); if (!K_) std::cout << "s No calibration given" << std::endl; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 33e9a58ca..c5bf6511c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -986,7 +986,7 @@ class CalibratedCamera { static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s = "") const; + void print(string s = "CalibratedCamera") const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold @@ -1163,8 +1163,9 @@ virtual class SymbolicFactor { // From Factor size_t size() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicFactor", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const; gtsam::KeyVector keys(); }; @@ -1177,8 +1178,9 @@ virtual class SymbolicFactorGraph { // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; bool exists(size_t idx) const; @@ -1242,8 +1244,9 @@ class SymbolicBayesNet { SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicBayesNet", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; // Standard interface @@ -2097,8 +2100,9 @@ class NonlinearFactorGraph { NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); // FactorGraph - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "NonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; bool empty() const; diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index a3bd87887..a73762258 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -26,30 +26,30 @@ namespace gtsam { - /* ************************************************************************* */ - template - void BayesNet::print(const std::string& s, const KeyFormatter& formatter) const - { - Base::print(s, formatter); - } - - /* ************************************************************************* */ - template - void BayesNet::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const - { - std::ofstream of(s.c_str()); - of << "digraph G{\n"; - - for (auto conditional: boost::adaptors::reverse(*this)) { - typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Key me = frontals.front(); - typename CONDITIONAL::Parents parents = conditional->parents(); - for(Key p: parents) - of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; - } - - of << "}"; - of.close(); - } - +/* ************************************************************************* */ +template +void BayesNet::print( + const std::string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); } + +/* ************************************************************************* */ +template +void BayesNet::saveGraph(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + for (auto conditional : boost::adaptors::reverse(*this)) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + typename CONDITIONAL::Parents parents = conditional->parents(); + for (Key p : parents) + of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; + } + + of << "}"; + of.close(); +} + +} // namespace gtsam diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 0597ece98..938278d5a 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -57,16 +57,18 @@ namespace gtsam { /// @name Testable /// @{ - /** print out graph */ - void print(const std::string& s = "BayesNet", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /** print out graph */ + void print( + const std::string& s = "BayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; - /// @} + /// @} - /// @name Standard Interface - /// @{ + /// @name Standard Interface + /// @{ - void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void saveGraph(const std::string& s, + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 58448edbb..6fe96c777 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -33,8 +33,8 @@ namespace gtsam { /* ************************************************************************* */ void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << " "; - for(Key key: keys_) std::cout << " " << formatter(key); + std::cout << (s.empty() ? "" : s + " "); + for (Key key : keys_) std::cout << " " << formatter(key); std::cout << std::endl; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 1aaaff0e4..57f95b0ea 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -135,10 +135,14 @@ typedef FastSet FactorIndexSet; /// @{ /// print - void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; /// print only keys - void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual void printKeys( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; protected: /// check equality diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index df68019e1..e1c18274a 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -37,7 +37,7 @@ namespace gtsam { template void FactorGraph::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << std::endl; + std::cout << (s.empty() ? "" : s + " ") << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 9d2308d9b..90b9d7ef2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -285,9 +285,9 @@ class FactorGraph { /// @name Testable /// @{ - /** print out graph */ - void print(const std::string& s = "FactorGraph", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /// print out graph + virtual void print(const std::string& s = "FactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 06782c3cf..a45168e0b 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -177,6 +177,13 @@ namespace gtsam { */ VectorValues backSubstituteTranspose(const VectorValues& gx) const; + /// print graph + virtual void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /** * @brief Save the GaussianBayesNet as an image. Requires `dot` to be * installed. diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 9b4c5f940..72ad69693 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -54,8 +54,11 @@ namespace gtsam { virtual ~GaussianFactor() {} // Implementing Testable interface - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const = 0; + + /// print + virtual void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0; /** Equals for testable */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7f335152e..8c8eb5772 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -42,7 +42,8 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb, //*************************************************************************** void Rot3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; + cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on " + << keyFormatter(this->key()) << "\n"; nZ_.print(" measured direction in nav frame: "); bRef_.print(" reference direction in body frame: "); this->noiseModel_->print(" noise model: "); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 23fbbca89..3016b31af 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -114,8 +114,8 @@ public: } /** 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; @@ -188,8 +188,8 @@ public: } /** 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; diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index f2448c488..1d6b78e13 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -24,7 +24,8 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; + cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) + << "\n"; cout << " GPS measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index f6469346e..8fcf0f099 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -71,8 +71,8 @@ public: } /// 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; @@ -143,8 +143,8 @@ public: } /// 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; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 8c257f7ca..adb6310e8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -70,8 +70,9 @@ public: /// @{ /** print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 989f493d3..9bca4a29d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -99,8 +99,9 @@ namespace gtsam { NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} /** print */ - void print(const std::string& str = "NonlinearFactorGraph: ", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print( + const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** print errors along with factors*/ void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 99e553f7a..7e102fee7 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -64,8 +64,8 @@ private: /// @name Testable /// @{ - void print(const std::string &s, - const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + void print(const std::string &s, const KeyFormatter &keyFormatter = + DefaultKeyFormatter) const override { std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; traits::Print(measured_, " measured: "); diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index ca87b2bbc..45df56abd 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -63,6 +63,13 @@ namespace gtsam { /** Check equality */ GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const; + /// print + GTSAM_EXPORT void print( + const std::string& s = "SymbolicBayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /// @} /// @name Standard Interface diff --git a/gtsam/symbolic/SymbolicConditional.cpp b/gtsam/symbolic/SymbolicConditional.cpp index d733c0937..f0d9944b2 100644 --- a/gtsam/symbolic/SymbolicConditional.cpp +++ b/gtsam/symbolic/SymbolicConditional.cpp @@ -20,18 +20,17 @@ namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SymbolicConditional::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - BaseConditional::print(str, keyFormatter); - } - - /* ************************************************************************* */ - bool SymbolicConditional::equals(const This& c, double tol) const - { - return BaseFactor::equals(c) && BaseConditional::equals(c); - } +using namespace std; +/* ************************************************************************* */ +void SymbolicConditional::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + BaseConditional::print(str, keyFormatter); } + +/* ************************************************************************* */ +bool SymbolicConditional::equals(const This& c, double tol) const { + return BaseFactor::equals(c) && BaseConditional::equals(c); +} + +} // namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index ead72a989..4088cbfb6 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -105,7 +105,9 @@ namespace gtsam { /// @name Testable /** Print with optional formatter */ - virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& str = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check equality */ bool equals(const This& c, double tol = 1e-9) const; diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 66657aa7d..2a488a4da 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -92,6 +92,20 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; + /// print + void print( + const std::string& s = "SymbolicFactor", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + + /// print only keys + void printKeys( + const std::string& s = "SymbolicFactor", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::printKeys(s, formatter); + } + /// @} /// @name Advanced Constructors diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index b6f0de2ae..7f4c84631 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -88,6 +88,13 @@ namespace gtsam { bool equals(const This& fg, double tol = 1e-9) const; + /// print + void print( + const std::string& s = "SymbolicFactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /// @} /// @name Standard Interface diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index d042b0436..7016a7e97 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -37,8 +37,9 @@ public: typedef boost::shared_ptr shared_ptr; /** print */ - void print(const std::string& str, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + void print( + const std::string& str = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(str, keyFormatter); }