diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 54751af76..5205d5cd0 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -22,6 +22,18 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +template +template +FactorGraph::FactorGraph(const BayesNet& bayesNet) +{ + typename BayesNet::const_iterator it = bayesNet.begin(); + for(; it != bayesNet.end(); it++) { + typename boost::shared_ptr::shared_ptr factor(new Factor(*it)); + push_back(factor); + } +} + /* ************************************************************************* */ template void FactorGraph::print(const string& s) const { @@ -229,5 +241,25 @@ boost::shared_ptr FactorGraph::eliminateOne(const std::stri return conditional; } +/* ************************************************************************* */ +// This doubly templated function is generic. There is a LinearFactorGraph +// version that returns a more specific GaussianBayesNet. +// Note, you will need to include this file to instantiate the function. +/* ************************************************************************* */ +template +template +boost::shared_ptr > +FactorGraph::eliminate(const Ordering& ordering) +{ + boost::shared_ptr > bayesNet (new BayesNet()); // empty + + BOOST_FOREACH(string key, ordering) { + boost::shared_ptr cg = eliminateOne(key); + bayesNet->push_back(cg); + } + + return bayesNet; +} + /* ************************************************************************* */ } diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index aecbc49c6..de0096359 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -16,6 +16,7 @@ #include #include "Testable.h" +#include "BayesNet.h" namespace gtsam { @@ -43,6 +44,13 @@ namespace gtsam { public: + /** Default constructor */ + FactorGraph() {} + + /** convert from Bayes net */ + template + FactorGraph(const BayesNet& bayesNet); + /** print out graph */ void print(const std::string& s = "FactorGraph") const; @@ -111,6 +119,13 @@ namespace gtsam { template boost::shared_ptr eliminateOne(const std::string& key); + /** + * eliminate factor graph using the given (not necessarily complete) + * ordering, yielding a chordal Bayes net and (partially eliminated) FG + */ + template + boost::shared_ptr > eliminate(const Ordering& ordering); + private: /** Serialization function */ diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 6558c746e..922bcf801 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -23,24 +23,10 @@ using namespace gtsam; template class FactorGraph; /* ************************************************************************* */ -LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) -{ - setCBN(CBN); +LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) : + FactorGraph (CBN) { } -/* ************************************************************************* */ -void LinearFactorGraph::setCBN(const GaussianBayesNet& CBN) -{ - clear(); - GaussianBayesNet::const_iterator it = CBN.begin(); - for(; it != CBN.end(); it++) { - LinearFactor::shared_ptr lf(new LinearFactor(*it)); - push_back(lf); - } -} - -/* ************************************************************************* */ -/* find the separators */ /* ************************************************************************* */ set LinearFactorGraph::find_separator(const string& key) const { @@ -51,35 +37,18 @@ set LinearFactorGraph::find_separator(const string& key) const return separator; } -/* ************************************************************************* */ -// eliminate factor graph using the given (not necessarily complete) -// ordering, yielding a chordal Bayes net and partially eliminated FG /* ************************************************************************* */ GaussianBayesNet::shared_ptr -LinearFactorGraph::eliminate_partially(const Ordering& ordering) +LinearFactorGraph::eliminate(const Ordering& ordering) { GaussianBayesNet::shared_ptr chordalBayesNet (new GaussianBayesNet()); // empty - BOOST_FOREACH(string key, ordering) { ConditionalGaussian::shared_ptr cg = eliminateOne(key); chordalBayesNet->push_back(cg); } - return chordalBayesNet; } -/* ************************************************************************* */ -/** eliminate factor graph in the given order, yielding a chordal Bayes net */ -/* ************************************************************************* */ -GaussianBayesNet::shared_ptr -LinearFactorGraph::eliminate(const Ordering& ordering) -{ - GaussianBayesNet::shared_ptr chordalBayesNet = eliminate_partially(ordering); - return chordalBayesNet; -} - -/* ************************************************************************* */ -/** optimize the linear factor graph */ /* ************************************************************************* */ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering) { @@ -92,8 +61,6 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering) return *newConfig; } -/* ************************************************************************* */ -/** combine two factor graphs */ /* ************************************************************************* */ void LinearFactorGraph::combine(const LinearFactorGraph &lfg){ for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ @@ -102,11 +69,9 @@ void LinearFactorGraph::combine(const LinearFactorGraph &lfg){ } /* ************************************************************************* */ -/** combine two factor graphs */ -/* ************************************************************************* */ - LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, const LinearFactorGraph& lfg2) { + // create new linear factor graph equal to the first one LinearFactorGraph fg = lfg1; @@ -115,13 +80,11 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, != lfg2.factors_.end(); factor++) { fg.push_back(*factor); } - return fg; } /* ************************************************************************* */ -// find all variables and their dimensions VariableSet LinearFactorGraph::variables() const { VariableSet result; BOOST_FOREACH(shared_factor factor,factors_) { diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index e37a17fab..8f7d3a08e 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -55,12 +55,6 @@ namespace gtsam { return exp(-0.5 * error(c)); } - /** - * given a chordal bayes net, sets the linear factor graph identical to that CBN - * FD: imperative !! - */ - void setCBN(const GaussianBayesNet& CBN); - /** * find the separator, i.e. all the nodes that have at least one * common factor with the given node. FD: not used AFAIK. @@ -69,15 +63,10 @@ namespace gtsam { /** * eliminate factor graph in place(!) in the given order, yielding - * a chordal Bayes net - */ - boost::shared_ptr eliminate(const Ordering& ordering); - - /** - * Same as eliminate but allows for passing an incomplete ordering + * a chordal Bayes net. Allows for passing an incomplete ordering * that does not completely eliminate the graph */ - boost::shared_ptr eliminate_partially(const Ordering& ordering); + boost::shared_ptr eliminate(const Ordering& ordering); /** * optimize a linear factor graph diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index b4860da79..aabed4e4f 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -18,6 +18,7 @@ using namespace boost::assign; #include "Ordering.h" #include "smallExample.h" #include "GaussianBayesNet.h" +#include // needed for FactorGraph::eliminate using namespace gtsam; @@ -394,10 +395,17 @@ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet ) Ordering ord; ord += "x2","l1","x1"; GaussianBayesNet::shared_ptr CBN = fg.eliminate(ord); + + // True LinearFactorGraph LinearFactorGraph fg2(*CBN); GaussianBayesNet::shared_ptr CBN2 = fg2.eliminate(ord); - CHECK(CBN->equals(*CBN2)); + + // Base FactorGraph only + FactorGraph fg3(*CBN); + boost::shared_ptr > CBN3 = + fg3.eliminate(ord); + CHECK(CBN->equals(*CBN3)); } /* ************************************************************************* */