diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h index ba63f0345..3e8a2cc6a 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h @@ -17,62 +17,63 @@ #pragma once #include +#include #include namespace gtsam { /* ************************************************************************* */ - template - bool BayesTreeCliqueBaseUnordered::equals( + template + bool BayesTreeCliqueBaseUnordered::equals( const DERIVED& other, double tol = 1e-9) const { return (!conditional_ && !other.conditional()) || conditional_->equals(*other.conditional(), tol); } - ///* ************************************************************************* */ - //template - //std::vector - // BayesTreeCliqueBaseUnordered::separator_setminus_B(derived_ptr B) const - //{ - // FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); - // FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); - // std::vector S_setminus_B; - // std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), - // indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); - // return S_setminus_B; - //} - - ///* ************************************************************************* */ - //template - //std::vector BayesTreeCliqueBaseUnordered::shortcut_indices( - // derived_ptr B, const FactorGraphType& p_Cp_B) const - //{ - // gttic(shortcut_indices); - // FastSet allKeys = p_Cp_B.keys(); - // FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); - // std::vector S_setminus_B = separator_setminus_B(B); - // std::vector keep; - // // keep = S\B intersect allKeys (S_setminus_B is already sorted) - // std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // - // allKeys.begin(), allKeys.end(), back_inserter(keep)); - // // keep += B intersect allKeys - // std::set_intersection(indicesB.begin(), indicesB.end(), // - // allKeys.begin(), allKeys.end(), back_inserter(keep)); - // return keep; - //} + /* ************************************************************************* */ + template + std::vector + BayesTreeCliqueBaseUnordered::separator_setminus_B(const derived_ptr& B) const + { + FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + std::vector S_setminus_B; + std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), + indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); + return S_setminus_B; + } /* ************************************************************************* */ - template - void BayesTreeCliqueBaseUnordered::print( + template + std::vector BayesTreeCliqueBaseUnordered::shortcut_indices( + const derived_ptr& B, const FactorGraphType& p_Cp_B) const + { + gttic(shortcut_indices); + FastSet allKeys = p_Cp_B.keys(); + FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + std::vector S_setminus_B = separator_setminus_B(B); + std::vector keep; + // keep = S\B intersect allKeys (S_setminus_B is already sorted) + std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + // keep += B intersect allKeys + std::set_intersection(indicesB.begin(), indicesB.end(), // + allKeys.begin(), allKeys.end(), back_inserter(keep)); + return keep; + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBaseUnordered::print( const std::string& s, const KeyFormatter& keyFormatter) const { conditional_->print(s, keyFormatter); } /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseUnordered::treeSize() const { + template + size_t BayesTreeCliqueBaseUnordered::treeSize() const { size_t size = 1; BOOST_FOREACH(const derived_ptr& child, children) size += child->treeSize(); @@ -80,8 +81,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - size_t BayesTreeCliqueBaseUnordered::numCachedSeparatorMarginals() const + template + size_t BayesTreeCliqueBaseUnordered::numCachedSeparatorMarginals() const { if (!cachedSeparatorMarginal_) return 0; @@ -98,140 +99,104 @@ namespace gtsam { // 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 /* ************************************************************************* */ - //template - //BayesNetUnordered BayesTreeCliqueBaseUnordered::shortcut( - // derived_ptr B, Eliminate function) const - //{ - // gttic(BayesTreeCliqueBaseUnordered_shortcut); + template + typename BayesTreeCliqueBaseUnordered::BayesNetType + BayesTreeCliqueBaseUnordered::shortcut(const derived_ptr& B, Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_shortcut); + // We only calculate the shortcut when this clique is not B + // and when the S\B is not empty + std::vector S_setminus_B = separator_setminus_B(B); + if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty()) + { + // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBaseUnordered_shortcut); + FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) + gttic(BayesTreeCliqueBaseUnordered_shortcut); + p_Cp_B += parent->conditional_; // P(Fp|Sp) - // // We only calculate the shortcut when this clique is not B - // // and when the S\B is not empty - // std::vector S_setminus_B = separator_setminus_B(B); - // if (B.get() != this && !S_setminus_B.empty()) { + // Determine the variables we want to keepSet, S union B + std::vector keep = shortcut_indices(B, p_Cp_B); - // // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph - // derived_ptr parent(parent_.lock()); - // gttoc(BayesTreeCliqueBaseUnordered_shortcut); - // FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) - // gttic(BayesTreeCliqueBaseUnordered_shortcut); - // p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + // Marginalize out everything except S union B + BayesNetType result = *p_Cp_B.marginalMultifrontalBayesNet( + OrderingUnordered(keep), boost::none, function); - // // Determine the variables we want to keepSet, S union B - // std::vector keep = shortcut_indices(B, p_Cp_B); + // Finally, we only want to have S\B variables in the Bayes net, so + size_t nrFrontals = S_setminus_B.size(); + result.erase(result.begin() + nrFrontals, result.end()); - // // Reduce the variable indices to start at zero - // gttic(Reduce); - // const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); - // internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - // BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp_B) { - // if(factor) factor->reduceWithInverse(inverseReduction); } - // inverseReduction.applyInverse(keep); - // gttoc(Reduce); - - // // Create solver that will marginalize for us - // GenericSequentialSolver solver(p_Cp_B); - - // // Finally, we only want to have S\B variables in the Bayes net, so - // size_t nrFrontals = S_setminus_B.size(); - // BayesNet result = *solver.conditionalBayesNet(keep, nrFrontals, function); - - // // Undo the reduction - // gttic(Undo_Reduce); - // BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp_B) { - // if (factor) factor->permuteWithInverse(reduction); } - // result.permuteWithInverse(reduction); - // gttoc(Undo_Reduce); - - // assertInvariants(); - - // return result; - // } else { - // return BayesNet(); - // } - //} + return result; + } + else + { + return BayesNetType(); + } + } /* ************************************************************************* */ // separator marginal, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) /* ************************************************************************* */ - //template - //FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< - // DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const - //{ - // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); - // // Check if the Separator marginal was already calculated - // if (!cachedSeparatorMarginal_) { - // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); - // // If this is the root, there is no separator - // if (R.get() == this) { - // // we are root, return empty - // FactorGraph empty; - // cachedSeparatorMarginal_ = empty; - // } else { - // // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) - // // initialize P(Cp) with the parent separator marginal - // derived_ptr parent(parent_.lock()); - // gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline - // gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); - // FactorGraph p_Cp(parent->separatorMarginal(R, function)); // P(Sp) - // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); - // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); - // // now add the parent conditional - // p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) + template + typename BayesTreeCliqueBaseUnordered::FactorGraphType + BayesTreeCliqueBaseUnordered::separatorMarginal(Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + // Check if the Separator marginal was already calculated + if (!cachedSeparatorMarginal_) + { + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // If this is the root, there is no separator + if (parent_.expired() /*(if we're the root)*/) + { + // we are root, return empty + FactorGraphType empty; + cachedSeparatorMarginal_ = empty; + } + else + { + // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) + // initialize P(Cp) with the parent separator marginal + derived_ptr parent(parent_.lock()); + gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline + gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); + FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); + gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); + // now add the parent conditional + p_Cp += parent->conditional_; // P(Fp|Sp) - // // Reduce the variable indices to start at zero - // gttic(Reduce); - // const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); - // internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); - // BOOST_FOREACH(const boost::shared_ptr& factor, p_Cp) { - // if(factor) factor->reduceWithInverse(inverseReduction); } + // The variables we want to keepSet are exactly the ones in S + std::vector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); + cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(OrderingUnordered(indicesS), boost::none, function); + } + } - // // The variables we want to keepSet are exactly the ones in S - // sharedConditional p_F_S = this->conditional(); - // std::vector indicesS(p_F_S->beginParents(), p_F_S->endParents()); - // inverseReduction.applyInverse(indicesS); - // gttoc(Reduce); - - // // Create solver that will marginalize for us - // GenericSequentialSolver solver(p_Cp); - - // cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function)); - - // // Undo the reduction - // gttic(Undo_Reduce); - // BOOST_FOREACH(const typename boost::shared_ptr& factor, p_Cp) { - // if (factor) factor->permuteWithInverse(reduction); } - // BOOST_FOREACH(const typename boost::shared_ptr& factor, *cachedSeparatorMarginal_) { - // if (factor) factor->permuteWithInverse(reduction); } - // gttoc(Undo_Reduce); - // } - // } else { - // gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit); - // } - - // // return the shortcut P(S||B) - // return *cachedSeparatorMarginal_; // return the cached version - //} + // return the shortcut P(S||B) + return *cachedSeparatorMarginal_; // return the cached version + } /* ************************************************************************* */ // marginal2, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) /* ************************************************************************* */ - //template - //FactorGraph::FactorType> BayesTreeCliqueBaseUnordered< - // DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const - //{ - // gttic(BayesTreeCliqueBaseUnordered_marginal2); - // // initialize with separator marginal P(S) - // FactorGraph p_C(this->separatorMarginal(R, function)); - // // add the conditional P(F|S) - // p_C.push_back(this->conditional()->toFactor()); - // return p_C; - //} + template + typename BayesTreeCliqueBaseUnordered::FactorGraphType + BayesTreeCliqueBaseUnordered::marginal2(Eliminate function) const + { + gttic(BayesTreeCliqueBaseUnordered_marginal2); + // initialize with separator marginal P(S) + FactorGraphType p_C = this->separatorMarginal(function); + // add the conditional P(F|S) + p_C += boost::shared_ptr(this->conditional_); + return p_C; + } /* ************************************************************************* */ - template - void BayesTreeCliqueBaseUnordered::deleteCachedShortcuts() { + template + void BayesTreeCliqueBaseUnordered::deleteCachedShortcuts() { // When a shortcut is requested, all of the shortcuts between it and the // root are also generated. So, if this clique's cached shortcut is set, diff --git a/gtsam/inference/BayesTreeCliqueBaseUnordered.h b/gtsam/inference/BayesTreeCliqueBaseUnordered.h index c237e6eaf..9b0d1d657 100644 --- a/gtsam/inference/BayesTreeCliqueBaseUnordered.h +++ b/gtsam/inference/BayesTreeCliqueBaseUnordered.h @@ -20,10 +20,10 @@ #include namespace gtsam { - template class BayesTreeUnordered; -} -namespace gtsam { + // Forward declarations + template class BayesTreeUnordered; + template struct EliminationTraits; /** * This is the base class for BayesTree cliques. The default and standard derived type is @@ -38,12 +38,13 @@ namespace gtsam { * @tparam DERIVED The derived clique type. * @tparam CONDITIONAL The conditional type. * \nosubgrouping */ - template + template class BayesTreeCliqueBaseUnordered { private: - typedef BayesTreeCliqueBaseUnordered This; + typedef BayesTreeCliqueBaseUnordered This; typedef DERIVED DerivedType; + typedef EliminationTraits EliminationTraits; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; typedef boost::shared_ptr derived_ptr; @@ -51,7 +52,7 @@ namespace gtsam { public: typedef FACTORGRAPH FactorGraphType; - typedef BAYESNET BayesNetType; + typedef typename EliminationTraits::BayesNetType BayesNetType; typedef typename BayesNetType::ConditionalType ConditionalType; typedef boost::shared_ptr sharedConditional; typedef typename FactorGraphType::FactorType FactorType; @@ -110,14 +111,14 @@ namespace gtsam { /// @name Advanced Interface /// @{ - ///** return the conditional P(S|Root) on the separator given the root */ - //BayesNetType shortcut(derived_ptr root, Eliminate function) const; + /** return the conditional P(S|Root) on the separator given the root */ + BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraits::DefaultEliminate) const; - ///** return the marginal P(S) on the separator */ - //FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const; + /** return the marginal P(S) on the separator */ + FactorGraphType separatorMarginal(Eliminate function = EliminationTraits::DefaultEliminate) const; - ///** return the marginal P(C) of the clique, using marginal caching */ - //FactorGraphType marginal2(derived_ptr root, Eliminate function) const; + /** return the marginal P(C) of the clique, using marginal caching */ + FactorGraphType marginal2(Eliminate function = EliminationTraits::DefaultEliminate) const; /** * This deletes the cached shortcuts of all cliques (subtree) below this clique. @@ -132,18 +133,13 @@ namespace gtsam { protected: - ///// Calculate set \f$ S \setminus B \f$ for shortcut calculations - //std::vector separator_setminus_B(derived_ptr B) const; + /// Calculate set \f$ S \setminus B \f$ for shortcut calculations + std::vector separator_setminus_B(const derived_ptr& B) const; - ///// Calculate set \f$ S_p \cap B \f$ for shortcut calculations - //std::vector parent_separator_intersection_B(derived_ptr B) const; - - ///** - // * Determine variable indices to keep in recursive separator shortcut calculation - // * The factor graph p_Cp_B has keys from the parent clique Cp and from B. - // * But we only keep the variables not in S union B. - // */ - //std::vector shortcut_indices(derived_ptr B, const FactorGraphType& p_Cp_B) const; + /** Determine variable indices to keep in recursive separator shortcut calculation The factor + * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables + * not in S union B. */ + std::vector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; /** Non-recursive delete cached shortcuts and marginals - internal only. */ void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } diff --git a/gtsam/inference/BayesTreeUnordered-inst.h b/gtsam/inference/BayesTreeUnordered-inst.h index 6343c43ad..a7b0ec5b5 100644 --- a/gtsam/inference/BayesTreeUnordered-inst.h +++ b/gtsam/inference/BayesTreeUnordered-inst.h @@ -22,10 +22,14 @@ #include #include +#include #include +#include #include +using boost::assign::cref_list_of; + namespace gtsam { /* ************************************************************************* */ @@ -193,6 +197,24 @@ namespace gtsam { return new_clique; } + /* ************************************************************************* */ + namespace { + template + int _pushClique(FactorGraphUnordered& fg, const boost::shared_ptr& clique) { + fg.push_back(clique->conditional_); + return 0; + } + } + + /* ************************************************************************* */ + template + void BayesTreeUnordered::addFactorsToGraph(FactorGraphUnordered& graph) const + { + // Traverse the BayesTree and add all conditionals to this graph + int data = 0; // Unused + treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique, graph, _1)); + } + /* ************************************************************************* */ template void BayesTreeUnordered::removeClique(sharedClique clique) @@ -364,201 +386,132 @@ namespace gtsam { fillNodesIndex(subtree); // Populate nodes index } -// /* ************************************************************************* */ -// // First finds clique marginal then marginalizes that -// /* ************************************************************************* */ -// template -// typename BayesTreeUnordered::sharedFactor BayesTreeUnordered::marginalFactor( -// Key j, Eliminate function) const -// { -// return boost::make_shared(); -//// gttic(BayesTree_marginalFactor); -//// -//// // get clique containing Index j -//// sharedClique clique = this->clique(j); -//// -//// // calculate or retrieve its marginal P(C) = P(F,S) -////#ifdef OLD_SHORTCUT_MARGINALS -//// FactorGraph cliqueMarginal = clique->marginal(root_,function); -////#else -//// FactorGraph cliqueMarginal = clique->marginal2(root_,function); -////#endif -//// -//// // Reduce the variable indices to start at zero -//// gttic(Reduce); -//// const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); -//// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); -//// BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { -//// if(factor) factor->reduceWithInverse(inverseReduction); } -//// gttoc(Reduce); -//// -//// // now, marginalize out everything that is not variable j -//// GenericSequentialSolver solver(cliqueMarginal); -//// boost::shared_ptr result = solver.marginalFactor(inverseReduction[j], function); -//// -//// // Undo the reduction -//// gttic(Undo_Reduce); -//// result->permuteWithInverse(reduction); -//// BOOST_FOREACH(const boost::shared_ptr& factor, cliqueMarginal) { -//// if(factor) factor->permuteWithInverse(reduction); } -//// gttoc(Undo_Reduce); -//// return result; -// } -// -// /* ************************************************************************* */ -// template -// typename BayesTreeUnordered::sharedBayesNet BayesTreeUnordered::marginalBayesNet( -// Key j, Eliminate function) const -// { -// return boost::make_shared(); -// //gttic(BayesTree_marginalBayesNet); -// -// //// calculate marginal as a factor graph -// //FactorGraph fg; -// //fg.push_back(this->marginalFactor(j,function)); -// -// //// Reduce the variable indices to start at zero -// //gttic(Reduce); -// //const Permutation reduction = internal::createReducingPermutation(fg.keys()); -// //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); -// //BOOST_FOREACH(const boost::shared_ptr& factor, fg) { -// // if(factor) factor->reduceWithInverse(inverseReduction); } -// //gttoc(Reduce); -// -// //// eliminate factor graph marginal to a Bayes net -// //boost::shared_ptr > bn = GenericSequentialSolver(fg).eliminate(function); -// -// //// Undo the reduction -// //gttic(Undo_Reduce); -// //bn->permuteWithInverse(reduction); -// //BOOST_FOREACH(const boost::shared_ptr& factor, fg) { -// // if(factor) factor->permuteWithInverse(reduction); } -// //gttoc(Undo_Reduce); -// //return bn; -// } -// -// /* ************************************************************************* */ -// // Find two cliques, their joint, then marginalizes -// /* ************************************************************************* */ -// template -// typename BayesTreeUnordered::sharedFactorGraph -// BayesTreeUnordered::joint(Key j1, Key j2, Eliminate function) const { -// return boost::make_shared(); -// //gttic(BayesTree_joint); -// -// //// get clique C1 and C2 -// //sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; -// -// //gttic(Lowest_common_ancestor); -// //// Find lowest common ancestor clique -// //sharedClique B; { -// // // Build two paths to the root -// // FastList path1, path2; { -// // sharedClique p = C1; -// // while(p) { -// // path1.push_front(p); -// // p = p->parent(); -// // } -// // } { -// // sharedClique p = C2; -// // while(p) { -// // path2.push_front(p); -// // p = p->parent(); -// // } -// // } -// // // Find the path intersection -// // B = this->root(); -// // typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); -// // while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { -// // B = *p1; -// // ++p1; -// // ++p2; -// // } -// //} -// //gttoc(Lowest_common_ancestor); -// -// //// Compute marginal on lowest common ancestor clique -// //gttic(LCA_marginal); -// //FactorGraph p_B = B->marginal2(this->root(), function); -// //gttoc(LCA_marginal); -// -// //// Compute shortcuts of the requested cliques given the lowest common ancestor -// //gttic(Clique_shortcuts); -// //BayesNet p_C1_Bred = C1->shortcut(B, function); -// //BayesNet p_C2_Bred = C2->shortcut(B, function); -// //gttoc(Clique_shortcuts); -// -// //// Factor the shortcuts to be conditioned on the full root -// //// Get the set of variables to eliminate, which is C1\B. -// //gttic(Full_root_factoring); -// //sharedConditional p_C1_B; { -// // std::vector C1_minus_B; { -// // FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); -// // BOOST_FOREACH(const Index j, *B->conditional()) { -// // C1_minus_B_set.erase(j); } -// // C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); -// // } -// // // Factor into C1\B | B. -// // FactorGraph temp_remaining; -// // boost::tie(p_C1_B, temp_remaining) = FactorGraph(p_C1_Bred).eliminate(C1_minus_B, function); -// //} -// //sharedConditional p_C2_B; { -// // std::vector C2_minus_B; { -// // FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); -// // BOOST_FOREACH(const Index j, *B->conditional()) { -// // C2_minus_B_set.erase(j); } -// // C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); -// // } -// // // Factor into C2\B | B. -// // FactorGraph temp_remaining; -// // boost::tie(p_C2_B, temp_remaining) = FactorGraph(p_C2_Bred).eliminate(C2_minus_B, function); -// //} -// //gttoc(Full_root_factoring); -// -// //gttic(Variable_joint); -// //// Build joint on all involved variables -// //FactorGraph p_BC1C2; -// //p_BC1C2.push_back(p_B); -// //p_BC1C2.push_back(p_C1_B->toFactor()); -// //p_BC1C2.push_back(p_C2_B->toFactor()); -// //if(C1 != B) -// // p_BC1C2.push_back(C1->conditional()->toFactor()); -// //if(C2 != B) -// // p_BC1C2.push_back(C2->conditional()->toFactor()); -// -// //// Reduce the variable indices to start at zero -// //gttic(Reduce); -// //const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); -// //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); -// //BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { -// // if(factor) factor->reduceWithInverse(inverseReduction); } -// //std::vector js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); -// //gttoc(Reduce); -// -// //// now, marginalize out everything that is not variable j -// //GenericSequentialSolver solver(p_BC1C2); -// //boost::shared_ptr > result = solver.jointFactorGraph(js, function); -// -// //// Undo the reduction -// //gttic(Undo_Reduce); -// //BOOST_FOREACH(const boost::shared_ptr& factor, *result) { -// // if(factor) factor->permuteWithInverse(reduction); } -// //BOOST_FOREACH(const boost::shared_ptr& factor, p_BC1C2) { -// // if(factor) factor->permuteWithInverse(reduction); } -// //gttoc(Undo_Reduce); -// //return result; -// } -// -// /* ************************************************************************* */ -// template -// typename BayesTreeUnordered::sharedBayesNet BayesTreeUnordered::jointBayesNet( -// Key j1, Key j2, Eliminate function) const -// { -// return boost::make_shared(); -// //// eliminate factor graph marginal to a Bayes net -// //return GenericSequentialSolver ( -// // *this->joint(j1, j2, function)).eliminate(function); -// } + /* ************************************************************************* */ + // First finds clique marginal then marginalizes that + /* ************************************************************************* */ + template + typename BayesTreeUnordered::sharedConditional + BayesTreeUnordered::marginalFactor(Key j, const Eliminate& function) const + { + gttic(BayesTree_marginalFactor); + + // get clique containing Index j + sharedClique clique = this->clique(j); + + // calculate or retrieve its marginal P(C) = P(F,S) + FactorGraphType cliqueMarginal = clique->marginal2(function); + + // Now, marginalize out everything that is not variable j + BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet( + OrderingUnordered(cref_list_of<1,Key>(j)), boost::none, function); + + // The Bayes net should contain only one conditional for variable j, so return it + return marginalBN.front(); + } + + /* ************************************************************************* */ + // Find two cliques, their joint, then marginalizes + /* ************************************************************************* */ + template + typename BayesTreeUnordered::sharedFactorGraph + BayesTreeUnordered::joint(Key j1, Key j2, const Eliminate& function) const + { + gttic(BayesTree_joint); + return boost::make_shared(*jointBayesNet(j1, j2, function)); + } + + /* ************************************************************************* */ + template + typename BayesTreeUnordered::sharedBayesNet + BayesTreeUnordered::jointBayesNet(Key j1, Key j2, const Eliminate& function) const + { + gttic(BayesTree_jointBayesNet); + // get clique C1 and C2 + sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; + + gttic(Lowest_common_ancestor); + // Find lowest common ancestor clique + sharedClique B; { + // Build two paths to the root + FastList path1, path2; { + sharedClique p = C1; + while(p) { + path1.push_front(p); + p = p->parent(); + } + } { + sharedClique p = C2; + while(p) { + path2.push_front(p); + p = p->parent(); + } + } + // Find the path intersection + typename FastList::const_iterator p1 = path1.begin(), p2 = path2.begin(); + if(*p1 == *p2) + B = *p1; + while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { + B = *p1; + ++p1; + ++p2; + } + } + gttoc(Lowest_common_ancestor); + + // Compute marginal on lowest common ancestor clique + gttic(LCA_marginal); + FactorGraphType p_B = B->marginal2(function); + gttoc(LCA_marginal); + + // Compute shortcuts of the requested cliques given the lowest common ancestor + gttic(Clique_shortcuts); + BayesNetType p_C1_Bred = C1->shortcut(B, function); + BayesNetType p_C2_Bred = C2->shortcut(B, function); + gttoc(Clique_shortcuts); + + // Factor the shortcuts to be conditioned on the full root + // Get the set of variables to eliminate, which is C1\B. + gttic(Full_root_factoring); + shared_ptr p_C1_B; { + std::vector C1_minus_B; { + FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + BOOST_FOREACH(const Index j, *B->conditional()) { + C1_minus_B_set.erase(j); } + C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); + } + // Factor into C1\B | B. + sharedFactorGraph temp_remaining; + boost::tie(p_C1_B, temp_remaining) = + FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(OrderingUnordered(C1_minus_B), function); + } + shared_ptr p_C2_B; { + std::vector C2_minus_B; { + FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + BOOST_FOREACH(const Index j, *B->conditional()) { + C2_minus_B_set.erase(j); } + C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); + } + // Factor into C2\B | B. + sharedFactorGraph temp_remaining; + boost::tie(p_C2_B, temp_remaining) = + FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(OrderingUnordered(C2_minus_B), function); + } + gttoc(Full_root_factoring); + + gttic(Variable_joint); + // Build joint on all involved variables + FactorGraphType p_BC1C2; + p_BC1C2 += p_B; + p_BC1C2 += *p_C1_B; + p_BC1C2 += *p_C2_B; + if(C1 != B) + p_BC1C2 += C1->conditional(); + if(C2 != B) + p_BC1C2 += C2->conditional(); + + // now, marginalize out everything that is not variable j1 or j2 + return p_BC1C2.marginalMultifrontalBayesNet(OrderingUnordered(cref_list_of<2,Key>(j1)(j2)), boost::none, function); + } /* ************************************************************************* */ template diff --git a/gtsam/inference/BayesTreeUnordered.h b/gtsam/inference/BayesTreeUnordered.h index 2f8db4f1c..13d35b038 100644 --- a/gtsam/inference/BayesTreeUnordered.h +++ b/gtsam/inference/BayesTreeUnordered.h @@ -28,6 +28,9 @@ namespace gtsam { + // Forward declarations + template class FactorGraphUnordered; + /** * Bayes tree * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, @@ -59,6 +62,7 @@ namespace gtsam { typedef typename CLIQUE::FactorGraphType FactorGraphType; typedef boost::shared_ptr sharedFactorGraph; typedef typename FactorGraphType::Eliminate Eliminate; + typedef typename CLIQUE::EliminationTraits EliminationTraits; /** A convenience class for a list of shared cliques */ struct Cliques : public FastList { @@ -135,7 +139,7 @@ namespace gtsam { const Nodes& nodes() const { return nodes_; } /** Access node by variable */ - const Node::shared_ptr operator[](Key j) const { return nodes_.at(j); } + const sharedNode operator[](Key j) const { return nodes_.at(j); } /** return root cliques */ const std::vector& roots() const { return roots_; } @@ -155,27 +159,24 @@ namespace gtsam { /** Collect number of cliques with cached separator marginals */ size_t numCachedSeparatorMarginals() const; - ///** return marginal on any variable */ - //sharedFactor marginalFactor(Key j, Eliminate function) const; + /** Return marginal on any variable. Note that this actually returns a conditional, for which a + * solution may be directly obtained by calling .solve() on the returned object. + * Alternatively, it may be directly used as its factor base class. For example, for Gaussian + * systems, this returns a GaussianConditional, which inherits from JacobianFactor and + * GaussianFactor. */ + sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraits::DefaultEliminate) const; - ///** - // * return marginal on any variable, as a Bayes Net - // * NOTE: this function calls marginal, and then eliminates it into a Bayes Net - // * This is more expensive than the above function - // */ - //sharedBayesNet marginalBayesNet(Key j, Eliminate function) const; + /** + * return joint on two variables + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ + sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const; - ///** - // * return joint on two variables - // * Limitation: can only calculate joint if cliques are disjoint or one of them is root - // */ - //sharedFactorGraph joint(Index j1, Index j2, Eliminate function) const; - - ///** - // * return joint on two variables as a BayesNet - // * Limitation: can only calculate joint if cliques are disjoint or one of them is root - // */ - //sharedBayesNet jointBayesNet(Index j1, Index j2, Eliminate function) const; + /** + * return joint on two variables as a BayesNet + * Limitation: can only calculate joint if cliques are disjoint or one of them is root + */ + sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const; /** * Read only with side effects @@ -226,6 +227,9 @@ namespace gtsam { /** add a clique (top down) */ void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); + /** Add all cliques in this BayesTree to the specified factor graph */ + void addFactorsToGraph(FactorGraphUnordered& graph) const; + protected: /** private helper method for saving the Tree to a text file in GraphViz format */ diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 3865f277a..8171e12a4 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -113,5 +113,57 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering, + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const + { + if(variableIndex) + { + if(marginalizedVariableOrdering) + { + // An ordering was provided for the marginalized variables, so we can first eliminate them + // in the order requested. + std::pair, boost::shared_ptr > eliminated = + eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex); + + if(const OrderingUnordered* varsAsOrdering = boost::get(&variables)) + { + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return eliminateSequential(*varsAsOrdering, function, *variableIndex); + } + else + { + // No ordering was provided for the unmarginalized variables, so order them with COLAMD. + return eliminateSequential(boost::none, function, *variableIndex); + } + } + else + { + // No ordering was provided for the marginalized variables, so order them using constrained + // COLAMD. + bool unmarginalizedAreOrdered = (boost::get(&variables) != 0); + OrderingUnordered totalOrdering = + OrderingUnordered::COLAMDConstrainedLast(*variableIndex, + boost::get&>(variables), unmarginalizedAreOrdered); + + // Split up ordering + const size_t nVars = boost::get&>(variables).size(); // Works because OrderingUnordered derives from std::vector + OrderingUnordered marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars); + OrderingUnordered marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); + + // Call this function again with the computed orderings + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + } + } else { + // If no variable index is provided, compute one and call this function again + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, VariableIndexUnordered(asDerived())); + } + } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8c5daf529..a62f5af8a 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -182,6 +183,23 @@ namespace gtsam { const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /** Compute the marginal of the requested variables and return the result as a Bayes net. + * @param variables Determines the variables whose marginal to compute, if provided as an + * Ordering they will be ordered in the returned BayesNet as specified, and if provided + * as a vector they will be ordered using constrained COLAMD. + * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized + * out, i.e. all variables not in \c variables. If this is boost::none, the ordering + * will be computed with COLAMD. + * @param function Optional dense elimination function, if not provided the default will be + * used. + * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not + * provided one will be computed. */ + boost::shared_ptr marginalMultifrontalBayesNet( + boost::variant&> variables, + OptionalOrdering marginalizedVariableOrdering = boost::none, + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + private: // Access the derived factor graph class diff --git a/gtsam/inference/FactorGraphUnordered-inst.h b/gtsam/inference/FactorGraphUnordered-inst.h index e7b4e62f6..b31b80a8f 100644 --- a/gtsam/inference/FactorGraphUnordered-inst.h +++ b/gtsam/inference/FactorGraphUnordered-inst.h @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -64,25 +65,6 @@ namespace gtsam { return true; } - /* ************************************************************************* */ - namespace { - template - int _pushClique(FactorGraphUnordered* fg, const boost::shared_ptr& clique) { - fg->push_back(clique->conditional_); - return 0; - } - } - - /* ************************************************************************* */ - template - template - void FactorGraphUnordered::push_back_bayesTree(const BayesTreeUnordered& bayesTree) - { - // Traverse the BayesTree and add all conditionals to this graph - int data = 0; // Unused - treeTraversal::DepthFirstForest(bayesTree, data, boost::bind(&_pushClique, this, _1)); - } - /* ************************************************************************* */ template size_t FactorGraphUnordered::nrFactors() const { diff --git a/gtsam/inference/FactorGraphUnordered.h b/gtsam/inference/FactorGraphUnordered.h index d2ae08b8c..26563da7a 100644 --- a/gtsam/inference/FactorGraphUnordered.h +++ b/gtsam/inference/FactorGraphUnordered.h @@ -24,15 +24,51 @@ #include #include +#include #include #include #include #include -#include namespace gtsam { + // Forward declarations + template class BayesTreeUnordered; + + /** Helper */ + template + class CRefCallPushBack + { + C& obj; + public: + CRefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(const A& a) { obj.push_back(a); } + }; + + /** Helper */ + template + class RefCallPushBack + { + C& obj; + public: + RefCallPushBack(C& obj) : obj(obj) {} + template + void operator()(A& a) { obj.push_back(a); } + }; + + /** Helper */ + template + class CRefCallAddCopy + { + C& obj; + public: + CRefCallAddCopy(C& obj) : obj(obj) {} + template + void operator()(const A& a) { obj.addCopy(a); } + }; + /** * A factor graph is a bipartite graph with factor nodes connected to variable nodes. * In this class, however, only factor nodes are kept around. @@ -44,6 +80,7 @@ namespace gtsam { public: typedef FACTOR FactorType; ///< factor type typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef sharedFactor value_type; typedef typename std::vector::iterator iterator; typedef typename std::vector::const_iterator const_iterator; @@ -70,7 +107,7 @@ namespace gtsam { /** Construct from container of factors (shared_ptr or plain objects) */ template - explicit FactorGraphUnordered(const CONTAINER& factors) { push_back(factors.begin(), factors.end()); } + explicit FactorGraphUnordered(const CONTAINER& factors) { push_back(factors); } /// @} /// @name Advanced Constructors @@ -112,18 +149,13 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - void push_back(const boost::shared_ptr& factor) { + typename std::enable_if::value>::type + push_back(boost::shared_ptr& factor) { factors_.push_back(boost::shared_ptr(factor)); } - /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid - * the copy). */ - template - void add(const DERIVEDFACTOR& factor) { - factors_.push_back(boost::make_shared(factor)); } - - /** push back many factors */ - void push_back(const This& factors) { - factors_.insert(end(), factors.begin(), factors.end()); } + /** Add a factor directly using a shared_ptr */ + void push_back(const sharedFactor& factor) { + factors_.push_back(factor); } /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template @@ -131,42 +163,92 @@ namespace gtsam { push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } + /** push back many factors as shared_ptr's in a container (factors are not copied) */ + template + typename std::enable_if::value>::type + push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); + } + + /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived + * classes in favor of a type-specialized version that calls this templated function. */ + template + typename std::enable_if::value>::type + push_back(const BayesTreeUnordered& bayesTree) { + bayesTree.addFactorsToGraph(*this); + } + + /** += syntax for push_back, e.g. graph += f1, f2, f3 */ + //template + //boost::assign::list_inserter > + // operator+=(const T& factorOrContainer) + //{ + // return boost::assign::make_list_inserter( + // boost::bind(&This::push_back, this, _1)); + //} + + /** Add a factor directly using a shared_ptr */ + template + typename std::enable_if::value, + boost::assign::list_inserter > >::type + operator+=(boost::shared_ptr& factor) { + return boost::assign::make_list_inserter(RefCallPushBack(*this)); + } + + template + boost::assign::list_inserter > + operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { + return boost::assign::make_list_inserter(CRefCallPushBack(*this)); + } + + ///** Add a factor directly using a shared_ptr */ + //boost::assign::list_inserter > + // operator+=(const sharedFactor& factor) { + // return boost::assign::make_list_inserter(CRefCallPushBack(*this)); + //} + + ///** push back many factors as shared_ptr's in a container (factors are not copied) */ + //template + //typename std::enable_if::value, + // boost::assign::list_inserter > >::type + // operator+=(const CONTAINER& container) { + // return boost::assign::make_list_inserter(CRefCallPushBack(*this)); + //} + + ///** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived + // * classes in favor of a type-specialized version that calls this templated function. */ + //template + //boost::assign::list_inserter > + // operator+=(const BayesTreeUnordered& bayesTree) { + // return boost::assign::make_list_inserter(CRefCallPushBack(*this)); + //} + + /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid + * the copy). */ + template + typename std::enable_if::value>::type + push_back(const DERIVEDFACTOR& factor) { + factors_.push_back(boost::make_shared(factor)); } + /** push back many factors with an iterator over plain factors (factors are copied) */ template typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { for(ITERATOR f = firstFactor; f != lastFactor; ++f) - add(*f); + push_back(*f); } - /** Add a factor by value, will be copy-constructed (pass a shared_ptr to avoid the copy). */ - template - void push_back(const DERIVEDFACTOR& factor) { - add(factor); } - - protected: - /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived - * classes in favor of a type-specialized version that calls this templated function. */ - template - void push_back_bayesTree(const BayesTreeUnordered& bayesTree); - - public: - /** += syntax for push_back, e.g. graph += f1, f2, f3 */ - template - boost::assign::list_inserter, T> - operator+=(const T& factorOrContainer) - { - return boost::assign::make_list_inserter( - boost::assign_detail::call_push_back(*this))(factorOrContainer); + /** push back many factors as non-pointer objects in a container (factors are copied) */ + template + typename std::enable_if::value>::type + push_back(const CONTAINER& container) { + push_back(container.begin(), container.end()); } - /** - * @brief Add a vector of derived factors - * @param factors to add - */ - //template - //void push_back(const std::vector >& factors) { - // factors_.insert(end(), factors.begin(), factors.end()); + //template + //boost::assign::list_inserter > + // operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) { + // return boost::assign::make_list_inserter(CRefCallAddCopy(*this)); //} /// @} @@ -246,6 +328,12 @@ namespace gtsam { /** replace a factor by index */ void replace(size_t index, sharedFactor factor) { at(index) = factor; } + /** Erase factor and rearrange other factors to take up the empty space */ + void erase(const_iterator item) { factors_.erase(item); } + + /** Erase factors and rearrange other factors to take up the empty space */ + void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); } + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/linear/GaussianBayesTreeUnordered.h b/gtsam/linear/GaussianBayesTreeUnordered.h index 396a4a9c3..4213b3289 100644 --- a/gtsam/linear/GaussianBayesTreeUnordered.h +++ b/gtsam/linear/GaussianBayesTreeUnordered.h @@ -33,11 +33,11 @@ namespace gtsam { /* ************************************************************************* */ /** A clique in a GaussianBayesTree */ class GTSAM_EXPORT GaussianBayesTreeCliqueUnordered : - public BayesTreeCliqueBaseUnordered + public BayesTreeCliqueBaseUnordered { public: typedef GaussianBayesTreeCliqueUnordered This; - typedef BayesTreeCliqueBaseUnordered Base; + typedef BayesTreeCliqueBaseUnordered Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; GaussianBayesTreeCliqueUnordered() {} diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp index 4a1c8405d..556d60e7f 100644 --- a/gtsam/linear/GaussianConditionalUnordered.cpp +++ b/gtsam/linear/GaussianConditionalUnordered.cpp @@ -73,7 +73,8 @@ namespace gtsam { } /* ************************************************************************* */ - bool GaussianConditionalUnordered::equals(const GaussianConditionalUnordered &c, double tol) const { + bool GaussianConditionalUnordered::equals(const GaussianConditionalUnordered &c, double tol) const + { // check if the size of the parents_ map is the same if (parents().size() != c.parents().size()) return false; diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp index 39048509a..b6bffee58 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -40,12 +40,6 @@ namespace gtsam { return Base::equals(fg, tol); } - /* ************************************************************************* */ - void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree) - { - Base::push_back_bayesTree(bayesTree); - } - /* ************************************************************************* */ GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const { FastSet keys; diff --git a/gtsam/linear/GaussianFactorGraphUnordered.h b/gtsam/linear/GaussianFactorGraphUnordered.h index 705ae74d8..09d57f95a 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.h +++ b/gtsam/linear/GaussianFactorGraphUnordered.h @@ -75,17 +75,13 @@ namespace gtsam { /** Default constructor */ GaussianFactorGraphUnordered() {} - /** Constructor that receives a BayesTree */ - GaussianFactorGraphUnordered(const GaussianBayesTreeUnordered& gbt) { - push_back_bayesTree(gbt); } - /** Construct from iterator over factors */ template GaussianFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} /** Construct from container of factors (shared_ptr or plain objects) */ template - GaussianFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} + explicit GaussianFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} /** Implicit copy/downcast constructor to override explicit template container constructor */ template @@ -131,9 +127,6 @@ namespace gtsam { void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) { add(JacobianFactorUnordered(terms,b,model)); } - /** push back a BayesTree as a collection of factors. */ - void push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree); - /** * Return the set of variables involved in the factors (computes a set * union). diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp index aaf34d9b3..f1563532f 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.cpp +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.cpp @@ -16,9 +16,11 @@ * @author Richard Roberts */ -#include #include #include +#include + +#include namespace gtsam { @@ -28,5 +30,23 @@ namespace gtsam { return Base::equals(bn, tol); } + /* ************************************************************************* */ + void SymbolicBayesNetUnordered::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const + { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) { + SymbolicConditionalUnordered::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + SymbolicConditionalUnordered::Parents parents = conditional->parents(); + BOOST_FOREACH(Key p, parents) + of << p << "->" << me << std::endl; + } + + of << "}"; + of.close(); + } + } diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h index 0f45d4a46..d2c3ea724 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.h +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -18,14 +18,12 @@ #pragma once +#include #include #include namespace gtsam { - // Forward declarations - class SymbolicConditionalUnordered; - /** Symbolic Bayes Net * \nosubgrouping */ @@ -70,6 +68,8 @@ namespace gtsam { /// @name Standard Interface /// @{ + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /// @} }; diff --git a/gtsam/symbolic/SymbolicBayesTreeUnordered.h b/gtsam/symbolic/SymbolicBayesTreeUnordered.h index 5d11240a5..99a2b4ec3 100644 --- a/gtsam/symbolic/SymbolicBayesTreeUnordered.h +++ b/gtsam/symbolic/SymbolicBayesTreeUnordered.h @@ -31,11 +31,11 @@ namespace gtsam { /* ************************************************************************* */ /// A clique in a SymbolicBayesTree class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered : - public BayesTreeCliqueBaseUnordered + public BayesTreeCliqueBaseUnordered { public: typedef SymbolicBayesTreeCliqueUnordered This; - typedef BayesTreeCliqueBaseUnordered Base; + typedef BayesTreeCliqueBaseUnordered Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; SymbolicBayesTreeCliqueUnordered() {} diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.cpp b/gtsam/symbolic/SymbolicConditionalUnordered.cpp index 7c014b587..fb85d42cf 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.cpp +++ b/gtsam/symbolic/SymbolicConditionalUnordered.cpp @@ -22,4 +22,16 @@ namespace gtsam { using namespace std; + /* ************************************************************************* */ + void SymbolicConditionalUnordered::print(const std::string& str, const KeyFormatter& keyFormatter) const + { + BaseConditional::print(str, keyFormatter); + } + + /* ************************************************************************* */ + bool SymbolicConditionalUnordered::equals(const This& c, double tol) const + { + return BaseFactor::equals(c) && BaseConditional::equals(c); + } + } diff --git a/gtsam/symbolic/SymbolicConditionalUnordered.h b/gtsam/symbolic/SymbolicConditionalUnordered.h index 0915f7e82..b4227f14b 100644 --- a/gtsam/symbolic/SymbolicConditionalUnordered.h +++ b/gtsam/symbolic/SymbolicConditionalUnordered.h @@ -64,17 +64,17 @@ namespace gtsam { /** Named constructor from an arbitrary number of keys and frontals */ template - static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) + static SymbolicConditionalUnordered FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { SymbolicConditionalUnordered result; - (BaseFactor&)result = BaseFactor::FromIterator(firstKey, lastKey); + (BaseFactor&)result = BaseFactor::FromIterators(firstKey, lastKey); result.nrFrontals_ = nrFrontals; return result; } /** Named constructor from an arbitrary number of keys and frontals */ template static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals) { - return FromIterator(keys.begin(), keys.end(), nrFrontals); } + return FromIterators(keys.begin(), keys.end(), nrFrontals); } virtual ~SymbolicConditionalUnordered() {} @@ -83,11 +83,10 @@ namespace gtsam { /// @name Testable /** Print with optional formatter */ - void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - BaseConditional::print(str, keyFormatter); } + void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check equality */ - bool equals(const This& c, double tol = 1e-9) const { return BaseConditional::equals(c); } + bool equals(const This& c, double tol = 1e-9) const; /// @} diff --git a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h index c1fc57195..57836dbfc 100644 --- a/gtsam/symbolic/SymbolicEliminationTreeUnordered.h +++ b/gtsam/symbolic/SymbolicEliminationTreeUnordered.h @@ -32,22 +32,18 @@ namespace gtsam { typedef SymbolicEliminationTreeUnordered This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - /** - * Build the elimination tree of a factor graph using pre-computed column structure. - * @param factorGraph The factor graph for which to build the elimination tree - * @param structure The set of factors involving each variable. If this is not - * precomputed, you can call the Create(const FactorGraph&) - * named constructor instead. - * @return The elimination tree - */ + /** Build the elimination tree of a factor graph using pre-computed column structure. + * @param factorGraph The factor graph for which to build the elimination tree + * @param structure The set of factors involving each variable. If this is not precomputed, + * you can call the Create(const FactorGraph&) named constructor instead. + * @return The elimination tree */ SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const VariableIndexUnordered& structure, const OrderingUnordered& order); /** Build the elimination tree of a factor graph. Note that this has to compute the column - * structure as a VariableIndex, so if you already have this precomputed, use the other - * constructor instead. - * @param factorGraph The factor graph for which to build the elimination tree - */ + * structure as a VariableIndex, so if you already have this precomputed, use the other + * constructor instead. + * @param factorGraph The factor graph for which to build the elimination tree */ SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order); @@ -64,6 +60,9 @@ namespace gtsam { private: + /** Private default constructor for testing */ + SymbolicEliminationTreeUnordered() {} + friend class ::EliminationTreeUnorderedTester; }; diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp index 9c2958e18..b4c1466e5 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.cpp @@ -55,11 +55,6 @@ namespace gtsam { push_back(boost::make_shared(key1,key2,key3,key4)); } - /* ************************************************************************* */ - void SymbolicFactorGraphUnordered::push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree) { - Base::push_back_bayesTree(bayesTree); - } - // /* ************************************************************************* */ // std::pair // SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h index 19b6126d8..53eb2af65 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -69,17 +69,13 @@ namespace gtsam { /** Construct empty factor graph */ SymbolicFactorGraphUnordered() {} - /** Constructor from a BayesTree */ - SymbolicFactorGraphUnordered(const SymbolicBayesTreeUnordered& bayesTree) { - push_back_bayesTree(bayesTree); } - /** Construct from iterator over factors */ template SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} /** Construct from container of factors (shared_ptr or plain objects) */ template - SymbolicFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} + explicit SymbolicFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} /** Implicit copy/downcast constructor to override explicit template container constructor */ template @@ -109,9 +105,6 @@ namespace gtsam { /** Push back 4-way factor */ void push_factor(Key key1, Key key2, Key key3, Key key4); - /** push back a BayesTree as a collection of factors. */ - void push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree); - /// @} }; diff --git a/gtsam/symbolic/SymbolicFactorUnordered.cpp b/gtsam/symbolic/SymbolicFactorUnordered.cpp index eb619f54c..5b24b34c2 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.cpp +++ b/gtsam/symbolic/SymbolicFactorUnordered.cpp @@ -56,7 +56,7 @@ namespace gtsam { boost::make_shared( SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)), boost::make_shared( - SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end()))); + SymbolicFactorUnordered::FromIterators(orderedKeys.begin() + nFrontals, orderedKeys.end()))); } /* ************************************************************************* */ @@ -70,7 +70,7 @@ namespace gtsam { SymbolicFactorUnordered::eliminate(const OrderingUnordered& keys) const { SymbolicFactorGraphUnordered graph; - graph.add(*this); + graph += *this; // TODO: Is there a way to avoid copying this factor? return EliminateSymbolicUnordered(graph, keys); } diff --git a/gtsam/symbolic/SymbolicFactorUnordered.h b/gtsam/symbolic/SymbolicFactorUnordered.h index 1783d2309..2f18ed567 100644 --- a/gtsam/symbolic/SymbolicFactorUnordered.h +++ b/gtsam/symbolic/SymbolicFactorUnordered.h @@ -94,7 +94,7 @@ namespace gtsam { public: /** Constructor from a collection of keys */ template - static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) { + static SymbolicFactorUnordered FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) { return SymbolicFactorUnordered(Base::FromIterators(beginKey, endKey)); } /** Constructor from a collection of keys - compatible with boost::assign::list_of and diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index b97e54096..e66f3d501 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -21,6 +21,10 @@ #pragma once #include +#include +#include +#include +#include #include #include diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index afa494f7e..c63234e77 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -80,13 +81,13 @@ TEST( SymbolicBayesNet, combine ) /* ************************************************************************* */ TEST(SymbolicBayesNet, saveGraph) { SymbolicBayesNetUnordered bn; - bn.add(SymbolicConditionalUnordered(_A_, _B_)); + bn += SymbolicConditionalUnordered(_A_, _B_); std::vector keys; keys.push_back(_B_); keys.push_back(_C_); keys.push_back(_D_); - bn.add(SymbolicConditionalUnordered::FromKeys(keys,2)); - bn.add(SymbolicConditionalUnordered(_D_)); + bn += SymbolicConditionalUnordered::FromKeys(keys,2); + bn += SymbolicConditionalUnordered(_D_); bn.saveGraph("SymbolicBayesNet.dot"); } diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 2e0ee78b3..ae184375c 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -33,20 +34,32 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; -/* ************************************************************************* */ -// Conditionals for ASIA example from the tutorial with A and D evidence -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_)); +namespace { + /* ************************************************************************* */ + // Conditionals for ASIA example from the tutorial with A and D evidence + 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))); + // Cliques + SymbolicConditionalUnordered::shared_ptr ELB( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(list_of(_E_)(_L_)(_B_), 3))); + + /* ************************************************************************* */ + // Helper function for below + template + SymbolicBayesTreeCliqueUnordered::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals) + { + return boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(keys, nrFrontals))); + } +} /* ************************************************************************* */ TEST_UNSAFE( SymbolicBayesTree, constructor ) @@ -204,76 +217,68 @@ TEST(SymbolicBayesTree, clear) // 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); + +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; + SymbolicFactorGraphUnordered chain = list_of + (SymbolicFactorUnordered(_A_)) + (SymbolicFactorUnordered(_B_, _A_)) + (SymbolicFactorUnordered(_C_, _A_)) + (SymbolicFactorUnordered(_D_, _C_)) + (SymbolicFactorUnordered(_E_, _B_)) + (SymbolicFactorUnordered(_F_, _E_)) + (SymbolicFactorUnordered(_G_, _F_)); + SymbolicBayesTreeUnordered bayesTree = *chain.eliminateMultifrontal( + OrderingUnordered(list_of(_A_)(_B_)(_C_)(_D_)(_E_)(_F_))); + + //bayesTree.print("BayesTree"); + //bayesTree.saveGraph("BT1.dot"); + + SymbolicBayesTreeUnordered::sharedClique rootClique = bayesTree.roots().front(); + //rootClique->printTree(); + SymbolicBayesTreeUnordered::Cliques allCliques; + getAllCliques(rootClique,allCliques); + + BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) { + //clique->print("Clique#"); + SymbolicBayesNetUnordered bn = clique->shortcut(rootClique); + //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, (long)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, 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 ) //{ @@ -372,98 +377,6 @@ TEST(SymbolicBayesTree, clear) // 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 @@ -519,291 +432,306 @@ TEST(SymbolicBayesTree, clear) //// 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)); -// } -// -//} + +/* ************************************************************************* */ +TEST( SymbolicBayesTreeUnordered, thinTree ) { + + // create a thin-tree Bayesnet, a la Jean-Guillaume + SymbolicBayesNetUnordered bayesNet; + bayesNet.push_back(boost::make_shared(14)); + + bayesNet.push_back(boost::make_shared(13, 14)); + bayesNet.push_back(boost::make_shared(12, 14)); + + bayesNet.push_back(boost::make_shared(11, 13, 14)); + bayesNet.push_back(boost::make_shared(10, 13, 14)); + bayesNet.push_back(boost::make_shared(9, 12, 14)); + bayesNet.push_back(boost::make_shared(8, 12, 14)); + + bayesNet.push_back(boost::make_shared(7, 11, 13)); + bayesNet.push_back(boost::make_shared(6, 11, 13)); + bayesNet.push_back(boost::make_shared(5, 10, 13)); + bayesNet.push_back(boost::make_shared(4, 10, 13)); + + bayesNet.push_back(boost::make_shared(3, 9, 12)); + bayesNet.push_back(boost::make_shared(2, 9, 12)); + bayesNet.push_back(boost::make_shared(1, 8, 12)); + bayesNet.push_back(boost::make_shared(0, 8, 12)); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree out of a Bayes net + OrderingUnordered ordering(bayesNet.keys()); + SymbolicBayesTreeUnordered bayesTree = *SymbolicFactorGraphUnordered(bayesNet).eliminateMultifrontal(ordering); + bayesTree.print("bayesTree: "); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); + } + + SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.roots().front(); + + { + // check shortcut P(S9||R) to root + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[9]; + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected; + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S8||R) to root + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[8]; + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(12, 14)); + EXPECT(assert_equal(expected, shortcut)); + } + + { + // check shortcut P(S4||R) to root + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected; + expected.push_back(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]; + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(12, 14)); + expected.push_back(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]; + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(12, 14)); + expected.push_back(boost::make_shared(8, 12, 14)); + EXPECT(assert_equal(expected, shortcut)); + } + + SymbolicBayesNetUnordered::shared_ptr actualJoint; + + // Check joint P(8,2) + if (false) { // TODO, not disjoint + actualJoint = bayesTree.jointBayesNet(8, 2); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(8)); + expected.push_back(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); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(2)); + expected.push_back(boost::make_shared(1, 2)); + EXPECT(assert_equal(expected, *actualJoint)); + } + + // Check joint P(2,6) + if (true) { + actualJoint = bayesTree.jointBayesNet(2, 6); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(6)); + expected.push_back(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); + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(6)); + expected.push_back(boost::make_shared(4, 6)); + EXPECT(assert_equal(expected, *actualJoint)); + } +} + +/* ************************************************************************* */ +TEST(SymbolicBayesTreeUnordered, forest_joint) +{ + // Create forest + SymbolicBayesTreeCliqueUnordered::shared_ptr root1 = MakeClique(list_of(1), 1); + SymbolicBayesTreeCliqueUnordered::shared_ptr root2 = MakeClique(list_of(2), 2); + SymbolicBayesTreeUnordered bayesTree; + bayesTree.insertRoot(root1); + bayesTree.insertRoot(root2); + + // Check joint + SymbolicBayesNetUnordered expected = list_of + (SymbolicConditionalUnordered(1)) + (SymbolicConditionalUnordered(2)); + SymbolicBayesNetUnordered actual = *bayesTree.jointBayesNet(1, 2); + + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* * + 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 + SymbolicFactorGraphUnordered 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); + + // Eliminate in numerical order 0..6 + OrderingUnordered ordering(smoother.keys()); + SymbolicBayesNetUnordered bayesNet = *smoother.eliminateSequential(ordering); + + if (debug) { + GTSAM_PRINT(bayesNet); + bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); + } + + // create a BayesTree + SymbolicBayesTreeUnordered bayesTree = *smoother.eliminateMultifrontal(ordering); + if (debug) { + GTSAM_PRINT(bayesTree); + bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); + } + + SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.roots().front(); + + { + // check shortcut P(S2||R) to root + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered 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 + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(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 + SymbolicBayesNetUnordered shortcut = c->shortcut(R); + SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(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 + SymbolicBayesTreeCliqueUnordered::shared_ptr cur; + SymbolicBayesTreeCliqueUnordered::shared_ptr root = MakeClique(list_of(11)(12), 2); + cur = root; + + cur->children += MakeClique(list_of(9)(10)(11)(12), 2); + cur->children.front()->parent_ = cur; + cur = root->children.front(); + + cur->children += MakeClique(list_of(7)(8)(11), 2); + cur->children.front()->parent_ = cur; + cur = root->children.front(); + + cur->children += MakeClique(list_of(5)(6)(7)(8), 2); + cur->children.front()->parent_ = cur; + cur = root->children.front(); + + cur->children += MakeClique(list_of(3)(4)(6), 2); + cur->children.front()->parent_ = cur; + cur = root->children.front(); + + cur->children += MakeClique(list_of(1)(2)(5), 2); + cur->children.front()->parent_ = cur; + cur = root->children.front(); + + // Create Bayes Tree + SymbolicBayesTreeUnordered bt; + bt.insertRoot(root); + if (debug) { + GTSAM_PRINT(bt); + bt.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); + } + + // Shortcut on 9 + { + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[9]; + SymbolicBayesNetUnordered shortcut = c->shortcut(root); + EXPECT(assert_equal(SymbolicBayesNetUnordered(), shortcut)); + } + + // Shortcut on 7 + { + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[7]; + SymbolicBayesNetUnordered shortcut = c->shortcut(root); + EXPECT(assert_equal(SymbolicBayesNetUnordered(), shortcut)); + } + + // Shortcut on 5 + { + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[5]; + SymbolicBayesNetUnordered shortcut = c->shortcut(root); + SymbolicBayesNetUnordered expected = list_of + (SymbolicConditionalUnordered(8, 11)) + (SymbolicConditionalUnordered(7, 8, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 3 + { + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[3]; + SymbolicBayesNetUnordered shortcut = c->shortcut(root); + SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(6, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Shortcut on 1 + { + SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[1]; + SymbolicBayesNetUnordered shortcut = c->shortcut(root); + SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(5, 11)); + EXPECT(assert_equal(expected, shortcut)); + } + + // Marginal on 5 + { + SymbolicFactorUnordered::shared_ptr actual = bt.marginalFactor(5); + EXPECT(assert_equal(SymbolicFactorUnordered(5), *actual, 1e-1)); + } + + // Shortcut on 6 + { + SymbolicFactorUnordered::shared_ptr actual = bt.marginalFactor(6); + EXPECT(assert_equal(SymbolicFactorUnordered(6), *actual, 1e-1)); + } + +} /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 707faa57c..525e25bed 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -52,10 +53,10 @@ TEST(SymbolicFactor, eliminate) { /* ************************************************************************* */ TEST(SymbolicFactor, EliminateSymbolic) { - const vector factors = list_of - (boost::make_shared(2,4,6)) - (boost::make_shared(1,2,5)) - (boost::make_shared(0,3)); + const SymbolicFactorGraphUnordered factors = list_of + (SymbolicFactorUnordered(2,4,6)) + (SymbolicFactorUnordered(1,2,5)) + (SymbolicFactorUnordered(0,3)); const SymbolicFactorUnordered expectedFactor(4,5,6); const SymbolicConditionalUnordered expectedConditional = diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index d613136a7..01f09ca14 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -49,13 +49,13 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) const OrderingUnordered order = list_of(0)(1); const SymbolicBayesNetUnordered expectedBayesNet = list_of - (boost::make_shared(0,1,2)) - (boost::make_shared(1,2,3,4)); + (SymbolicConditionalUnordered(0,1,2)) + (SymbolicConditionalUnordered(1,2,3,4)); const SymbolicFactorGraphUnordered expectedSfg = list_of - (boost::make_shared(2,3)) - (boost::make_shared(4,5)) - (boost::make_shared(2,3,4)); + (SymbolicFactorUnordered(2,3)) + (SymbolicFactorUnordered(4,5)) + (SymbolicFactorUnordered(2,3,4)); SymbolicBayesNetUnordered::shared_ptr actualBayesNet; SymbolicFactorGraphUnordered::shared_ptr actualSfg; @@ -88,11 +88,11 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) expectedBayesTree.insertRoot(boost::make_shared(root)); SymbolicFactorGraphUnordered expectedFactorGraph = list_of - (boost::make_shared(0,1)) - (boost::make_shared(0,2)) - (boost::make_shared(1,3)) - (boost::make_shared(2,3)) - (boost::make_shared(1)); + (SymbolicFactorUnordered(0,1)) + (SymbolicFactorUnordered(0,2)) + (SymbolicFactorUnordered(1,3)) + (SymbolicFactorUnordered(2,3)) + (SymbolicFactorUnordered(1)); SymbolicBayesTreeUnordered::shared_ptr actualBayesTree; SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph; @@ -103,6 +103,20 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); } +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) +{ + SymbolicBayesNetUnordered expectedBayesNet = list_of + (SymbolicConditionalUnordered(0, 1, 2)) + (SymbolicConditionalUnordered(1, 2, 3)) + (SymbolicConditionalUnordered(2, 3)) + (SymbolicConditionalUnordered(3)); + + SymbolicBayesNetUnordered actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( + OrderingUnordered(list_of(0)(1)(2)(3))); + EXPECT(assert_equal(expectedBayesNet, actual1)); +} + /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { SymbolicFactorGraphUnordered fg; @@ -219,9 +233,9 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) // create Bayes Net SymbolicBayesNetUnordered bayesNet; - bayesNet.add(SymbolicConditionalUnordered(0, 1, 2)); - bayesNet.add(SymbolicConditionalUnordered(1, 2)); - bayesNet.add(SymbolicConditionalUnordered(1)); + bayesNet += SymbolicConditionalUnordered(0, 1, 2); + bayesNet += SymbolicConditionalUnordered(1, 2); + bayesNet += SymbolicConditionalUnordered(1); // create actual factor graph from a Bayes Net SymbolicFactorGraphUnordered actual(bayesNet);