diff --git a/cpp/BayesChain-inl.h b/cpp/BayesChain-inl.h index 81788c8e6..4ce5ffca7 100644 --- a/cpp/BayesChain-inl.h +++ b/cpp/BayesChain-inl.h @@ -30,14 +30,14 @@ namespace gtsam { /* ************************************************************************* */ template bool BayesChain::equals(const BayesChain& cbn, double tol) const { - const_iterator it1 = nodes_.begin(), it2 = cbn.nodes_.begin(); - if(nodes_.size() != cbn.nodes_.size()) return false; - for(; it1 != nodes_.end(); it1++, it2++) { - const string& j1 = it1->first, j2 = it2->first; - boost::shared_ptr node1 = it1->second, node2 = it2->second; - if (j1 != j2) return false; - if (!node1->equals(*node2,tol)) - return false; + if(size() != cbn.size()) return false; + if(keys_ != cbn.keys_) return false; + string key; + boost::shared_ptr node; + FOREACH_PAIR( key, node, nodes_) { + const_iterator cg = cbn.nodes_.find(key); + if (cg == nodes_.end()) return false; + if (!equals_star(node,cg->second,tol)) return false; } return true; } diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 89ceb5253..fa202057f 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -15,7 +15,6 @@ #include "ChordalBayesNet.h" #include "FactorGraph-inl.h" #include "LinearFactorGraph.h" -#include "SymbolicBayesChain-inl.h" using namespace std; using namespace gtsam; @@ -23,10 +22,6 @@ using namespace gtsam; // Explicitly instantiate so we don't have to include everywhere template class FactorGraph; -// explicitly instantiate conversion from LinearFG to SymbolicFG -template SymbolicBayesChain::SymbolicBayesChain - (FactorGraph const&, Ordering const&); - /* ************************************************************************* */ LinearFactorGraph::LinearFactorGraph(const ChordalBayesNet& CBN) { diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index fc587bc45..59326e8cf 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -5,39 +5,23 @@ */ #include -#include //Added for linux compatibility +#include #include "Ordering.h" using namespace std; using namespace gtsam; /* ************************************************************************* */ -void Ordering::print() const { - std::cout << "Ordering:" << std::endl; - printf("Ordering size: %d \n", (int)this->size()); - - for(size_t i = 0; i < this->size(); i++) - cout << (*this)[i] << " "; - +void Ordering::print(const string& s) const { + cout << s; + BOOST_FOREACH(string key, *this) + cout << " " << key; cout << endl; } /* ************************************************************************* */ -bool Ordering::equals(Ordering &ord){ - if(this->size() != ord.size()) - return false; - - vector::iterator key; - vector::iterator ord_key; - for(key = this->begin(); key != this->end(); key++){ - for(ord_key = ord.begin(); ord_key != ord.end(); ord_key++){ - if(strcmp((*key).c_str(), (*ord_key).c_str()) == 0) - break; - if(key == this->end()) - return false; - } - } - return true; +bool Ordering::equals(const Ordering &other, double tol) const { + return *this == other; } /* ************************************************************************* */ diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 05283e344..946f4e60b 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -6,8 +6,9 @@ #pragma once -#include +#include #include +#include "Testable.h" namespace gtsam { @@ -15,7 +16,7 @@ namespace gtsam { * @class Ordering * @brief ordering of indices for eliminating a factor graph */ - class Ordering: public std::vector { + class Ordering: public std::list, public Testable { public: /** * Default constructor creates empty ordering @@ -25,20 +26,19 @@ namespace gtsam { /** * Copy constructor from string vector - * TODO: should take reference? */ - Ordering(std::vector strings_in) : - std::vector(strings_in) { + Ordering(const std::list& strings_in) : + std::list(strings_in) { } - void print() const; + void print(const std::string& s = "Ordering") const; /** * check if two orderings are the same * @param ordering * @return bool */ - bool equals(Ordering &ord); + bool equals(const Ordering &ord, double tol=0) const; }; } diff --git a/cpp/SymbolicBayesChain-inl.h b/cpp/SymbolicBayesChain-inl.h deleted file mode 100644 index b783824e9..000000000 --- a/cpp/SymbolicBayesChain-inl.h +++ /dev/null @@ -1,24 +0,0 @@ -/** - * @file SymbolicBayesChain-inl.h - * @brief Template definitions for SymbolicBayesChain - * @author Frank Dellaert - */ - -#include -#include - -#include "SymbolicBayesChain.h" - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - template - SymbolicBayesChain::SymbolicBayesChain( - const FactorGraph& factorGraph, const Ordering& ordering) { - } - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/cpp/SymbolicBayesChain.h b/cpp/SymbolicBayesChain.h index a8bf8df66..b8b06e8d2 100644 --- a/cpp/SymbolicBayesChain.h +++ b/cpp/SymbolicBayesChain.h @@ -41,13 +41,6 @@ namespace gtsam { SymbolicBayesChain(const std::map& nodes); - /** - * Construct from any factor graph - */ - template - SymbolicBayesChain(const FactorGraph& factorGraph, - const Ordering& ordering); - /** Destructor */ virtual ~SymbolicBayesChain() { } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 6ebdbcae8..f6f9fd118 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -5,15 +5,15 @@ */ #include // for 'insert()' -#include // for operator += +#include // for operator += +using namespace boost::assign; + #include #include "SymbolicBayesChain.h" #include "BayesTree-inl.h" -//using namespace std; using namespace gtsam; -using namespace boost::assign; // Conditionals for ASIA example from the tutorial with A and D evidence SymbolicConditional::shared_ptr diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 36091a18c..fae023c84 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -8,6 +8,9 @@ #include #include +#include // for operator += +using namespace boost::assign; + #include #include "Matrix.h" @@ -450,8 +453,7 @@ TEST( LinearFactor, matrix ) // render with a given ordering Ordering ord; - ord.push_back("x1"); - ord.push_back("x2"); + ord += "x1","x2"; Matrix A; Vector b; boost::tie(A,b) = lf->matrix(ord); diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index a2e48e03c..bb9d40747 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -4,13 +4,15 @@ * @author Christian Potthast **/ -/*STL/C++*/ +#include #include using namespace std; -#include -#include #include +#include // for operator += +using namespace boost::assign; + +#include #include "Matrix.h" #include "smallExample.h" @@ -271,16 +273,14 @@ TEST( LinearFactorGraph, eliminateAll ) ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22)); ChordalBayesNet expected; - expected.insert("x1", cg1); - expected.insert("l1", cg2); expected.insert("x2", cg3); + expected.insert("l1", cg2); + expected.insert("x1", cg1); // Check one ordering LinearFactorGraph fg1 = createLinearFactorGraph(); Ordering ord1; - ord1.push_back("x2"); - ord1.push_back("l1"); - ord1.push_back("x1"); + ord1 += "x2","l1","x1"; ChordalBayesNet::shared_ptr actual = fg1.eliminate(ord1); CHECK(assert_equal(expected,*actual,tol)); } @@ -310,9 +310,7 @@ TEST( LinearFactorGraph, copying ) // now eliminate the copy Ordering ord1; - ord1.push_back("x2"); - ord1.push_back("l1"); - ord1.push_back("x1"); + ord1 += "x2","l1","x1"; ChordalBayesNet::shared_ptr actual1 = copy.eliminate(ord1); // Create the same graph, but not by copying @@ -330,9 +328,7 @@ TEST( LinearFactorGraph, matrix ) // render with a given ordering Ordering ord; - ord.push_back("x2"); - ord.push_back("l1"); - ord.push_back("x1"); + ord += "x2","l1","x1"; Matrix A; Vector b; boost::tie(A,b) = fg.matrix(ord); @@ -361,10 +357,7 @@ TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet ) // render with a given ordering Ordering ord; - ord.push_back("x2"); - ord.push_back("l1"); - ord.push_back("x1"); - + ord += "x2","l1","x1"; ChordalBayesNet::shared_ptr CBN = fg.eliminate(ord); LinearFactorGraph fg2(*CBN); ChordalBayesNet::shared_ptr CBN2 = fg2.eliminate(ord); @@ -375,11 +368,11 @@ TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet ) /* ************************************************************************* */ TEST( LinearFactorGraph, GET_ORDERING) { + Ordering expected; + expected += "l1","x1","x2"; LinearFactorGraph fg = createLinearFactorGraph(); - Ordering ord = fg.getOrdering(); - CHECK(ord[0] == string("l1")); - CHECK(ord[1] == string("x1")); - CHECK(ord[2] == string("x2")); + Ordering actual = fg.getOrdering(); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index d9024985c..66c965de0 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -7,10 +7,14 @@ #include using namespace std; +#include // for operator += +using namespace boost::assign; + #include #include "Matrix.h" #include "smallExample.h" + // template definitions #include "NonlinearFactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" @@ -43,27 +47,21 @@ TEST( NonlinearOptimizer, delta ) // Check one ordering Ordering ord1; - ord1.push_back("x2"); - ord1.push_back("l1"); - ord1.push_back("x1"); + ord1 += "x2","l1","x1"; Optimizer optimizer1(fg, ord1, initial); VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual1,expected)); // Check another Ordering ord2; - ord2.push_back("x1"); - ord2.push_back("x2"); - ord2.push_back("l1"); + ord2 += "x1","x2","l1"; Optimizer optimizer2(fg, ord2, initial); VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual2,expected)); // And yet another... Ordering ord3; - ord3.push_back("l1"); - ord3.push_back("x1"); - ord3.push_back("x2"); + ord3 += "l1","x1","x2"; Optimizer optimizer3(fg, ord3, initial); VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual3,expected)); diff --git a/cpp/testSymbolicBayesChain.cpp b/cpp/testSymbolicBayesChain.cpp index 44fafc59d..daae3e1e3 100644 --- a/cpp/testSymbolicBayesChain.cpp +++ b/cpp/testSymbolicBayesChain.cpp @@ -5,13 +5,14 @@ */ #include // for 'insert()' -#include // for operator += +#include // for operator += using namespace boost::assign; #include #include "smallExample.h" #include "SymbolicBayesChain.h" +#include "SymbolicFactorGraph.h" using namespace std; using namespace gtsam; @@ -21,21 +22,24 @@ TEST( SymbolicBayesChain, constructor ) { // Create manually SymbolicConditional::shared_ptr - x2(new SymbolicConditional("x1", "l1")), + x2(new SymbolicConditional("l1", "x1")), l1(new SymbolicConditional("x1")), x1(new SymbolicConditional()); - map nodes; - insert(nodes)("x2", x2)("l1", l1)("x1", x1); - SymbolicBayesChain expected(nodes); + SymbolicBayesChain expected; + expected.insert("x2",x2); + expected.insert("l1",l1); + expected.insert("x1",x1); // Create from a factor graph + LinearFactorGraph factorGraph = createLinearFactorGraph(); + SymbolicFactorGraph fg(factorGraph); + + // eliminate it Ordering ordering; ordering += "x2","l1","x1"; - LinearFactorGraph factorGraph = createLinearFactorGraph(); - SymbolicBayesChain actual(factorGraph, ordering); - CHECK(assert_equal(expected, actual)); + SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering); - //bayesChain.ordering(); + CHECK(assert_equal(expected, *actual)); } /* ************************************************************************* */ diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 05115c838..076b6e6c4 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -4,6 +4,9 @@ * @author Frank Dellaert */ +#include // for operator += +using namespace boost::assign; + #include #include "smallExample.h" @@ -19,7 +22,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // construct expected symbolic graph SymbolicFactorGraph expected; - list f1_keys; f1_keys.push_back("x1"); + list f1_keys; f1_keys += "x1"; SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys)); expected.push_back(f1); @@ -120,14 +123,14 @@ TEST( LinearFactorGraph, eliminateOne ) TEST( LinearFactorGraph, eliminate ) { // create expected Chordal bayes Net - SymbolicConditional::shared_ptr c1(new SymbolicConditional()); - SymbolicConditional::shared_ptr c2(new SymbolicConditional("x1")); - SymbolicConditional::shared_ptr c3(new SymbolicConditional("l1", "x1")); + SymbolicConditional::shared_ptr x2(new SymbolicConditional("l1", "x1")); + SymbolicConditional::shared_ptr l1(new SymbolicConditional("x1")); + SymbolicConditional::shared_ptr x1(new SymbolicConditional()); SymbolicBayesChain expected; - expected.insert("x1", c1); - expected.insert("l1", c2); - expected.insert("x2", c3); + expected.insert("x2", x2); + expected.insert("l1", l1); + expected.insert("x1", x1); // create a test graph LinearFactorGraph factorGraph = createLinearFactorGraph(); @@ -135,9 +138,7 @@ TEST( LinearFactorGraph, eliminate ) // eliminate it Ordering ordering; - ordering.push_back("x2"); - ordering.push_back("l1"); - ordering.push_back("x1"); + ordering += "x2","l1","x1"; SymbolicBayesChain::shared_ptr actual = fg.eliminate(ordering); CHECK(assert_equal(expected,*actual));