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>
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!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
saveGraph(of, root_);
saveGraph(of, root_, indexFormatter);
of<<"}";
of.close();
}
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;
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);
}
}

6
gtsam/inference/BayesTree.h
View File

@ -25,6 +25,7 @@
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
@ -33,6 +34,7 @@
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Key.h>
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<std::string, Index>) const;
/// @}
/// @name Advanced Interface

17
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -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));
}
/* ************************************************************************* */

2
gtsam/nonlinear/NonlinearISAM.h
View File

@ -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