From 7f3d62eccd93fcf13b12cf07d9fa14cf0fefe77f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:55 +0000 Subject: [PATCH] Copied over all existing inference and symbolic unit tests that should now be symbolic. More work on multifrontal elimination. --- .../inference/EliminateableFactorGraph-inst.h | 77 +- gtsam/inference/EliminateableFactorGraph.h | 13 +- gtsam/inference/JunctionTreeUnordered-inst.h | 6 +- gtsam/inference/VariableIndexUnordered-inl.h | 6 +- gtsam/inference/tests/testBayesTree.cpp | 2 + gtsam/symbolic/SymbolicFactorUnordered.h | 6 + gtsam/symbolic/tests/testConditional.cpp0 | 106 +++ gtsam/symbolic/tests/testFactorGraph.cpp0 | 65 ++ ...JunctionTree.cpp => testJunctionTree.cpp0} | 0 .../symbolic/tests/testSymbolicBayesTree.cpp | 826 ++++++++++++++++++ gtsam/symbolic/tests/testSymbolicFactor.cpp0 | 113 +++ .../tests/testVariableIndexUnordered.cpp | 51 +- 12 files changed, 1205 insertions(+), 66 deletions(-) create mode 100644 gtsam/symbolic/tests/testConditional.cpp0 create mode 100644 gtsam/symbolic/tests/testFactorGraph.cpp0 rename gtsam/symbolic/tests/{testSymbolicJunctionTree.cpp => testJunctionTree.cpp0} (100%) create mode 100644 gtsam/symbolic/tests/testSymbolicBayesTree.cpp create mode 100644 gtsam/symbolic/tests/testSymbolicFactor.cpp0 rename gtsam/{inference => symbolic}/tests/testVariableIndexUnordered.cpp (66%) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 56017a327..47f314ed7 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -29,25 +29,30 @@ namespace gtsam { boost::shared_ptr EliminateableFactorGraph:: eliminateSequential( - const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const + const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const { - // Do elimination - std::pair, boost::shared_ptr > result; - if(ordering) { - // Do elimination with given ordering - result = ELIMINATIONTREE(asDerived(), variableIndex, *ordering).eliminate(function); - } else { - // Compute ordering - OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex); - result = ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering).eliminate(function); + if(ordering && variableIndex) { + // Do elimination + std::pair, boost::shared_ptr > result = + ELIMINATIONTREE(asDerived(), *variableIndex, *ordering).eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!result.second->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes net + return result.first; + } + else if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check + // for no variable index first so that it's always computed if we need to call COLAMD because + // no Ordering is provided. + return eliminateSequential(function, ordering, VariableIndexUnordered(asDerived())); + } + else /*if(!ordering)*/ { + // If no Ordering provided, compute one and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in the previous 'else' + // block. + return eliminateSequential(function, OrderingUnordered::COLAMD(*variableIndex)); } - - // If any factors are remaining, the ordering was incomplete - if(!result.second->empty()) - throw InconsistentEliminationRequested(); - - // Return the Bayes net - return result.first; } /* ************************************************************************* */ @@ -56,25 +61,31 @@ namespace gtsam { boost::shared_ptr EliminateableFactorGraph:: eliminateMultifrontal( - const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const + const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const { - // Do elimination - std::pair, boost::shared_ptr > result; - if(ordering) { + if(ordering && variableIndex) { + // Do elimination + std::pair, boost::shared_ptr > result; // Do elimination with given ordering - result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, *ordering)).eliminate(function); - } else { - // Compute ordering - OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex); - result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering)).eliminate(function); + result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, *ordering)).eliminate(function); + // If any factors are remaining, the ordering was incomplete + if(!result.second->empty()) + throw InconsistentEliminationRequested(); + // Return the Bayes tree + return result.first; + } + else if(!variableIndex) { + // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check + // for no variable index first so that it's always computed if we need to call COLAMD because + // no Ordering is provided. + return eliminateMultifrontal(function, ordering, VariableIndexUnordered(asDerived())); + } + else /*if(!ordering)*/ { + // If no Ordering provided, compute one and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in the previous 'else' + // block. + return eliminateMultifrontal(function, OrderingUnordered::COLAMD(*variableIndex)); } - - // If any factors are remaining, the ordering was incomplete - if(!result.second->empty()) - throw InconsistentEliminationRequested(); - - // Return the Bayes tree - return result.first; } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 81d426999..b37f4c2da 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -36,6 +36,7 @@ namespace gtsam { public: typedef EliminateableFactorGraph This; typedef boost::optional OptionalOrdering; + typedef boost::optional OptionalVariableIndex; typedef boost::function, boost::shared_ptr >( std::vector >, std::vector)> Eliminate; ///< Typedef for an eliminate subroutine @@ -63,7 +64,7 @@ namespace gtsam { * */ boost::shared_ptr eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -87,7 +88,7 @@ namespace gtsam { * */ boost::shared_ptr eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, @@ -95,7 +96,7 @@ namespace gtsam { * B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + OptionalVariableIndex variableIndex = boost::none); /** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) @@ -103,7 +104,7 @@ namespace gtsam { * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialSequential(const Eliminate& function, const std::vector& variables, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + OptionalVariableIndex variableIndex = boost::none); /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) @@ -111,7 +112,7 @@ namespace gtsam { * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + OptionalVariableIndex variableIndex = boost::none); /** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) @@ -119,7 +120,7 @@ namespace gtsam { * \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal(const Eliminate& function, const std::vector& variables, - const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + OptionalVariableIndex variableIndex = boost::none); private: diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index ccce28a8a..b41663fc3 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -87,7 +87,8 @@ namespace gtsam { // Store symbolic elimination results in the parent myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + if(!symbolicElimResult.second->empty()) + myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); // Merge our children if they are in our clique - if our conditional has exactly one fewer // parent than our child's conditional. @@ -173,7 +174,8 @@ namespace gtsam { myData.bayesTreeNode->conditional_ = eliminationResult.first; // Store remaining factor in parent's gathered factors - myData.parentData->childFactors.push_back(eliminationResult.second); + if(!eliminationResult.second->empty()) + myData.parentData->childFactors.push_back(eliminationResult.second); } } diff --git a/gtsam/inference/VariableIndexUnordered-inl.h b/gtsam/inference/VariableIndexUnordered-inl.h index 4b7cbfd2a..480e42fee 100644 --- a/gtsam/inference/VariableIndexUnordered-inl.h +++ b/gtsam/inference/VariableIndexUnordered-inl.h @@ -59,9 +59,9 @@ void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, c if(i >= factors.size()) throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); if(factors[i]) { - BOOST_FOREACH(Key j, factors[i]) { + BOOST_FOREACH(Key j, *factors[i]) { Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[i]); + Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); if(entry == factorEntries.end()) throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); factorEntries.erase(entry); @@ -75,7 +75,7 @@ void VariableIndexUnordered::remove(ITERATOR firstFactor, ITERATOR lastFactor, c template void VariableIndexUnordered::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { for(ITERATOR key = firstKey; key != lastKey; ++key) { - assert(!internalAt(*key).empty()); + assert(internalAt(*key).empty()); index_.erase(*key); } } diff --git a/gtsam/inference/tests/testBayesTree.cpp b/gtsam/inference/tests/testBayesTree.cpp index 4943dedb4..702c8682a 100644 --- a/gtsam/inference/tests/testBayesTree.cpp +++ b/gtsam/inference/tests/testBayesTree.cpp @@ -92,6 +92,8 @@ TEST( BayesTree, constructor ) // Create using insert SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + bayesTree.print("bayesTree (ordered): "); + // Check Size LONGS_EQUAL(4,bayesTree.size()); EXPECT(!bayesTree.empty()); diff --git a/gtsam/symbolic/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h index 7515fab09..aca3ca63f 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -92,6 +92,12 @@ namespace gtsam { /// @} + /// @name Standard Interface + /// @{ + + /** Whether the factor is empty (involves zero variables). */ + bool empty() const { return keys_.empty(); } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/symbolic/tests/testConditional.cpp0 b/gtsam/symbolic/tests/testConditional.cpp0 new file mode 100644 index 000000000..5ee695bb5 --- /dev/null +++ b/gtsam/symbolic/tests/testConditional.cpp0 @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testConditional.cpp + * @brief Unit tests for IndexConditional class + * @author Frank Dellaert + */ + +#include // for operator += +#include // for operator += +using namespace boost::assign; + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( IndexConditional, empty ) +{ + IndexConditional c0; + LONGS_EQUAL(0,c0.nrFrontals()) + LONGS_EQUAL(0,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, noParents ) +{ + IndexConditional c0(0); + LONGS_EQUAL(1,c0.nrFrontals()) + LONGS_EQUAL(0,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, oneParents ) +{ + IndexConditional c0(0,1); + LONGS_EQUAL(1,c0.nrFrontals()) + LONGS_EQUAL(1,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, twoParents ) +{ + IndexConditional c0(0,1,2); + LONGS_EQUAL(1,c0.nrFrontals()) + LONGS_EQUAL(2,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, threeParents ) +{ + IndexConditional c0(0,1,2,3); + LONGS_EQUAL(1,c0.nrFrontals()) + LONGS_EQUAL(3,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, fourParents ) +{ + vector parents; + parents += 1,2,3,4; + IndexConditional c0(0,parents); + LONGS_EQUAL(1,c0.nrFrontals()) + LONGS_EQUAL(4,c0.nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, FromRange ) +{ + vector keys; + keys += 1,2,3,4,5; + IndexConditional::shared_ptr c0(new IndexConditional(keys,2)); + LONGS_EQUAL(2,c0->nrFrontals()) + LONGS_EQUAL(3,c0->nrParents()) +} + +/* ************************************************************************* */ +TEST( IndexConditional, equals ) +{ + IndexConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); + CHECK(c0.equals(c1)); + CHECK(c1.equals(c0)); + CHECK(!c0.equals(c2)); + CHECK(!c2.equals(c0)); + CHECK(!c0.equals(c3)); + CHECK(!c3.equals(c0)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testFactorGraph.cpp0 b/gtsam/symbolic/tests/testFactorGraph.cpp0 new file mode 100644 index 000000000..43402d2cb --- /dev/null +++ b/gtsam/symbolic/tests/testFactorGraph.cpp0 @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testFactorgraph.cpp + * @brief Unit tests for IndexFactor Graphs + * @author Christian Potthast + **/ + +/*STL/C++*/ +#include +#include +#include +#include +#include // for operator += +#include +using namespace boost::assign; + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(FactorGraph, eliminateFrontals) { + + SymbolicFactorGraph sfgOrig; + sfgOrig.push_factor(0,1); + sfgOrig.push_factor(0,2); + sfgOrig.push_factor(1,3); + sfgOrig.push_factor(1,4); + sfgOrig.push_factor(2,3); + sfgOrig.push_factor(4,5); + + IndexConditional::shared_ptr actualCond; + SymbolicFactorGraph actualSfg; + boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); + + vector condIndices; + condIndices += 0,1,2,3,4; + IndexConditional expectedCond(condIndices, 2); + + SymbolicFactorGraph expectedSfg; + expectedSfg.push_factor(2,3); + expectedSfg.push_factor(4,5); + expectedSfg.push_factor(2,3,4); + + EXPECT(assert_equal(expectedSfg, actualSfg)); + EXPECT(assert_equal(expectedCond, *actualCond)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testJunctionTree.cpp0 similarity index 100% rename from gtsam/symbolic/tests/testSymbolicJunctionTree.cpp rename to gtsam/symbolic/tests/testJunctionTree.cpp0 diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp new file mode 100644 index 000000000..c55851a9e --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -0,0 +1,826 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testSymbolicBayesTreeUnordered.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + */ + +#include + +#include +#include +using namespace boost::assign; + +#include + +using namespace std; +using namespace gtsam; + +static bool debug = false; + +/* ************************************************************************* */ +// Conditionals for ASIA example from the tutorial with A and D evidence +static const Key _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; +static SymbolicConditionalUnordered::shared_ptr + B(new SymbolicConditionalUnordered(_B_)), + L(new SymbolicConditionalUnordered(_L_, _B_)), + E(new SymbolicConditionalUnordered(_E_, _L_, _B_)), + S(new SymbolicConditionalUnordered(_S_, _L_, _B_)), + T(new SymbolicConditionalUnordered(_T_, _E_, _L_)), + X(new SymbolicConditionalUnordered(_X_, _E_)); + +// Cliques +static SymbolicConditionalUnordered::shared_ptr ELB( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(list_of(_E_)(_L_)(_B_), 3))); + +// Bayes Tree for Asia example +static SymbolicBayesTreeUnordered createAsiaSymbolicBayesTree() { + SymbolicFactorGraphUnordered asiaGraph; + asiaGraph.push_factor(_T_); + asiaGraph.push_factor(_S_); + asiaGraph.push_factor(_T_, _E_, _L_); + asiaGraph.push_factor(_L_, _S_); + asiaGraph.push_factor(_S_, _B_); + asiaGraph.push_factor(_E_, _B_); + asiaGraph.push_factor(_E_, _X_); + OrderingUnordered asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_; + SymbolicBayesTreeUnordered bayesTree = *asiaGraph.eliminateMultifrontal(EliminateSymbolicUnordered, asiaOrdering); + return bayesTree; +} + +/* ************************************************************************* */ +TEST_UNSAFE( BayesTree, constructor ) +{ + // Create using insert + SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); + + bayesTree.print("bayesTree: "); + + // Check Size +// LONGS_EQUAL(4, bayesTree.size()); +// EXPECT(!bayesTree.empty()); +// +// // Check root +// boost::shared_ptr actual_root = bayesTree.root()->conditional(); +// CHECK(assert_equal(*ELB,*actual_root)); +// +// // Create from symbolic Bayes chain in which we want to discover cliques +// BayesNet ASIA; +// ASIA.push_back(X); +// ASIA.push_back(T); +// ASIA.push_back(S); +// ASIA.push_back(E); +// ASIA.push_back(L); +// ASIA.push_back(B); +// SymbolicBayesTreeUnordered bayesTree2(ASIA); +// +// // Check whether the same +// CHECK(assert_equal(bayesTree,bayesTree2)); +// +// // CHECK findParentClique, should *not depend on order of parents* +//// Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_; +//// IndexTable index(ordering); +// +// list parents1; parents1 += _E_, _L_; +// CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1))); +// +// list parents2; parents2 += _L_, _E_; +// CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2))); +// +// list parents3; parents3 += _L_, _B_; +// CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3))); +} + +///* ************************************************************************* */ +//TEST(BayesTree, clear) +//{ +//// SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); +//// bayesTree.clear(); +//// +//// SymbolicBayesTreeUnordered expected; +//// +//// // Check whether cleared BayesTree is equal to a new BayesTree +//// CHECK(assert_equal(expected, bayesTree)); +//} +// +///* ************************************************************************* * +//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 ) +//{ +// const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; +// SymbolicConditionalUnordered::shared_ptr +// A(new SymbolicConditionalUnordered(_A_)), +// B(new SymbolicConditionalUnordered(_B_, _A_)), +// C(new SymbolicConditionalUnordered(_C_, _A_)), +// D(new SymbolicConditionalUnordered(_D_, _C_)), +// E(new SymbolicConditionalUnordered(_E_, _B_)), +// F(new SymbolicConditionalUnordered(_F_, _E_)); +// SymbolicBayesTreeUnordered bayesTree; +// EXPECT(bayesTree.empty()); +//// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; +// SymbolicBayesTreeUnordered::insert(bayesTree, A); +// SymbolicBayesTreeUnordered::insert(bayesTree, B); +// SymbolicBayesTreeUnordered::insert(bayesTree, C); +// SymbolicBayesTreeUnordered::insert(bayesTree, D); +// SymbolicBayesTreeUnordered::insert(bayesTree, E); +// SymbolicBayesTreeUnordered::insert(bayesTree, F); +// +// // remove C, expected outcome: factor graph with ABC, +// // Bayes Tree now contains two orphan trees: D|C and E|B,F|E +// SymbolicFactorGraph expected; +// expected.push_factor(_B_,_A_); +//// expected.push_factor(_A_); +// expected.push_factor(_C_,_A_); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans; +// expectedOrphans += bayesTree[_D_], bayesTree[_E_]; +// +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// bayesTree.removePath(bayesTree[_C_], bn, orphans); +// SymbolicFactorGraph factors(bn); +// CHECK(assert_equal((SymbolicFactorGraph)expected, factors)); +// CHECK(assert_equal(expectedOrphans, orphans)); +// +// // remove E: factor graph with EB; E|B removed from second orphan tree +// SymbolicFactorGraph expected2; +// expected2.push_factor(_E_,_B_); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans2; +// expectedOrphans2 += bayesTree[_F_]; +// +// BayesNet bn2; +// SymbolicBayesTreeUnordered::Cliques orphans2; +// bayesTree.removePath(bayesTree[_E_], bn2, orphans2); +// SymbolicFactorGraph factors2(bn2); +// CHECK(assert_equal((SymbolicFactorGraph)expected2, factors2)); +// CHECK(assert_equal(expectedOrphans2, orphans2)); +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, removePath2 ) +//{ +// SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); +// +// // Call remove-path with clique B +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// bayesTree.removePath(bayesTree[_B_], bn, orphans); +// SymbolicFactorGraph factors(bn); +// +// // Check expected outcome +// SymbolicFactorGraph expected; +// expected.push_factor(_E_,_L_,_B_); +//// expected.push_factor(_L_,_B_); +//// expected.push_factor(_B_); +// CHECK(assert_equal(expected, factors)); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans; +// expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; +// CHECK(assert_equal(expectedOrphans, orphans)); +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, removePath3 ) +//{ +// SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); +// +// // Call remove-path with clique S +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// bayesTree.removePath(bayesTree[_S_], bn, orphans); +// SymbolicFactorGraph factors(bn); +// +// // Check expected outcome +// SymbolicFactorGraph expected; +// expected.push_factor(_E_,_L_,_B_); +//// expected.push_factor(_L_,_B_); +//// expected.push_factor(_B_); +// expected.push_factor(_S_,_L_,_B_); +// CHECK(assert_equal(expected, factors)); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans; +// expectedOrphans += bayesTree[_T_], bayesTree[_X_]; +// CHECK(assert_equal(expectedOrphans, orphans)); +//} +// +//void getAllCliques(const SymbolicBayesTreeUnordered::sharedClique& subtree, SymbolicBayesTreeUnordered::Cliques& cliques) { +// // Check if subtree exists +// if (subtree) { +// cliques.push_back(subtree); +// // Recursive call over all child cliques +// BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& childClique, subtree->children()) { +// getAllCliques(childClique,cliques); +// } +// } +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, shortcutCheck ) +//{ +// const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; +// SymbolicConditionalUnordered::shared_ptr +// A(new SymbolicConditionalUnordered(_A_)), +// B(new SymbolicConditionalUnordered(_B_, _A_)), +// C(new SymbolicConditionalUnordered(_C_, _A_)), +// D(new SymbolicConditionalUnordered(_D_, _C_)), +// E(new SymbolicConditionalUnordered(_E_, _B_)), +// F(new SymbolicConditionalUnordered(_F_, _E_)), +// G(new SymbolicConditionalUnordered(_G_, _F_)); +// SymbolicBayesTreeUnordered bayesTree; +//// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; +// SymbolicBayesTreeUnordered::insert(bayesTree, A); +// SymbolicBayesTreeUnordered::insert(bayesTree, B); +// SymbolicBayesTreeUnordered::insert(bayesTree, C); +// SymbolicBayesTreeUnordered::insert(bayesTree, D); +// SymbolicBayesTreeUnordered::insert(bayesTree, E); +// SymbolicBayesTreeUnordered::insert(bayesTree, F); +// SymbolicBayesTreeUnordered::insert(bayesTree, G); +// +// //bayesTree.print("BayesTree"); +// //bayesTree.saveGraph("BT1.dot"); +// +// SymbolicBayesTreeUnordered::sharedClique rootClique= bayesTree.root(); +// //rootClique->printTree(); +// SymbolicBayesTreeUnordered::Cliques allCliques; +// getAllCliques(rootClique,allCliques); +// +// BayesNet bn; +// BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) { +// //clique->print("Clique#"); +// bn = clique->shortcut(rootClique, &EliminateSymbolic); +// //bn.print("Shortcut:\n"); +// //cout << endl; +// } +// +// // Check if all the cached shortcuts are cleared +// rootClique->deleteCachedShortcuts(); +// BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) { +// bool notCleared = clique->cachedSeparatorMarginal(); +// CHECK( notCleared == false); +// } +// EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals()); +// +//// BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) { +//// clique->print("Clique#"); +//// if(clique->cachedShortcut()){ +//// bn = clique->cachedShortcut().get(); +//// bn.print("Shortcut:\n"); +//// } +//// else +//// cout << "Not Initialized" << endl; +//// cout << endl; +//// } +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, removeTop ) +//{ +// SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); +// +// // create a new factor to be inserted +// boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); +// +// // Remove the contaminated part of the Bayes tree +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// list keys; keys += _B_,_S_; +// bayesTree.removeTop(keys, bn, orphans); +// SymbolicFactorGraph factors(bn); +// +// // Check expected outcome +// SymbolicFactorGraph expected; +// expected.push_factor(_E_,_L_,_B_); +//// expected.push_factor(_L_,_B_); +//// expected.push_factor(_B_); +// expected.push_factor(_S_,_L_,_B_); +// CHECK(assert_equal(expected, factors)); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans; +// expectedOrphans += bayesTree[_T_], bayesTree[_X_]; +// CHECK(assert_equal(expectedOrphans, orphans)); +// +// // Try removeTop again with a factor that should not change a thing +// boost::shared_ptr newFactor2(new IndexFactor(_B_)); +// BayesNet bn2; +// SymbolicBayesTreeUnordered::Cliques orphans2; +// keys.clear(); keys += _B_; +// bayesTree.removeTop(keys, bn2, orphans2); +// SymbolicFactorGraph factors2(bn2); +// SymbolicFactorGraph expected2; +// CHECK(assert_equal(expected2, factors2)); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans2; +// CHECK(assert_equal(expectedOrphans2, orphans2)); +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, removeTop2 ) +//{ +// SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); +// +// // create two factors to be inserted +// SymbolicFactorGraph newFactors; +// newFactors.push_factor(_B_); +// newFactors.push_factor(_S_); +// +// // Remove the contaminated part of the Bayes tree +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// list keys; keys += _B_,_S_; +// bayesTree.removeTop(keys, bn, orphans); +// SymbolicFactorGraph factors(bn); +// +// // Check expected outcome +// SymbolicFactorGraph expected; +// expected.push_factor(_E_,_L_,_B_); +//// expected.push_factor(_L_,_B_); +//// expected.push_factor(_B_); +// expected.push_factor(_S_,_L_,_B_); +// CHECK(assert_equal(expected, factors)); +// SymbolicBayesTreeUnordered::Cliques expectedOrphans; +// expectedOrphans += bayesTree[_T_], bayesTree[_X_]; +// CHECK(assert_equal(expectedOrphans, orphans)); +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, removeTop3 ) +//{ +// const Index _x4_=5, _l5_=6; +// // simple test case that failed after COLAMD was fixed/activated +// SymbolicConditionalUnordered::shared_ptr +// X(new SymbolicConditionalUnordered(_l5_)), +// A(new SymbolicConditionalUnordered(_x4_, _l5_)), +// B(new SymbolicConditionalUnordered(_x2_, _x4_)), +// C(new SymbolicConditionalUnordered(_x3_, _x2_)); +// +//// Ordering newOrdering; +//// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; +// SymbolicBayesTreeUnordered bayesTree; +// SymbolicBayesTreeUnordered::insert(bayesTree, X); +// SymbolicBayesTreeUnordered::insert(bayesTree, A); +// SymbolicBayesTreeUnordered::insert(bayesTree, B); +// SymbolicBayesTreeUnordered::insert(bayesTree, C); +// +// // remove all +// list keys; +// keys += _l5_, _x2_, _x3_, _x4_; +// BayesNet bn; +// SymbolicBayesTreeUnordered::Cliques orphans; +// bayesTree.removeTop(keys, bn, orphans); +// SymbolicFactorGraph factors(bn); +// +// CHECK(orphans.size() == 0); +//} +// +///* ************************************************************************* */ +//TEST( BayesTree, permute ) +//{ +// // creates a permutation and ensures that the nodes listing is updated +// +// // initial keys - more than just 6 variables - for a system with 9 variables +// const Index _A0_=8, _B0_=7, _C0_=6, _D0_=5, _E0_=4, _F0_=0; +// +// // reduced keys - back to just 6 variables +// const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; +// +// // Create and verify the permutation +// std::set indices; indices += _A0_, _B0_, _C0_, _D0_, _E0_, _F0_; +// Permutation actReducingPermutation = gtsam::internal::createReducingPermutation(indices); +// Permutation expReducingPermutation(6); +// expReducingPermutation[_A_] = _A0_; +// expReducingPermutation[_B_] = _B0_; +// expReducingPermutation[_C_] = _C0_; +// expReducingPermutation[_D_] = _D0_; +// expReducingPermutation[_E_] = _E0_; +// expReducingPermutation[_F_] = _F0_; +// EXPECT(assert_equal(expReducingPermutation, actReducingPermutation)); +// +// // Invert the permutation +// gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation); +// +// // Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables +// SymbolicConditionalUnordered::shared_ptr +// A(new SymbolicConditionalUnordered(_A_)), +// B(new SymbolicConditionalUnordered(_B_, _A_)), +// C(new SymbolicConditionalUnordered(_C_, _A_)), +// D(new SymbolicConditionalUnordered(_D_, _C_)), +// E(new SymbolicConditionalUnordered(_E_, _B_)), +// F(new SymbolicConditionalUnordered(_F_, _E_)); +// SymbolicBayesTreeUnordered bayesTreeReduced; +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, A); +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, B); +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, C); +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, D); +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, E); +// SymbolicBayesTreeUnordered::insert(bayesTreeReduced, F); +// +//// bayesTreeReduced.print("Reduced bayes tree"); +//// P( 4 5) +//// P( 3 | 5) +//// P( 2 | 3) +//// P( 1 | 4) +//// P( 0 | 1) +// +// // Apply the permutation - should add placeholders for variables not present in nodes +// SymbolicBayesTreeUnordered actBayesTree = *bayesTreeReduced.clone(); +// actBayesTree.permuteWithInverse(expReducingPermutation); +// +//// actBayesTree.print("Full bayes tree"); +//// P( 7 8) +//// P( 6 | 8) +//// P( 5 | 6) +//// P( 4 | 7) +//// P( 0 | 4) +// +// // check keys in cliques +// std::vector expRootIndices; expRootIndices += _B0_, _A0_; +// SymbolicConditionalUnordered::shared_ptr +// expRoot(new SymbolicConditionalUnordered(expRootIndices, 2)), // root +// A0(new SymbolicConditionalUnordered(_A0_)), +// B0(new SymbolicConditionalUnordered(_B0_, _A0_)), +// C0(new SymbolicConditionalUnordered(_C0_, _A0_)), // leaf level 1 +// D0(new SymbolicConditionalUnordered(_D0_, _C0_)), // leaf level 2 +// E0(new SymbolicConditionalUnordered(_E0_, _B0_)), // leaf level 2 +// F0(new SymbolicConditionalUnordered(_F0_, _E0_)); // leaf level 3 +// +// CHECK(actBayesTree.root()); +// EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional())); +// EXPECT(assert_equal(*C0, *actBayesTree.root()->children().front()->conditional())); +// EXPECT(assert_equal(*D0, *actBayesTree.root()->children().front()->children().front()->conditional())); +// EXPECT(assert_equal(*E0, *actBayesTree.root()->children().back()->conditional())); +// EXPECT(assert_equal(*F0, *actBayesTree.root()->children().back()->children().front()->conditional())); +// +// // check nodes structure +// LONGS_EQUAL(9, actBayesTree.nodes().size()); +// +// SymbolicBayesTreeUnordered expFullTree; +// SymbolicBayesTreeUnordered::insert(expFullTree, A0); +// SymbolicBayesTreeUnordered::insert(expFullTree, B0); +// SymbolicBayesTreeUnordered::insert(expFullTree, C0); +// SymbolicBayesTreeUnordered::insert(expFullTree, D0); +// SymbolicBayesTreeUnordered::insert(expFullTree, E0); +// SymbolicBayesTreeUnordered::insert(expFullTree, F0); +// +// EXPECT(assert_equal(expFullTree, actBayesTree)); +//} +// +/////* ************************************************************************* */ +/////** +//// * x2 - x3 - x4 - x5 +//// * | / \ | +//// * x1 / \ x6 +//// */ +////TEST( BayesTree, insert ) +////{ +//// // construct bayes tree by split the graph along the separator x3 - x4 +//// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5; +//// SymbolicFactorGraph fg1, fg2, fg3; +//// fg1.push_factor(_x3_, _x4_); +//// fg2.push_factor(_x1_, _x2_); +//// fg2.push_factor(_x2_, _x3_); +//// fg2.push_factor(_x1_, _x3_); +//// fg3.push_factor(_x5_, _x4_); +//// fg3.push_factor(_x6_, _x5_); +//// fg3.push_factor(_x6_, _x4_); +//// +////// Ordering ordering1; ordering1 += _x3_, _x4_; +////// Ordering ordering2; ordering2 += _x1_, _x2_; +////// Ordering ordering3; ordering3 += _x6_, _x5_; +//// +//// BayesNet bn1, bn2, bn3; +//// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1); +//// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1); +//// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1); +//// +//// // insert child cliques +//// SymbolicBayesTreeUnordered actual; +//// list children; +//// SymbolicBayesTreeUnordered::sharedClique r1 = actual.insert(bn2, children); +//// SymbolicBayesTreeUnordered::sharedClique r2 = actual.insert(bn3, children); +//// +//// // insert root clique +//// children.push_back(r1); +//// children.push_back(r2); +//// actual.insert(bn1, children, true); +//// +//// // traditional way +//// SymbolicFactorGraph fg; +//// fg.push_factor(_x3_, _x4_); +//// fg.push_factor(_x1_, _x2_); +//// fg.push_factor(_x2_, _x3_); +//// fg.push_factor(_x1_, _x3_); +//// fg.push_factor(_x5_, _x4_); +//// fg.push_factor(_x6_, _x5_); +//// fg.push_factor(_x6_, _x4_); +//// +////// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; +//// BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); +//// SymbolicBayesTreeUnordered expected(bn); +//// CHECK(assert_equal(expected, actual)); +//// +////} +// +///* ************************************************************************* */ +// +//TEST_UNSAFE( SymbolicBayesTreeUnordered, thinTree ) { +// +// // create a thin-tree Bayesnet, a la Jean-Guillaume +// SymbolicBayesNet bayesNet; +// bayesNet.push_front(boost::make_shared(14)); +// +// bayesNet.push_front(boost::make_shared(13, 14)); +// bayesNet.push_front(boost::make_shared(12, 14)); +// +// bayesNet.push_front(boost::make_shared(11, 13, 14)); +// bayesNet.push_front(boost::make_shared(10, 13, 14)); +// bayesNet.push_front(boost::make_shared(9, 12, 14)); +// bayesNet.push_front(boost::make_shared(8, 12, 14)); +// +// bayesNet.push_front(boost::make_shared(7, 11, 13)); +// bayesNet.push_front(boost::make_shared(6, 11, 13)); +// bayesNet.push_front(boost::make_shared(5, 10, 13)); +// bayesNet.push_front(boost::make_shared(4, 10, 13)); +// +// bayesNet.push_front(boost::make_shared(3, 9, 12)); +// bayesNet.push_front(boost::make_shared(2, 9, 12)); +// bayesNet.push_front(boost::make_shared(1, 8, 12)); +// bayesNet.push_front(boost::make_shared(0, 8, 12)); +// +// if (debug) { +// GTSAM_PRINT(bayesNet); +// bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); +// } +// +// // create a BayesTree out of a Bayes net +// SymbolicBayesTreeUnordered bayesTree(bayesNet); +// if (debug) { +// GTSAM_PRINT(bayesTree); +// bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); +// } +// +// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.root(); +// +// { +// // check shortcut P(S9||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[9]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S8||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[8]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(12, 14)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S4||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(10, 13, 14)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S2||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(12, 14)); +// expected.push_front(boost::make_shared(9, 12, 14)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S0||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[0]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(12, 14)); +// expected.push_front(boost::make_shared(8, 12, 14)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// SymbolicBayesNet::shared_ptr actualJoint; +// +// // Check joint P(8,2) +// if (false) { // TODO, not disjoint +// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(8)); +// expected.push_front(boost::make_shared(2, 8)); +// EXPECT(assert_equal(expected, *actualJoint)); +// } +// +// // Check joint P(1,2) +// if (false) { // TODO, not disjoint +// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(2)); +// expected.push_front(boost::make_shared(1, 2)); +// EXPECT(assert_equal(expected, *actualJoint)); +// } +// +// // Check joint P(2,6) +// if (true) { +// actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(6)); +// expected.push_front(boost::make_shared(2, 6)); +// EXPECT(assert_equal(expected, *actualJoint)); +// } +// +// // Check joint P(4,6) +// if (false) { // TODO, not disjoint +// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(6)); +// expected.push_front(boost::make_shared(4, 6)); +// EXPECT(assert_equal(expected, *actualJoint)); +// } +//} +// +///* ************************************************************************* * +// Bayes tree for smoother with "natural" ordering: +// C1 5 6 +// C2 4 : 5 +// C3 3 : 4 +// C4 2 : 3 +// C5 1 : 2 +// C6 0 : 1 +// **************************************************************************** */ +// +//TEST_UNSAFE( SymbolicBayesTreeUnordered, linear_smoother_shortcuts ) { +// // Create smoother with 7 nodes +// SymbolicFactorGraph smoother; +// smoother.push_factor(0); +// smoother.push_factor(0, 1); +// smoother.push_factor(1, 2); +// smoother.push_factor(2, 3); +// smoother.push_factor(3, 4); +// smoother.push_factor(4, 5); +// smoother.push_factor(5, 6); +// +// BayesNet bayesNet = +// *SymbolicSequentialSolver(smoother).eliminate(); +// +// if (debug) { +// GTSAM_PRINT(bayesNet); +// bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); +// } +// +// // create a BayesTree out of a Bayes net +// SymbolicBayesTreeUnordered bayesTree(bayesNet); +// if (debug) { +// GTSAM_PRINT(bayesTree); +// bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); +// } +// +// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.root(); +// +// { +// // check shortcut P(S2||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S3||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(4, 5)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// { +// // check shortcut P(S4||R) to root +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(3, 5)); +// EXPECT(assert_equal(expected, shortcut)); +// } +//} +// +///* ************************************************************************* */ +//// from testSymbolicJunctionTree, which failed at one point +//TEST(SymbolicBayesTreeUnordered, complicatedMarginal) { +// +// // Create the conditionals to go in the BayesTree +// list L; +// L = list_of(1)(2)(5); +// SymbolicConditionalUnordered::shared_ptr R_1_2(new SymbolicConditionalUnordered(L, 2)); +// L = list_of(3)(4)(6); +// SymbolicConditionalUnordered::shared_ptr R_3_4(new SymbolicConditionalUnordered(L, 2)); +// L = list_of(5)(6)(7)(8); +// SymbolicConditionalUnordered::shared_ptr R_5_6(new SymbolicConditionalUnordered(L, 2)); +// L = list_of(7)(8)(11); +// SymbolicConditionalUnordered::shared_ptr R_7_8(new SymbolicConditionalUnordered(L, 2)); +// L = list_of(9)(10)(11)(12); +// SymbolicConditionalUnordered::shared_ptr R_9_10(new SymbolicConditionalUnordered(L, 2)); +// L = list_of(11)(12); +// SymbolicConditionalUnordered::shared_ptr R_11_12(new SymbolicConditionalUnordered(L, 2)); +// +// // Symbolic Bayes Tree +// typedef SymbolicBayesTreeUnordered::Clique Clique; +// typedef SymbolicBayesTreeUnordered::sharedClique sharedClique; +// +// // Create Bayes Tree +// SymbolicBayesTreeUnordered bt; +// bt.insert(sharedClique(new Clique(R_11_12))); +// bt.insert(sharedClique(new Clique(R_9_10))); +// bt.insert(sharedClique(new Clique(R_7_8))); +// bt.insert(sharedClique(new Clique(R_5_6))); +// bt.insert(sharedClique(new Clique(R_3_4))); +// bt.insert(sharedClique(new Clique(R_1_2))); +// if (debug) { +// GTSAM_PRINT(bt); +// bt.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); +// } +// +// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bt.root(); +// SymbolicBayesNet empty; +// +// // Shortcut on 9 +// { +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[9]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// EXPECT(assert_equal(empty, shortcut)); +// } +// +// // Shortcut on 7 +// { +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[7]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// EXPECT(assert_equal(empty, shortcut)); +// } +// +// // Shortcut on 5 +// { +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[5]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(8, 11)); +// expected.push_front(boost::make_shared(7, 8, 11)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// // Shortcut on 3 +// { +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[3]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(6, 11)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// // Shortcut on 1 +// { +// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[1]; +// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(5, 11)); +// EXPECT(assert_equal(expected, shortcut)); +// } +// +// // Marginal on 5 +// { +// IndexFactor::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic); +// EXPECT(assert_equal(IndexFactor(5), *actual, 1e-1)); +// } +// +// // Shortcut on 6 +// { +// IndexFactor::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic); +// EXPECT(assert_equal(IndexFactor(6), *actual, 1e-1)); +// } +// +//} +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp0 b/gtsam/symbolic/tests/testSymbolicFactor.cpp0 new file mode 100644 index 000000000..a474c09a1 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp0 @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSymbolicFactor.cpp + * @brief Unit tests for a symbolic IndexFactor + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +/* ************************************************************************* */ +TEST(SymbolicFactor, constructor) { + + // Frontals sorted, parents not sorted + vector keys1; keys1 += 3, 4, 5, 9, 7, 8; + (void)IndexConditional(keys1, 3); + +// // Frontals not sorted +// vector keys2; keys2 += 3, 5, 4, 9, 7, 8; +// (void)IndexConditional::FromRange(keys2.begin(), keys2.end(), 3); + +// // Frontals not before parents +// vector keys3; keys3 += 3, 4, 5, 1, 7, 8; +// (void)IndexConditional::FromRange(keys3.begin(), keys3.end(), 3); +} + +/* ************************************************************************* */ +#ifdef TRACK_ELIMINATE +TEST(SymbolicFactor, eliminate) { + vector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; + IndexFactor actual(keys.begin(), keys.end()); + BayesNet fragment = *actual.eliminate(3); + + IndexFactor expected(keys.begin()+3, keys.end()); + IndexConditional::shared_ptr expected0 = IndexConditional::FromRange(keys.begin(), keys.end(), 1); + IndexConditional::shared_ptr expected1 = IndexConditional::FromRange(keys.begin()+1, keys.end(), 1); + IndexConditional::shared_ptr expected2 = IndexConditional::FromRange(keys.begin()+2, keys.end(), 1); + + CHECK(assert_equal(fragment.size(), size_t(3))); + CHECK(assert_equal(expected, actual)); + BayesNet::const_iterator fragmentCond = fragment.begin(); + CHECK(assert_equal(**fragmentCond++, *expected0)); + CHECK(assert_equal(**fragmentCond++, *expected1)); + CHECK(assert_equal(**fragmentCond++, *expected2)); +} +#endif +/* ************************************************************************* */ +TEST(SymbolicFactor, EliminateSymbolic) { + SymbolicFactorGraph factors; + factors.push_factor(2,4,6); + factors.push_factor(1,2,5); + factors.push_factor(0,3); + + IndexFactor expectedFactor(4,5,6); + std::vector keys; keys += 0,1,2,3,4,5,6; + IndexConditional::shared_ptr expectedConditional(new IndexConditional(keys, 4)); + + IndexFactor::shared_ptr actualFactor; + IndexConditional::shared_ptr actualConditional; + boost::tie(actualConditional, actualFactor) = EliminateSymbolic(factors, 4); + + CHECK(assert_equal(*expectedConditional, *actualConditional)); + CHECK(assert_equal(expectedFactor, *actualFactor)); + +// BayesNet expected_bn; +// vector parents; +// +// parents.clear(); parents += 1,2,3,4,5,6; +// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(0, parents))); +// +// parents.clear(); parents += 2,3,4,5,6; +// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(1, parents))); +// +// parents.clear(); parents += 3,4,5,6; +// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(2, parents))); +// +// parents.clear(); parents += 4,5,6; +// expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(3, parents))); +// +// BayesNet::shared_ptr actual_bn; +// IndexFactor::shared_ptr actual_factor; +// boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4); +// +// CHECK(assert_equal(expected_bn, *actual_bn)); +// CHECK(assert_equal(expected_factor, *actual_factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testVariableIndexUnordered.cpp b/gtsam/symbolic/tests/testVariableIndexUnordered.cpp similarity index 66% rename from gtsam/inference/tests/testVariableIndexUnordered.cpp rename to gtsam/symbolic/tests/testVariableIndexUnordered.cpp index 097acdda6..5bc5cbdb9 100644 --- a/gtsam/inference/tests/testVariableIndexUnordered.cpp +++ b/gtsam/symbolic/tests/testVariableIndexUnordered.cpp @@ -16,11 +16,14 @@ * @date Sep 26, 2010 */ +#include +using namespace boost::assign; + #include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -38,10 +41,10 @@ TEST(VariableIndexUnordered, augment) { fg2.push_factor(3, 5); fg2.push_factor(5, 6); - SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); + SymbolicFactorGraphUnordered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); - VariableIndex expected(fgCombined); - VariableIndex actual(fg1); + VariableIndexUnordered expected(fgCombined); + VariableIndexUnordered actual(fg1); actual.augment(fg2); LONGS_EQUAL(16, actual.nEntries()); @@ -50,9 +53,9 @@ TEST(VariableIndexUnordered, augment) { } /* ************************************************************************* */ -TEST(VariableIndex, remove) { +TEST(VariableIndexUnordered, remove) { - SymbolicFactorGraph fg1, fg2; + SymbolicFactorGraphUnordered fg1, fg2; fg1.push_factor(0, 1); fg1.push_factor(0, 2); fg1.push_factor(5, 9); @@ -62,28 +65,30 @@ TEST(VariableIndex, remove) { fg2.push_factor(3, 5); fg2.push_factor(5, 6); - SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); + SymbolicFactorGraphUnordered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); // Create a factor graph containing only the factors from fg2 and with null // factors in the place of those of fg1, so that the factor indices are correct. - SymbolicFactorGraph fg2removed(fgCombined); + SymbolicFactorGraphUnordered fg2removed(fgCombined); fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); - // The expected VariableIndex has the same factor indices as fgCombined but + // The expected VariableIndexUnordered has the same factor indices as fgCombined but // with entries from fg1 removed, and still has all 10 variables. - VariableIndex expected(fg2removed, 10); - VariableIndex actual(fgCombined); + VariableIndexUnordered expected(fg2removed); + VariableIndexUnordered actual(fgCombined); vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - actual.remove(indices, fg1); + actual.remove(indices.begin(), indices.end(), fg1); + std::list unusedVariables; unusedVariables += 0, 9; + actual.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(VariableIndex, deep_copy) { +TEST(VariableIndexUnordered, deep_copy) { - SymbolicFactorGraph fg1, fg2; + SymbolicFactorGraphUnordered fg1, fg2; fg1.push_factor(0, 1); fg1.push_factor(0, 2); fg1.push_factor(5, 9); @@ -93,22 +98,24 @@ TEST(VariableIndex, deep_copy) { fg2.push_factor(3, 5); fg2.push_factor(5, 6); - // Create original graph and VariableIndex - SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); - VariableIndex original(fgOriginal); - VariableIndex expectedOriginal(fgOriginal); + // Create original graph and VariableIndexUnordered + SymbolicFactorGraphUnordered fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); + VariableIndexUnordered original(fgOriginal); + VariableIndexUnordered expectedOriginal(fgOriginal); // Create a factor graph containing only the factors from fg2 and with null // factors in the place of those of fg1, so that the factor indices are correct. - SymbolicFactorGraph fg2removed(fgOriginal); + SymbolicFactorGraphUnordered fg2removed(fgOriginal); fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3); - VariableIndex expectedRemoved(fg2removed); + VariableIndexUnordered expectedRemoved(fg2removed); // Create a clone and modify the clone - the original should not change - VariableIndex clone(original); + VariableIndexUnordered clone(original); vector indices; indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3); - clone.remove(indices, fg1); + clone.remove(indices.begin(), indices.end(), fg1); + std::list unusedVariables; unusedVariables += 0, 9; + clone.removeUnusedVariables(unusedVariables.begin(), unusedVariables.end()); // When modifying the clone, the original should have stayed the same EXPECT(assert_equal(expectedOriginal, original));