diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index c95cc36d6..6de9989f2 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -63,9 +63,11 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print(const std::string& s = "Discrete Conditional: ") const { + void print(const std::string& s = "Discrete Conditional: ", + const boost::function& formatter + = &(boost::lexical_cast)) const { std::cout << s << std::endl; - IndexConditional::print(s); + IndexConditional::print(s, formatter); Potentials::print(s); } diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 0bfdb3068..deac7efce 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -82,8 +82,10 @@ namespace gtsam { /// @{ // print - virtual void print(const std::string& s = "DiscreteFactor\n") const { - IndexFactor::print(s); + virtual void print(const std::string& s = "DiscreteFactor\n", + const boost::function& formatter + = &(boost::lexical_cast)) const { + IndexFactor::print(s,formatter); } /// @} diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4726c752f..f3c89721b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -63,6 +63,18 @@ namespace gtsam { return product; } + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } + /* ************************************************************************* */ std::pair // EliminateDiscrete(const FactorGraph& factors, size_t num) { @@ -90,6 +102,7 @@ namespace gtsam { return std::make_pair(cond, sum); } + /* ************************************************************************* */ } // namespace diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d96a3049a..44b814507 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -77,15 +77,8 @@ public: double operator()(const DiscreteFactor::Values & values) const; /// print - void print(const std::string& s = "DiscreteFactorGraph") const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); - } - } + void print(const std::string& s = "DiscreteFactorGraph", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; }; // DiscreteFactorGraph diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 112782ecc..89361e8fc 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -34,10 +34,11 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNet::print(const std::string& s) const { + void BayesNet::print(const std::string& s, + const boost::function& formatter) const { std::cout << s; BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) - conditional->print(); + conditional->print("Conditional", formatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index e465fdfd3..32abd9ed6 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,7 @@ public: typedef typename boost::shared_ptr > shared_ptr; /** We store shared pointers to Conditional densities */ + typedef typename CONDITIONAL::KeyType KeyType; typedef typename boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr const_sharedConditional; typedef typename std::list Conditionals; @@ -88,7 +90,8 @@ public: /// @{ /** print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "", + const boost::function& formatter = &(boost::lexical_cast)) const; /** print statistics */ void printStats(const std::string& s = "") const; diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index 548a55cf4..b3fe204e7 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -71,10 +71,11 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::print(const std::string& indent) const { + void ClusterTree::Cluster::print(const std::string& indent, + const boost::function& formatter) const { std::cout << indent; BOOST_FOREACH(const Index key, frontal) - std::cout << key << " "; + std::cout << formatter(key) << " "; std::cout << ": "; BOOST_FOREACH(const Index key, separator) std::cout << key << " "; @@ -83,19 +84,21 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::printTree(const std::string& indent) const { - print(indent); + void ClusterTree::Cluster::printTree(const std::string& indent, + const boost::function& formatter) const { + print(indent, formatter); BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent + " "); + child->printTree(indent + " ", formatter); } /* ************************************************************************* * * ClusterTree * ************************************************************************* */ template - void ClusterTree::print(const std::string& str) const { + void ClusterTree::print(const std::string& str, + const boost::function& formatter) const { std::cout << str << std::endl; - if (root_) root_->printTree(""); + if (root_) root_->printTree("", formatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 92919b936..f253fe4d6 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -25,6 +25,7 @@ #include #include +#include #include @@ -38,6 +39,9 @@ namespace gtsam { */ template class ClusterTree { + public: + // Access to factor types + typedef typename FG::KeyType KeyType; protected: @@ -74,10 +78,10 @@ namespace gtsam { Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); /// print - void print(const std::string& indent) const; + void print(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// print the enire tree - void printTree(const std::string& indent) const; + void printTree(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const Cluster& other) const; @@ -123,7 +127,7 @@ namespace gtsam { /// @{ /// print the object - void print(const std::string& str="") const; + void print(const std::string& str="", const boost::function& formatter = &(boost::lexical_cast)) const; /** check equality */ bool equals(const ClusterTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index c4cb9d4cf..81c98c926 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -166,12 +166,13 @@ EliminationTree::Create(const FactorGraph& factorGraph) { /* ************************************************************************* */ template -void EliminationTree::print(const std::string& name) const { - std::cout << name << " (" << key_ << ")" << std::endl; +void EliminationTree::print(const std::string& name, + const boost::function& formatter) const { + std::cout << name << " (" << formatter(key_) << ")" << std::endl; BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->print(name + " "); } + factor->print(name + " ", formatter); } BOOST_FOREACH(const shared_ptr& child, subTrees_) { - child->print(name + " "); } + child->print(name + " ", formatter); } } /* ************************************************************************* */ diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 6ab7dcf49..8e57a7208 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -54,6 +54,8 @@ public: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef gtsam::BayesNet BayesNet; ///< The BayesNet corresponding to FACTOR + typedef FACTOR Factor; + typedef typename FACTOR::KeyType KeyType; /** Typedef for an eliminate subroutine */ typedef typename FactorGraph::Eliminate Eliminate; @@ -67,7 +69,7 @@ private: typedef FastList SubTrees; typedef std::vector Conditionals; - Index key_; ///< index associated with root + Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor? Factors factors_; ///< factors associated with root SubTrees subTrees_; ///< sub-trees @@ -141,7 +143,8 @@ public: /// @{ /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ") const; + void print(const std::string& name = "EliminationTree: ", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Test whether the tree is equal to another */ bool equals(const EliminationTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index d9fb1e0de..ad9f43701 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -56,9 +56,10 @@ namespace gtsam { /* ************************************************************************* */ template - void Factor::print(const std::string& s) const { + void Factor::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << key; + BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); std::cout << std::endl; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 6ed9c809b..0e80fc8ea 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include namespace gtsam { @@ -190,7 +192,8 @@ public: /// @{ /// print - void print(const std::string& s = "Factor") const; + void print(const std::string& s = "Factor", + const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 90212f39f..c787655af 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -48,13 +48,14 @@ namespace gtsam { /* ************************************************************************* */ template - void FactorGraph::print(const std::string& s) const { + void FactorGraph::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 89662ffc4..905afc55c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -138,7 +138,8 @@ template class BayesTree; /// @{ /** print out graph */ - void print(const std::string& s = "FactorGraph") const; + void print(const std::string& s = "FactorGraph", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 97e42d9ba..c8fc4c21a 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,11 +23,11 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - void GaussianDensity::print(const string &s) const + void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const { cout << s << ": density on "; for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) - cout << (boost::format("[%1%]")%(*it)).str() << " "; + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; gtsam::print(Matrix(get_R()),"R"); gtsam::print(Vector(get_d()),"d"); diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 2c13e847c..61db582f4 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -54,7 +54,8 @@ namespace gtsam { } /// print - void print(const std::string& = "GaussianDensity") const; + void print(const std::string& = "GaussianDensity", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 5868d8452..1413d1517 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -77,10 +77,10 @@ void VectorValues::insert(Index j, const Vector& value) { } /* ************************************************************************* */ -void VectorValues::print(const std::string& str) const { +void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; for (Index var = 0; var < size(); ++var) - std::cout << " " << var << ": \n" << operator[](var) << "\n"; + std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n"; std::cout.flush(); } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index f3d82511e..89c59a6ad 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -159,15 +160,16 @@ namespace gtsam { iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables - iterator end() { chk(); return maps_.end(); } ///< Iterator over variables + iterator end() { chk(); return maps_.end(); } ///< Iterator over variables const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables + reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ") const; + void print(const std::string& str = "VectorValues: ", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const;