From eed9cf1e5c43a5792e392b196b55ead8b7a803bd Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 18 Feb 2013 18:26:36 +0000 Subject: [PATCH] Fixed permutation function in BayesTree to properly update the nodes listing. Added assertion to removeClique() to fail under malformed bayes trees. --- .cproject | 362 +++++++++++++----------- gtsam/inference/BayesTree-inl.h | 20 ++ gtsam/inference/BayesTree.h | 6 +- gtsam/inference/Permutation.h | 13 +- gtsam/inference/tests/testBayesTree.cpp | 103 ++++++- 5 files changed, 315 insertions(+), 189 deletions(-) diff --git a/.cproject b/.cproject index 5eb64684d..b76924d5b 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -309,14 +307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +333,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +341,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +389,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +397,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +405,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +421,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -519,22 +523,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -551,6 +539,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -575,34 +579,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -687,26 +683,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -887,6 +891,14 @@ false true + + make + -j5 + testBayesTree.run + true + true + true + make -j2 @@ -1049,6 +1061,7 @@ make + testGraph.run true false @@ -1056,6 +1069,7 @@ make + testJunctionTree.run true false @@ -1063,6 +1077,7 @@ make + testSymbolicBayesNetB.run true false @@ -1222,6 +1237,7 @@ make + testErrors.run true false @@ -1267,14 +1283,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1355,6 +1363,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1677,7 +1693,6 @@ make - testSimulated2DOriented.run true false @@ -1717,7 +1732,6 @@ make - testSimulated2D.run true false @@ -1725,7 +1739,6 @@ make - testSimulated3D.run true false @@ -1957,7 +1970,6 @@ make - tests/testGaussianISAM2 true false @@ -1979,102 +1991,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2276,6 +2192,7 @@ cpack + -G DEB true false @@ -2283,6 +2200,7 @@ cpack + -G RPM true false @@ -2290,6 +2208,7 @@ cpack + -G TGZ true false @@ -2297,6 +2216,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2422,34 +2342,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2493,6 +2477,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 131a3813f..e4ca4a9c1 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -201,6 +201,25 @@ namespace gtsam { return new_clique; } + /* ************************************************************************* */ + template + void BayesTree::permuteWithInverse(const Permutation& inversePermutation) { + // recursively permute the cliques and internal conditionals + if (root_) + root_->permuteWithInverse(inversePermutation); + + // need to know what the largest key is to get the right number of cliques + Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end()); + + // Update the nodes structure + typename BayesTree::Nodes newNodes(maxIndex+1); +// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation + for(size_t i = 0; i < nodes_.size(); ++i) + newNodes[inversePermutation[i]] = nodes_[i]; + + nodes_ = newNodes; + } + /* ************************************************************************* */ template inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { @@ -242,6 +261,7 @@ namespace gtsam { child->parent_ = typename Clique::weak_ptr(); BOOST_FOREACH(Index j, (*clique->conditional())) { + assert(j < nodes_.size()); // Don't want to overrun nodes list nodes_[j].reset(); } } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8bb28865f..458e5fef2 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -236,10 +236,8 @@ namespace gtsam { root_->deleteCachedShortcuts(); } - /** Apply a permutation to all cliques */ - void permuteWithInverse(const Permutation& inversePermutation) { - root_->permuteWithInverse(inversePermutation); - } + /** Apply a permutation to the full tree - also updates the nodes structure */ + void permuteWithInverse(const Permutation& inversePermutation); /** * Remove path from clique to root and return that path as factors diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 8139bdd28..984267282 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -189,7 +189,9 @@ protected: namespace internal { - // An internal class used for storing and applying a permutation from a map + /** + * An internal class used for storing and applying a permutation from a map + */ class Reduction : public gtsam::FastMap { public: typedef gtsam::FastMap Base; @@ -205,8 +207,9 @@ namespace internal { bool equals(const Reduction& other, double tol = 1e-9) const; }; - // Reduce the variable indices so that those in the set are mapped to start at zero + /** + * Reduce the variable indices so that those in the set are mapped to start at zero + */ Permutation createReducingPermutation(const std::set& indices); -} - -} +} // \namespace internal +} // \namespace gtsam diff --git a/gtsam/inference/tests/testBayesTree.cpp b/gtsam/inference/tests/testBayesTree.cpp index a29b609a5..10a3232af 100644 --- a/gtsam/inference/tests/testBayesTree.cpp +++ b/gtsam/inference/tests/testBayesTree.cpp @@ -19,6 +19,7 @@ #include // for operator += #include +#include #include using namespace boost::assign; @@ -309,8 +310,6 @@ TEST( BayesTree, shortcutCheck ) // } } - - /* ************************************************************************* */ TEST( BayesTree, removeTop ) { @@ -408,6 +407,99 @@ TEST( BayesTree, removeTop3 ) 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 + IndexConditional::shared_ptr + A(new IndexConditional(_A_)), + B(new IndexConditional(_B_, _A_)), + C(new IndexConditional(_C_, _A_)), + D(new IndexConditional(_D_, _C_)), + E(new IndexConditional(_E_, _B_)), + F(new IndexConditional(_F_, _E_)); + SymbolicBayesTree bayesTreeReduced; + SymbolicBayesTree::insert(bayesTreeReduced, A); + SymbolicBayesTree::insert(bayesTreeReduced, B); + SymbolicBayesTree::insert(bayesTreeReduced, C); + SymbolicBayesTree::insert(bayesTreeReduced, D); + SymbolicBayesTree::insert(bayesTreeReduced, E); + SymbolicBayesTree::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 + SymbolicBayesTree 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_; + IndexConditional::shared_ptr + expRoot(new IndexConditional(expRootIndices, 2)), // root + A0(new IndexConditional(_A0_)), + B0(new IndexConditional(_B0_, _A0_)), + C0(new IndexConditional(_C0_, _A0_)), // leaf level 1 + D0(new IndexConditional(_D0_, _C0_)), // leaf level 2 + E0(new IndexConditional(_E0_, _B0_)), // leaf level 2 + F0(new IndexConditional(_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()); + + SymbolicBayesTree expFullTree; + SymbolicBayesTree::insert(expFullTree, A0); + SymbolicBayesTree::insert(expFullTree, B0); + SymbolicBayesTree::insert(expFullTree, C0); + SymbolicBayesTree::insert(expFullTree, D0); + SymbolicBayesTree::insert(expFullTree, E0); + SymbolicBayesTree::insert(expFullTree, F0); + + EXPECT(assert_equal(expFullTree, actBayesTree)); +} + ///* ************************************************************************* */ ///** // * x2 - x3 - x4 - x5 @@ -463,10 +555,7 @@ TEST( BayesTree, removeTop3 ) // CHECK(assert_equal(expected, actual)); // //} -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */