diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 411f0bba7..5d2a64627 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -58,17 +58,17 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::saveGraph(const std::string &s) const { + void BayesTree::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!"); std::ofstream of(s.c_str()); of<< "digraph G{\n"; - saveGraph(of, root_); + saveGraph(of, root_, indexFormatter); of<<"}"; of.close(); } template - void BayesTree::saveGraph(std::ostream &s, sharedClique clique, int parentnum) const { + void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { static int num = 0; bool first = true; std::stringstream out; @@ -78,7 +78,7 @@ namespace gtsam { BOOST_FOREACH(Index index, clique->conditional_->frontals()) { if(!first) parent += ","; first = false; - parent += (boost::format("%1%")%index).str();; + parent += indexFormatter(index); } if( clique != root_){ @@ -89,7 +89,7 @@ namespace gtsam { first = true; BOOST_FOREACH(Index sep, clique->conditional_->parents()) { if(!first) parent += ","; first = false; - parent += (boost::format("%1%")%sep).str(); + parent += indexFormatter(sep); } parent += "\"];\n"; s << parent; @@ -97,7 +97,7 @@ namespace gtsam { BOOST_FOREACH(sharedClique c, clique->children_) { num++; - saveGraph(s, c, parentnum); + saveGraph(s, c, indexFormatter, parentnum); } } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8c7ba1a39..c62af258a 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include namespace gtsam { @@ -97,7 +99,7 @@ namespace gtsam { Nodes nodes_; /** private helper method for saving the Tree to a text file in GraphViz format */ - void saveGraph(std::ostream &s, sharedClique clique, + void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum = 0) const; /** Gather data on a single clique */ @@ -232,7 +234,7 @@ namespace gtsam { */ /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s) const; + void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = &boost::lexical_cast) const; /// @} /// @name Advanced Interface diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 17fba2432..3bddbb635 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -30,8 +30,21 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -void NonlinearISAM::saveGraph(const string& s) const { - isam_.saveGraph(s); +void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const { + + // Create an index formatter that looks up the Key in an inverse ordering, then + // formats the key using the provided key formatter. + struct OrderingIndexFormatter { + Ordering::InvertedMap inverseOrdering; + const KeyFormatter& keyFormatter; + OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) : + inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {} + string operator()(Index index) { + return keyFormatter(inverseOrdering.at(index)); + } + }; + + isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 7a7b9cd2c..fc2f69f55 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -92,7 +92,7 @@ public: void printStats() const; /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s) const; + void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} /// @name Advanced Interface