diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index 0e1bf8826..fc38ed84f 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -5,11 +5,16 @@ * Author: Frank Dellaert */ +#include +#include +#include #include +#include "Point2.h" #include "Ordering.h" #include "SymbolicFactorGraph.h" #include "SymbolicBayesNet.h" #include "inference-inl.h" +#include "LieConfig-inl.h" using namespace std; @@ -38,5 +43,40 @@ namespace gtsam { return bayesNet; } + /* ************************************************************************* */ + void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s) { + + Symbol key; + Point2 pt; + float scale = 100; + + ofstream of(s.c_str()); + of << "graph G{" << endl; + of << "bgcolor=\"transparent\";" << endl; + + BOOST_FOREACH(boost::tie(key, pt), config){ + of << (string)key << "[label=\"" << (string)key << "\"][pos=\"" << pt.x()*scale << "," << pt.y()*scale << "\"];" << endl; + } + + int index = 0; + BOOST_FOREACH(const SymbolicFactorGraph::sharedFactor& factor, fg) { + index++; + Point2 center; + BOOST_FOREACH(const Symbol& key, factor->keys()) + center = center + config[key]; + center = Point2(center.x() / factor->keys().size(), center.y() / factor->keys().size()); + of << "f" << index << "[pos=\"" << center.x()*scale << "," << center.y()*scale << "\"][shape=\"point\"];" << endl; + BOOST_FOREACH(const Symbol& key, factor->keys()) + of << "f" << index << "--" << (string)key << endl; + } + of<<"}"; + of.close(); + + char filename[100]; + sscanf(s.c_str(), "%s.dot", filename); + string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % s % filename); + system(cmd.c_str()); + } + /* ************************************************************************* */ } diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h index 1b77806a8..8210aa0b3 100644 --- a/cpp/SymbolicFactorGraph.h +++ b/cpp/SymbolicFactorGraph.h @@ -14,9 +14,14 @@ #include "SymbolicFactor.h" #include "SymbolicBayesNet.h" #include "Key.h" +#include "LieConfig.h" namespace gtsam { + class Point2; + + typedef LieConfig SymbolicConfig; + class SymbolicConditional; /** Symbolic Factor Graph */ @@ -78,6 +83,8 @@ namespace gtsam { }; + // save graph to the graphviz format + void saveGraph(const SymbolicFactorGraph& fg, const SymbolicConfig& config, const std::string& s); } #endif /* SYMBOLICFACTORGRAPH_H_ */ diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 2bb3b853a..fb0fb438a 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -134,6 +134,21 @@ TEST( GaussianFactorGraph, eliminate ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, eliminate2 ) +{ + // create a test graph + SymbolicFactorGraph fg; + fg.push_factor("x1", "x2"); + + fg.eliminateOne("x1"); + SymbolicFactorGraph expected; + expected.push_back(boost::shared_ptr()); + expected.push_factor("x2"); + + CHECK(assert_equal(expected, fg)); +} + /* ************************************************************************* */ TEST( SymbolicFactorGraph, constructFromBayesNet ) {