diff --git a/cpp/SymbolicFactor.h b/cpp/SymbolicFactor.h index 0f71af289..f9ebb1317 100644 --- a/cpp/SymbolicFactor.h +++ b/cpp/SymbolicFactor.h @@ -29,18 +29,32 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; - /** - * Construct from SymbolicConditional - */ + /** Construct from SymbolicConditional */ SymbolicFactor(const boost::shared_ptr& c); - /** - * Constructor from a list of keys - */ + /** Constructor from a list of keys */ SymbolicFactor(std::list keys) : keys_(keys) { } + /** Construct unary factor */ + SymbolicFactor(const std::string& key) { + keys_.push_back(key); + } + + /** Construct binary factor */ + SymbolicFactor(const std::string& key1, const std::string& key2) { + keys_.push_back(key1); + keys_.push_back(key2); + } + + /** Construct ternary factor */ + SymbolicFactor(const std::string& key1, const std::string& key2, const std::string& key3) { + keys_.push_back(key1); + keys_.push_back(key2); + keys_.push_back(key3); + } + /** * Constructor that combines a set of factors * @param factors Set of factors to combine diff --git a/cpp/SymbolicFactorGraph.h b/cpp/SymbolicFactorGraph.h index e0b1e4e80..4f760a107 100644 --- a/cpp/SymbolicFactorGraph.h +++ b/cpp/SymbolicFactorGraph.h @@ -22,10 +22,25 @@ namespace gtsam { class SymbolicFactorGraph: public FactorGraph { public: - /** - * Construct empty factor graph - */ - SymbolicFactorGraph() { + /** Construct empty factor graph */ + SymbolicFactorGraph() {} + + /** Push back unary factor */ + void push_factor(const std::string& key) { + boost::shared_ptr factor(new SymbolicFactor(key)); + push_back(factor); + } + + /** Push back binary factor */ + void push_factor(const std::string& key1, const std::string& key2) { + boost::shared_ptr factor(new SymbolicFactor(key1,key2)); + push_back(factor); + } + + /** Push back ternary factor */ + void push_factor(const std::string& key1, const std::string& key2, const std::string& key3) { + boost::shared_ptr factor(new SymbolicFactor(key1,key2,key3)); + push_back(factor); } /** diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 2fa274d99..10089bc11 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -73,7 +73,6 @@ TEST( BayesTree, constructor ) ASIA.push_back(L); ASIA.push_back(B); SymbolicBayesTree bayesTree2(ASIA); - //bayesTree2.print("bayesTree2"); // Check whether the same CHECK(assert_equal(bayesTree,bayesTree2)); @@ -304,14 +303,15 @@ TEST( BayesTree, balanced_smoother_joint ) } /* ************************************************************************* * - Bayes Tree, for testing conversion to a forest of orphans needed for incremental. +Bayes Tree for testing conversion to a forest of orphans needed for incremental. A,B C|A E|B D|C F|E /* ************************************************************************* */ TEST( BayesTree, removePath ) { - SymbolicConditional::shared_ptr A(new SymbolicConditional("A")), + SymbolicConditional::shared_ptr + A(new SymbolicConditional("A")), B(new SymbolicConditional("B", "A")), C(new SymbolicConditional("C", "A")), D(new SymbolicConditional("D", "C")), @@ -325,34 +325,26 @@ TEST( BayesTree, removePath ) bayesTree.insert(E); bayesTree.insert(F); - // remove C, expected outcome: factor graph with ABC, Bayes Tree now contains two orphan trees: D|C and E|B,F|E + // remove C, expected outcome: factor graph with ABC, + // Bayes Tree now contains two orphan trees: D|C and E|B,F|E SymbolicFactorGraph expected; - list keys; - keys += "A","C"; - boost::shared_ptr _CA(new SymbolicFactor(keys)); - expected.push_back(_CA); - keys.clear(); - keys += "A","B"; - boost::shared_ptr _BA(new SymbolicFactor(keys)); - expected.push_back(_BA); - keys.clear(); - keys += "A"; - boost::shared_ptr _A(new SymbolicFactor(keys)); - expected.push_back(_A); + expected.push_factor("A","C"); + expected.push_factor("A","B"); + expected.push_factor("A"); + SymbolicFactorGraph actual = bayesTree.removePath("C"); CHECK(assert_equal(expected, actual)); // remove A, nothing should happen (already removed) SymbolicFactorGraph expected2; // empty factor + actual = bayesTree.removePath("A"); // CHECK(assert_equal(expected2, actual)); // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected3; - keys.clear(); - keys += "C","A"; - boost::shared_ptr CA(new SymbolicFactor(keys)); - expected3.push_back(CA); + expected3.push_factor("C","A"); + actual = bayesTree.removePath("E"); // CHECK(assert_equal(expected3, actual)); } diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index e02de0c55..d5dd4cd5d 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -23,24 +23,12 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) { // construct expected symbolic graph SymbolicFactorGraph expected; + expected.push_factor("x1"); + expected.push_factor("x1","x2"); + expected.push_factor("l1","x1"); + expected.push_factor("l1","x2"); - list f1_keys; f1_keys += "x1"; - SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys)); - expected.push_back(f1); - - list f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2"); - SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys)); - expected.push_back(f2); - - list f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1"); - SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys)); - expected.push_back(f3); - - list f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2"); - SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys)); - expected.push_back(f4); - - // construct it from the factor graph graph + // construct it from the factor graph GaussianFactorGraph factorGraph = createGaussianFactorGraph(); SymbolicFactorGraph actual(factorGraph); @@ -97,10 +85,8 @@ TEST( SymbolicFactorGraph, removeAndCombineFactors ) // combine all factors connected to x1 SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); - list keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2"); - SymbolicFactor expected(keys); - - // check if the two factors are the same + // check result + SymbolicFactor expected("l1","x1","x2"); CHECK(assert_equal(expected,*actual)); } @@ -149,20 +135,10 @@ TEST( GaussianFactorGraph, eliminate ) TEST( SymbolicFactorGraph, constructFromBayesNet ) { // create expected factor graph - FactorGraph expected; - - list f1_keys; f1_keys += "l1","x1","x2"; - SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys)); - expected.push_back(f1); - - list f2_keys; f2_keys += "x1","l1"; - SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys)); - expected.push_back(f2); - - list f3_keys; f3_keys += "x1"; - SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys)); - expected.push_back(f3); - + SymbolicFactorGraph expected; + expected.push_factor("l1","x1","x2"); + expected.push_factor("x1","l1"); + expected.push_factor("x1"); // create Bayes Net SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1")); @@ -177,7 +153,7 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) // create actual factor graph from a Bayes Net FactorGraph actual(bayesNet); - CHECK(assert_equal(expected,actual)); + CHECK(assert_equal((FactorGraph)expected,actual)); } /* ************************************************************************* */ @@ -186,25 +162,16 @@ TEST( SymbolicFactorGraph, push_back ) // Create two factor graphs and expected combined graph SymbolicFactorGraph fg1, fg2, expected; - list f1_keys; f1_keys += "x1"; - SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys)); - fg1.push_back(f1); - expected.push_back(f1); + fg1.push_factor("x1"); + fg1.push_factor("x1","x2"); - list f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2"); - SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys)); - fg1.push_back(f2); - expected.push_back(f2); + fg2.push_factor("l1","x1"); + fg2.push_factor("l1","x2"); - list f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1"); - SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys)); - fg2.push_back(f3); - expected.push_back(f3); - - list f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2"); - SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys)); - fg2.push_back(f4); - expected.push_back(f4); + expected.push_factor("x1"); + expected.push_factor("x1","x2"); + expected.push_factor("l1","x1"); + expected.push_factor("l1","x2"); // combine SymbolicFactorGraph actual = combine(fg1,fg2);