diff --git a/.cproject b/.cproject index 318dedccb..98eedc90b 100644 --- a/.cproject +++ b/.cproject @@ -322,14 +322,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -356,6 +348,7 @@ make + tests/testBayesTree.run true false @@ -363,6 +356,7 @@ make + testBinaryBayesNet.run true false @@ -410,6 +404,7 @@ make + testSymbolicBayesNet.run true false @@ -417,6 +412,7 @@ make + tests/testSymbolicFactor.run true false @@ -424,6 +420,7 @@ make + testSymbolicFactorGraph.run true false @@ -439,11 +436,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -478,7 +484,6 @@ make - testGraph.run true false @@ -574,7 +579,6 @@ make - testInference.run true false @@ -582,7 +586,6 @@ make - testGaussianBayesNet.run true false @@ -590,7 +593,6 @@ make - testGaussianFactor.run true false @@ -598,7 +600,6 @@ make - testJunctionTree.run true false @@ -606,7 +607,6 @@ make - testSymbolicBayesNet.run true false @@ -614,7 +614,6 @@ make - testSymbolicFactorGraph.run true false @@ -820,6 +819,14 @@ true true + + make + + testGaussianISAM.run + true + true + true + make -j2 @@ -918,6 +925,7 @@ make + testErrors.run true false @@ -1253,7 +1261,6 @@ make - testSimulated2DOriented.run true false @@ -1293,7 +1300,6 @@ make - testSimulated2D.run true false @@ -1301,7 +1307,6 @@ make - testSimulated3D.run true false @@ -1349,7 +1354,6 @@ make - tests/testGaussianISAM2 true false @@ -1371,6 +1375,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -1467,86 +1551,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 @@ -1901,14 +1905,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1935,6 +1931,7 @@ make + tests/testBayesTree.run true false @@ -1942,6 +1939,7 @@ make + testBinaryBayesNet.run true false @@ -1989,6 +1987,7 @@ make + testSymbolicBayesNet.run true false @@ -1996,6 +1995,7 @@ make + tests/testSymbolicFactor.run true false @@ -2003,6 +2003,7 @@ make + testSymbolicFactorGraph.run true false @@ -2018,11 +2019,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2057,7 +2067,6 @@ make - testGraph.run true false @@ -2153,7 +2162,6 @@ make - testInference.run true false @@ -2161,7 +2169,6 @@ make - testGaussianBayesNet.run true false @@ -2169,7 +2176,6 @@ make - testGaussianFactor.run true false @@ -2177,7 +2183,6 @@ make - testJunctionTree.run true false @@ -2185,7 +2190,6 @@ make - testSymbolicBayesNet.run true false @@ -2193,7 +2197,6 @@ make - testSymbolicFactorGraph.run true false @@ -2399,6 +2402,14 @@ true true + + make + + testGaussianISAM.run + true + true + true + make -j2 @@ -2497,6 +2508,7 @@ make + testErrors.run true false @@ -2832,7 +2844,6 @@ make - testSimulated2DOriented.run true false @@ -2872,7 +2883,6 @@ make - testSimulated2D.run true false @@ -2880,7 +2890,6 @@ make - testSimulated3D.run true false @@ -2928,7 +2937,6 @@ make - tests/testGaussianISAM2 true false @@ -2950,6 +2958,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -3046,86 +3134,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - make -j2 diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 7c5f91eb7..22c12e00b 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -16,6 +16,7 @@ #include #include #include +#include #include using namespace boost::assign; namespace lam = boost::lambda; @@ -248,67 +249,85 @@ namespace gtsam { return stats; } -// /* ************************************************************************* */ -// // The shortcut density is a conditional P(S|R) of the separator of this -// // clique on the root. We can compute it recursively from the parent shortcut -// // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p -// // TODO, why do we actually return a shared pointer, why does eliminate? -// /* ************************************************************************* */ -// template -// template -// BayesNet -// BayesTree::Clique::shortcut(shared_ptr R) { -// // A first base case is when this clique or its parent is the root, -// // in which case we return an empty Bayes net. -// -// if (R.get()==this || parent_==R) { -// BayesNet empty; -// return empty; -// } -// -// // The parent clique has a Conditional for each frontal node in Fp -// // so we can obtain P(Fp|Sp) in factor graph form -// FactorGraph p_Fp_Sp(*parent_); -// -// // If not the base case, obtain the parent shortcut P(Sp|R) as factors -// FactorGraph p_Sp_R(parent_->shortcut(R)); -// -// // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) -// FactorGraph p_Cp_R = combine(p_Fp_Sp, p_Sp_R); -// -// // Eliminate into a Bayes net with ordering designed to integrate out -// // any variables not in *our* separator. Variables to integrate out must be -// // eliminated first hence the desired ordering is [Cp\S S]. -// // However, an added wrinkle is that Cp might overlap with the root. -// // Keys corresponding to the root should not be added to the ordering at all. -// -// // Get the key list Cp=Fp+Sp, which will form the basis for the integrands -// Ordering integrands = parent_->keys(); -// -// // Start ordering with the separator -// Ordering ordering = separator_; -// -// // remove any variables in the root, after this integrands = Cp\R, ordering = S\R -// BOOST_FOREACH(varid_t key, R->ordering()) { -// integrands.remove(key); -// ordering.remove(key); -// } -// -// // remove any variables in the separator, after this integrands = Cp\R\S -// BOOST_FOREACH(varid_t key, separator_) integrands.remove(key); -// -// // form the ordering as [Cp\R\S S\R] -// BOOST_REVERSE_FOREACH(varid_t key, integrands) ordering.push_front(key); -// -// // eliminate to get marginal -// BayesNet p_S_R = eliminate(p_Cp_R,ordering); -// -// // remove all integrands -// BOOST_FOREACH(varid_t key, integrands) p_S_R.pop_front(); -// -// // return the parent shortcut P(Sp|R) -// return p_S_R; -// } + /* ************************************************************************* */ + // The shortcut density is a conditional P(S|R) of the separator of this + // clique on the root. We can compute it recursively from the parent shortcut + // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p + // TODO, why do we actually return a shared pointer, why does eliminate? + /* ************************************************************************* */ + template + template + BayesNet + BayesTree::Clique::shortcut(shared_ptr R) { + // A first base case is when this clique or its parent is the root, + // in which case we return an empty Bayes net. + + sharedClique parent(parent_.lock()); + + if (R.get()==this || parent==R) { + BayesNet empty; + return empty; + } + + // The parent clique has a Conditional for each frontal node in Fp + // so we can obtain P(Fp|Sp) in factor graph form + FactorGraph p_Fp_Sp(*parent); + + // If not the base case, obtain the parent shortcut P(Sp|R) as factors + FactorGraph p_Sp_R(parent->shortcut(R)); + + // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) + FactorGraph p_Cp_R = combine(p_Fp_Sp, p_Sp_R); + + // Eliminate into a Bayes net with ordering designed to integrate out + // any variables not in *our* separator. Variables to integrate out must be + // eliminated first hence the desired ordering is [Cp\S S]. + // However, an added wrinkle is that Cp might overlap with the root. + // Keys corresponding to the root should not be added to the ordering at all. + + typedef set, boost::fast_pool_allocator > FastJSet; + + // Get the key list Cp=Fp+Sp, which will form the basis for the integrands + vector parentKeys(parent->keys()); + FastJSet integrands(parentKeys.begin(), parentKeys.end()); + + // Start ordering with the separator + FastJSet separator(separator_.begin(), separator_.end()); + + // remove any variables in the root, after this integrands = Cp\R, ordering = S\R + BOOST_FOREACH(varid_t key, R->ordering()) { + integrands.erase(key); + separator.erase(key); + } + + // remove any variables in the separator, after this integrands = Cp\R\S + BOOST_FOREACH(varid_t key, separator_) integrands.erase(key); + + // form the ordering as [Cp\R\S S\R] + vector ordering; ordering.reserve(integrands.size() + separator.size()); + BOOST_FOREACH(varid_t key, integrands) ordering.push_back(key); + BOOST_FOREACH(varid_t key, separator) ordering.push_back(key); + + // eliminate to get marginal + typename FactorGraph::variableindex_type varIndex(p_Cp_R); + Permutation toFront = Permutation::PullToFront(ordering, varIndex.size()); + Permutation::shared_ptr toFrontInverse(toFront.inverse()); + BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, p_Cp_R) { + factor->permuteWithInverse(*toFrontInverse); + } + varIndex.permute(toFront); + BayesNet p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); + + // remove all integrands + for(varid_t j=0; j -// BayesNet shortcut(shared_ptr root); -// + /** return the conditional P(S|Root) on the separator given the root */ + // TODO: create a cached version + template + BayesNet shortcut(shared_ptr root); + // /** return the marginal P(C) of the clique */ // template // FactorGraph marginal(shared_ptr root); diff --git a/inference/FactorGraph-inl.h b/inference/FactorGraph-inl.h index dabb138bd..fb79554bf 100644 --- a/inference/FactorGraph-inl.h +++ b/inference/FactorGraph-inl.h @@ -359,11 +359,10 @@ namespace gtsam { // } /* ************************************************************************* */ - template - FactorGraph combine(const FactorGraph& fg1, - const FactorGraph& fg2) { + template + FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { // create new linear factor graph equal to the first one - FactorGraph fg = fg1; + FactorGraph fg = fg1; // add the second factors_ in the graph fg.push_back(fg2); diff --git a/inference/FactorGraph.h b/inference/FactorGraph.h index 738ffc3bc..1919a7b6d 100644 --- a/inference/FactorGraph.h +++ b/inference/FactorGraph.h @@ -188,8 +188,8 @@ namespace gtsam { * @param const &fg2 Linear factor graph * @return a new combined factor graph */ - template - FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2); + template + FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2); // /** // * Extract and combine all the factors that involve a given node @@ -214,7 +214,10 @@ namespace gtsam { FactorGraph::FactorGraph(const Derived& factorGraph) { factors_.reserve(factorGraph.size()); BOOST_FOREACH(const typename Derived::sharedFactor& factor, factorGraph) { - this->push_back(factor); + if(factor) + this->push_back(sharedFactor(new Factor(*factor))); + else + this->push_back(sharedFactor()); } } diff --git a/inference/Makefile.am b/inference/Makefile.am index 1d4f69669..e1c020d83 100644 --- a/inference/Makefile.am +++ b/inference/Makefile.am @@ -24,9 +24,8 @@ check_PROGRAMS += tests/testVariableIndex tests/testVariableSlots # Inference headers += inference-inl.h VariableSlots-inl.h -sources += inference.cpp VariableSlots.cpp +sources += inference.cpp VariableSlots.cpp Permutation.cpp headers += graph.h graph-inl.h -headers += Permutation.h headers += VariableIndex.h headers += FactorGraph.h FactorGraph-inl.h headers += ClusterTree.h ClusterTree-inl.h diff --git a/inference/Permutation.cpp b/inference/Permutation.cpp new file mode 100644 index 000000000..b88a3fe63 --- /dev/null +++ b/inference/Permutation.cpp @@ -0,0 +1,88 @@ +/** + * @file Permutation.cpp + * @brief + * @author Richard Roberts + * @created Oct 9, 2010 + */ + +#include + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +Permutation Permutation::Identity(varid_t nVars) { + Permutation ret(nVars); + for(varid_t i=0; i& toFront, size_t size) { + + Permutation ret(size); + + // Mask of which variables have been pulled, used to reorder + vector pulled(size, false); + + for(varid_t j=0; jsize())); + for(varid_t i=0; isize(); ++i) { + assert((*this)[i] < this->size()); + (*result)[(*this)[i]] = i; + } + return result; +} + +/* ************************************************************************* */ +void Permutation::print(const std::string& str) const { + std::cout << str; + BOOST_FOREACH(varid_t s, rangeIndices_) { std::cout << s << " "; } + std::cout << std::endl; +} + +} diff --git a/inference/Permutation.h b/inference/Permutation.h index 50454313a..4b6964bde 100644 --- a/inference/Permutation.h +++ b/inference/Permutation.h @@ -10,7 +10,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -81,6 +81,12 @@ public: */ static Permutation Identity(varid_t nVars); + /** + * Create a permutation that pulls the given variables to the front while + * pushing the rest to the back. + */ + static Permutation PullToFront(const std::vector& toFront, size_t size); + iterator begin() { return rangeIndices_.begin(); } const_iterator begin() const { return rangeIndices_.begin(); } iterator end() { return rangeIndices_.end(); } @@ -180,52 +186,4 @@ public: }; -/* ************************************************************************* */ -inline Permutation Permutation::Identity(varid_t nVars) { - Permutation ret(nVars); - for(varid_t i=0; isize())); - for(varid_t i=0; isize(); ++i) { - assert((*this)[i] < this->size()); - (*result)[(*this)[i]] = i; - } - return result; -} - -/* ************************************************************************* */ -inline void Permutation::print(const std::string& str) const { - std::cout << str; - BOOST_FOREACH(varid_t s, rangeIndices_) { std::cout << s << " "; } - std::cout << std::endl; -} - } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 567a18fb9..db96dc507 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -93,51 +93,50 @@ C3 x4 : x5 C4 x3 : x4 C5 x2 : x3 C6 x1 : x2 -/* ************************************************************************* */ -// SL-FIX TEST( BayesTree, linear_smoother_shortcuts ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// Ordering ordering; -// for (int t = 1; t <= 7; t++) -// ordering.push_back(symbol('x', t)); -// -// // eliminate using the "natural" ordering -// GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); -// -// // Create the Bayes tree -// GaussianISAM bayesTree(chordalBayesNet); -// LONGS_EQUAL(6,bayesTree.size()); -// -// // Check the conditional P(Root|Root) -// GaussianBayesNet empty; -// GaussianISAM::sharedClique R = bayesTree.root(); -// GaussianBayesNet actual1 = R->shortcut(R); -// CHECK(assert_equal(empty,actual1,tol)); -// -// // Check the conditional P(C2|Root) -// GaussianISAM::sharedClique C2 = bayesTree["x5"]; -// GaussianBayesNet actual2 = C2->shortcut(R); -// CHECK(assert_equal(empty,actual2,tol)); -// -// // Check the conditional P(C3|Root) -// double sigma3 = 0.61808; -// Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); -// GaussianBayesNet expected3; -// push_front(expected3,"x5", zero(2), eye(2)/sigma3, "x6", A56/sigma3, ones(2)); -// GaussianISAM::sharedClique C3 = bayesTree["x4"]; -// GaussianBayesNet actual3 = C3->shortcut(R); -// CHECK(assert_equal(expected3,actual3,tol)); -// -// // Check the conditional P(C4|Root) -// double sigma4 = 0.661968; -// Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); -// GaussianBayesNet expected4; -// push_front(expected4,"x4", zero(2), eye(2)/sigma4, "x6", A46/sigma4, ones(2)); -// GaussianISAM::sharedClique C4 = bayesTree["x3"]; -// GaussianBayesNet actual4 = C4->shortcut(R); -// CHECK(assert_equal(expected4,actual4,tol)); -//} +**************************************************************************** */ +TEST( BayesTree, linear_smoother_shortcuts ) +{ + // Create smoother with 7 nodes + Ordering ordering; + GaussianFactorGraph smoother; + boost::tie(smoother, ordering) = createSmoother(7); + + // eliminate using the "natural" ordering + GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); + + // Create the Bayes tree + GaussianISAM bayesTree(chordalBayesNet); + LONGS_EQUAL(6,bayesTree.size()); + + // Check the conditional P(Root|Root) + GaussianBayesNet empty; + GaussianISAM::sharedClique R = bayesTree.root(); + GaussianBayesNet actual1 = R->shortcut(R); + CHECK(assert_equal(empty,actual1,tol)); + + // Check the conditional P(C2|Root) + GaussianISAM::sharedClique C2 = bayesTree[ordering["x5"]]; + GaussianBayesNet actual2 = C2->shortcut(R); + CHECK(assert_equal(empty,actual2,tol)); + + // Check the conditional P(C3|Root) + double sigma3 = 0.61808; + Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); + GaussianBayesNet expected3; + push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2)); + GaussianISAM::sharedClique C3 = bayesTree[ordering["x4"]]; + GaussianBayesNet actual3 = C3->shortcut(R); + CHECK(assert_equal(expected3,actual3,tol)); + + // Check the conditional P(C4|Root) + double sigma4 = 0.661968; + Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); + GaussianBayesNet expected4; + push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2)); + GaussianISAM::sharedClique C4 = bayesTree[ordering["x3"]]; + GaussianBayesNet actual4 = C4->shortcut(R); + CHECK(assert_equal(expected4,actual4,tol)); +} /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: @@ -157,7 +156,7 @@ C6 x1 : x2 C3 x1 : x2 C4 x7 : x6 -/* ************************************************************************* */ +************************************************************************* */ // SL-FIX TEST( BayesTree, balanced_smoother_marginals ) //{ // // Create smoother with 7 nodes @@ -208,35 +207,35 @@ C6 x1 : x2 //} /* ************************************************************************* */ -// SL-FIX TEST( BayesTree, balanced_smoother_shortcuts ) -//{ -// // Create smoother with 7 nodes -// GaussianFactorGraph smoother = createSmoother(7); -// Ordering ordering; -// ordering += "x1","x3","x5","x7","x2","x6","x4"; -// -// // Create the Bayes tree -// GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); -// GaussianISAM bayesTree(chordalBayesNet); -// -// // Check the conditional P(Root|Root) -// GaussianBayesNet empty; -// GaussianISAM::sharedClique R = bayesTree.root(); -// GaussianBayesNet actual1 = R->shortcut(R); -// CHECK(assert_equal(empty,actual1,tol)); -// -// // Check the conditional P(C2|Root) -// GaussianISAM::sharedClique C2 = bayesTree["x3"]; -// GaussianBayesNet actual2 = C2->shortcut(R); -// CHECK(assert_equal(empty,actual2,tol)); -// -// // Check the conditional P(C3|Root), which should be equal to P(x2|x4) -// GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; -// GaussianBayesNet expected3; expected3.push_back(p_x2_x4); -// GaussianISAM::sharedClique C3 = bayesTree["x1"]; -// GaussianBayesNet actual3 = C3->shortcut(R); -// CHECK(assert_equal(expected3,actual3,tol)); -//} +TEST( BayesTree, balanced_smoother_shortcuts ) +{ + // Create smoother with 7 nodes + Ordering ordering; + ordering += "x1","x3","x5","x7","x2","x6","x4"; + GaussianFactorGraph smoother = createSmoother(7, ordering).first; + + // Create the Bayes tree + GaussianBayesNet chordalBayesNet = *Inference::Eliminate(smoother); + GaussianISAM bayesTree(chordalBayesNet); + + // Check the conditional P(Root|Root) + GaussianBayesNet empty; + GaussianISAM::sharedClique R = bayesTree.root(); + GaussianBayesNet actual1 = R->shortcut(R); + CHECK(assert_equal(empty,actual1,tol)); + + // Check the conditional P(C2|Root) + GaussianISAM::sharedClique C2 = bayesTree[ordering["x3"]]; + GaussianBayesNet actual2 = C2->shortcut(R); + CHECK(assert_equal(empty,actual2,tol)); + + // Check the conditional P(C3|Root), which should be equal to P(x2|x4) + GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet[ordering["x2"]]; + GaussianBayesNet expected3; expected3.push_back(p_x2_x4); + GaussianISAM::sharedClique C3 = bayesTree[ordering["x1"]]; + GaussianBayesNet actual3 = C3->shortcut(R); + CHECK(assert_equal(expected3,actual3,tol)); +} /* ************************************************************************* */ // SL-FIX TEST( BayesTree, balanced_smoother_clique_marginals )