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

Added key formatting in saveGraph, which uses inverse ordering to find Key and formats the Key

release/4.3a0
Richard Roberts 2012-06-25 03:47:07 +00:00
parent 3dd2eec937
commit b16fe4f7fc
4 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/inference/BayesTree-inl.h
View File

@ -58,17 +58,17 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s) const { void BayesTree<CONDITIONAL,CLIQUE>::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!"); if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of<< "digraph G{\n"; of<< "digraph G{\n";
saveGraph(of, root_); saveGraph(of, root_, indexFormatter);
of<<"}"; of<<"}";
of.close(); of.close();
} }
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, int parentnum) const { void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;
std::stringstream out; std::stringstream out;
@ -78,7 +78,7 @@ namespace gtsam {
BOOST_FOREACH(Index index, clique->conditional_->frontals()) { BOOST_FOREACH(Index index, clique->conditional_->frontals()) {
if(!first) parent += ","; first = false; if(!first) parent += ","; first = false;
parent += (boost::format("%1%")%index).str();; parent += indexFormatter(index);
} }
if( clique != root_){ if( clique != root_){
@ -89,7 +89,7 @@ namespace gtsam {
first = true; first = true;
BOOST_FOREACH(Index sep, clique->conditional_->parents()) { BOOST_FOREACH(Index sep, clique->conditional_->parents()) {
if(!first) parent += ","; first = false; if(!first) parent += ","; first = false;
parent += (boost::format("%1%")%sep).str(); parent += indexFormatter(sep);
} }
parent += "\"];\n"; parent += "\"];\n";
s << parent; s << parent;
@ -97,7 +97,7 @@ namespace gtsam {
BOOST_FOREACH(sharedClique c, clique->children_) { BOOST_FOREACH(sharedClique c, clique->children_) {
num++; num++;
saveGraph(s, c, parentnum); saveGraph(s, c, indexFormatter, parentnum);
} }
} }

6
gtsam/inference/BayesTree.h
View File

@ -25,6 +25,7 @@
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
@ -33,6 +34,7 @@
#include <gtsam/inference/BayesTreeCliqueBase.h> #include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Key.h>
namespace gtsam { namespace gtsam {
@ -97,7 +99,7 @@ namespace gtsam {
Nodes nodes_; Nodes nodes_;
/** private helper method for saving the Tree to a text file in GraphViz format */ /** 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; int parentnum = 0) const;
/** Gather data on a single clique */ /** Gather data on a single clique */
@ -232,7 +234,7 @@ namespace gtsam {
*/ */
/** saves the Tree to a text file in GraphViz format */ /** 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<std::string, Index>) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface

17
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -30,8 +30,21 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void NonlinearISAM::saveGraph(const string& s) const { void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
isam_.saveGraph(s);
// 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/nonlinear/NonlinearISAM.h
View File

@ -92,7 +92,7 @@ public:
void printStats() const; void printStats() const;
/** saves the Tree to a text file in GraphViz format */ /** 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 /// @name Advanced Interface