12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Combined changes - fixed up push_back and += functions in FactorGraphUnordered, enabled BayesTree marginals/shortcut code and unit tests, and cleaned up other template arguments, etc.

release/4.3a0
Richard Roberts 2013-07-23 02:17:09 +00:00
parent 25de39c481
commit 472f246b97
27 changed files with 998 additions and 986 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

275
gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h
View File

@ -17,62 +17,63 @@
#pragma once #pragma once
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h> #include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
bool BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::equals( bool BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::equals(
const DERIVED& other, double tol = 1e-9) const const DERIVED& other, double tol = 1e-9) const
{ {
return (!conditional_ && !other.conditional()) return (!conditional_ && !other.conditional())
|| conditional_->equals(*other.conditional(), tol); || conditional_->equals(*other.conditional(), tol);
} }
///* ************************************************************************* */ /* ************************************************************************* */
//template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
//std::vector<Key> std::vector<Key>
// BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::separator_setminus_B(derived_ptr B) const BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
//{ {
// FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
// FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
// std::vector<Key> S_setminus_B; std::vector<Key> S_setminus_B;
// std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
// indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
// return S_setminus_B; return S_setminus_B;
//} }
///* ************************************************************************* */
//template<class DERIVED, class FACTORGRAPH, class BAYESNET>
//std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut_indices(
// derived_ptr B, const FactorGraphType& p_Cp_B) const
//{
// gttic(shortcut_indices);
// FastSet<Key> allKeys = p_Cp_B.keys();
// FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
// std::vector<Key> S_setminus_B = separator_setminus_B(B);
// std::vector<Key> 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<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::print( std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::shortcut_indices(
const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{
gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
std::vector<Key> S_setminus_B = separator_setminus_B(B);
std::vector<Key> 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<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::print(
const std::string& s, const KeyFormatter& keyFormatter) const const std::string& s, const KeyFormatter& keyFormatter) const
{ {
conditional_->print(s, keyFormatter); conditional_->print(s, keyFormatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::treeSize() const { size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::treeSize() const {
size_t size = 1; size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children) BOOST_FOREACH(const derived_ptr& child, children)
size += child->treeSize(); size += child->treeSize();
@ -80,8 +81,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::numCachedSeparatorMarginals() const size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
{ {
if (!cachedSeparatorMarginal_) if (!cachedSeparatorMarginal_)
return 0; return 0;
@ -98,140 +99,104 @@ namespace gtsam {
// clique on the root. We can compute it recursively from the parent shortcut // 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 // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */ /* ************************************************************************* */
//template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
//BayesNetUnordered<CONDITIONAL> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut( typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::BayesNetType
// derived_ptr B, Eliminate function) const BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
//{ {
// gttic(BayesTreeCliqueBaseUnordered_shortcut); gttic(BayesTreeCliqueBaseUnordered_shortcut);
// We only calculate the shortcut when this clique is not B
// and when the S\B is not empty
std::vector<Key> 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 // Determine the variables we want to keepSet, S union B
// // and when the S\B is not empty std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
// std::vector<Key> S_setminus_B = separator_setminus_B(B);
// if (B.get() != this && !S_setminus_B.empty()) {
// // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph // Marginalize out everything except S union B
// derived_ptr parent(parent_.lock()); BayesNetType result = *p_Cp_B.marginalMultifrontalBayesNet(
// gttoc(BayesTreeCliqueBaseUnordered_shortcut); OrderingUnordered(keep), boost::none, function);
// 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)
// // Determine the variables we want to keepSet, S union B // Finally, we only want to have S\B variables in the Bayes net, so
// std::vector<Key> keep = shortcut_indices(B, p_Cp_B); size_t nrFrontals = S_setminus_B.size();
result.erase(result.begin() + nrFrontals, result.end());
// // Reduce the variable indices to start at zero return result;
// gttic(Reduce); }
// const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys()); else
// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); {
// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) { return BayesNetType();
// if(factor) factor->reduceWithInverse(inverseReduction); } }
// inverseReduction.applyInverse(keep); }
// gttoc(Reduce);
// // Create solver that will marginalize for us
// GenericSequentialSolver<FactorType> 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<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
// // Undo the reduction
// gttic(Undo_Reduce);
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
// if (factor) factor->permuteWithInverse(reduction); }
// result.permuteWithInverse(reduction);
// gttoc(Undo_Reduce);
// assertInvariants();
// return result;
// } else {
// return BayesNet<CONDITIONAL>();
// }
//}
/* ************************************************************************* */ /* ************************************************************************* */
// separator marginal, uses separator marginal of parent recursively // separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
//template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
//FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered< typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType
// DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
//{ {
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
// // Check if the Separator marginal was already calculated // Check if the Separator marginal was already calculated
// if (!cachedSeparatorMarginal_) { if (!cachedSeparatorMarginal_)
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); {
// // If this is the root, there is no separator gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
// if (R.get() == this) { // If this is the root, there is no separator
// // we are root, return empty if (parent_.expired() /*(if we're the root)*/)
// FactorGraph<FactorType> empty; {
// cachedSeparatorMarginal_ = empty; // we are root, return empty
// } else { FactorGraphType empty;
// // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) cachedSeparatorMarginal_ = empty;
// // initialize P(Cp) with the parent separator marginal }
// derived_ptr parent(parent_.lock()); else
// gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline {
// gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal); // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp) // initialize P(Cp) with the parent separator marginal
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal); derived_ptr parent(parent_.lock());
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
// // now add the parent conditional gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
// p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp) 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 // The variables we want to keepSet are exactly the ones in S
// gttic(Reduce); std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
// const Permutation reduction = internal::createReducingPermutation(p_Cp.keys()); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(OrderingUnordered(indicesS), boost::none, function);
// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); }
// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) { }
// if(factor) factor->reduceWithInverse(inverseReduction); }
// // The variables we want to keepSet are exactly the ones in S // return the shortcut P(S||B)
// sharedConditional p_F_S = this->conditional(); return *cachedSeparatorMarginal_; // return the cached version
// std::vector<Key> indicesS(p_F_S->beginParents(), p_F_S->endParents()); }
// inverseReduction.applyInverse(indicesS);
// gttoc(Reduce);
// // Create solver that will marginalize for us
// GenericSequentialSolver<FactorType> solver(p_Cp);
// cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
// // Undo the reduction
// gttic(Undo_Reduce);
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
// if (factor) factor->permuteWithInverse(reduction); }
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& 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
//}
/* ************************************************************************* */ /* ************************************************************************* */
// marginal2, uses separator marginal of parent recursively // marginal2, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* ************************************************************************* */
//template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
//FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered< typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::FactorGraphType
// DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
//{ {
// gttic(BayesTreeCliqueBaseUnordered_marginal2); gttic(BayesTreeCliqueBaseUnordered_marginal2);
// // initialize with separator marginal P(S) // initialize with separator marginal P(S)
// FactorGraph<FactorType> p_C(this->separatorMarginal(R, function)); FactorGraphType p_C = this->separatorMarginal(function);
// // add the conditional P(F|S) // add the conditional P(F|S)
// p_C.push_back(this->conditional()->toFactor()); p_C += boost::shared_ptr<FactorType>(this->conditional_);
// return p_C; return p_C;
//} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::deleteCachedShortcuts() { void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the // 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, // root are also generated. So, if this clique's cached shortcut is set,

42
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

@ -20,10 +20,10 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
namespace gtsam { namespace gtsam {
template<class CLIQUE> class BayesTreeUnordered;
}
namespace gtsam { // Forward declarations
template<class CLIQUE> class BayesTreeUnordered;
template<class GRAPH> struct EliminationTraits;
/** /**
* This is the base class for BayesTree cliques. The default and standard derived type is * 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 DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type. * @tparam CONDITIONAL The conditional type.
* \nosubgrouping */ * \nosubgrouping */
template<class DERIVED, class FACTORGRAPH, class BAYESNET> template<class DERIVED, class FACTORGRAPH>
class BayesTreeCliqueBaseUnordered class BayesTreeCliqueBaseUnordered
{ {
private: private:
typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET> This; typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH> This;
typedef DERIVED DerivedType; typedef DERIVED DerivedType;
typedef EliminationTraits<FACTORGRAPH> EliminationTraits;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr; typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr; typedef boost::shared_ptr<DerivedType> derived_ptr;
@ -51,7 +52,7 @@ namespace gtsam {
public: public:
typedef FACTORGRAPH FactorGraphType; typedef FACTORGRAPH FactorGraphType;
typedef BAYESNET BayesNetType; typedef typename EliminationTraits::BayesNetType BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType; typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional; typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename FactorGraphType::FactorType FactorType; typedef typename FactorGraphType::FactorType FactorType;
@ -110,14 +111,14 @@ namespace gtsam {
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
///** return the conditional P(S|Root) on the separator given the root */ /** return the conditional P(S|Root) on the separator given the root */
//BayesNetType shortcut(derived_ptr root, Eliminate function) const; BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraits::DefaultEliminate) const;
///** return the marginal P(S) on the separator */ /** return the marginal P(S) on the separator */
//FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const; FactorGraphType separatorMarginal(Eliminate function = EliminationTraits::DefaultEliminate) const;
///** return the marginal P(C) of the clique, using marginal caching */ /** return the marginal P(C) of the clique, using marginal caching */
//FactorGraphType marginal2(derived_ptr root, Eliminate function) const; FactorGraphType marginal2(Eliminate function = EliminationTraits::DefaultEliminate) const;
/** /**
* This deletes the cached shortcuts of all cliques (subtree) below this clique. * This deletes the cached shortcuts of all cliques (subtree) below this clique.
@ -132,18 +133,13 @@ namespace gtsam {
protected: protected:
///// Calculate set \f$ S \setminus B \f$ for shortcut calculations /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
//std::vector<Key> separator_setminus_B(derived_ptr B) const; std::vector<Key> separator_setminus_B(const derived_ptr& B) const;
///// Calculate set \f$ S_p \cap B \f$ for shortcut calculations /** Determine variable indices to keep in recursive separator shortcut calculation The factor
//std::vector<Key> parent_separator_intersection_B(derived_ptr B) const; * 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<Key> shortcut_indices(const 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<Key> shortcut_indices(derived_ptr B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }

343
gtsam/inference/BayesTreeUnordered-inst.h
View File

@ -22,10 +22,14 @@
#include <gtsam/inference/BayesTreeUnordered.h> #include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>
#include <fstream> #include <fstream>
using boost::assign::cref_list_of;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
@ -193,6 +197,24 @@ namespace gtsam {
return new_clique; return new_clique;
} }
/* ************************************************************************* */
namespace {
template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraphUnordered<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {
fg.push_back(clique->conditional_);
return 0;
}
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::addFactorsToGraph(FactorGraphUnordered<FactorType>& graph) const
{
// Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused
treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, graph, _1));
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique) void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
@ -364,201 +386,132 @@ namespace gtsam {
fillNodesIndex(subtree); // Populate nodes index fillNodesIndex(subtree); // Populate nodes index
} }
// /* ************************************************************************* */ /* ************************************************************************* */
// // First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
// /* ************************************************************************* */ /* ************************************************************************* */
// template<class CLIQUE> template<class CLIQUE>
// typename BayesTreeUnordered<CLIQUE>::sharedFactor BayesTreeUnordered<CLIQUE>::marginalFactor( typename BayesTreeUnordered<CLIQUE>::sharedConditional
// Key j, Eliminate function) const BayesTreeUnordered<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
// { {
// return boost::make_shared<FactorType>(); gttic(BayesTree_marginalFactor);
//// gttic(BayesTree_marginalFactor);
//// // get clique containing Index j
//// // get clique containing Index j sharedClique clique = this->clique(j);
//// sharedClique clique = this->clique(j);
//// // calculate or retrieve its marginal P(C) = P(F,S)
//// // calculate or retrieve its marginal P(C) = P(F,S) FactorGraphType cliqueMarginal = clique->marginal2(function);
////#ifdef OLD_SHORTCUT_MARGINALS
//// FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function); // Now, marginalize out everything that is not variable j
////#else BayesNetType marginalBN = *cliqueMarginal.marginalMultifrontalBayesNet(
//// FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function); OrderingUnordered(cref_list_of<1,Key>(j)), boost::none, function);
////#endif
//// // The Bayes net should contain only one conditional for variable j, so return it
//// // Reduce the variable indices to start at zero return marginalBN.front();
//// gttic(Reduce); }
//// const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys());
//// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); /* ************************************************************************* */
//// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) { // Find two cliques, their joint, then marginalizes
//// if(factor) factor->reduceWithInverse(inverseReduction); } /* ************************************************************************* */
//// gttoc(Reduce); template<class CLIQUE>
//// typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph
//// // now, marginalize out everything that is not variable j BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
//// GenericSequentialSolver<FactorType> solver(cliqueMarginal); {
//// boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function); gttic(BayesTree_joint);
//// return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
//// // Undo the reduction }
//// gttic(Undo_Reduce);
//// result->permuteWithInverse(reduction); /* ************************************************************************* */
//// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) { template<class CLIQUE>
//// if(factor) factor->permuteWithInverse(reduction); } typename BayesTreeUnordered<CLIQUE>::sharedBayesNet
//// gttoc(Undo_Reduce); BayesTreeUnordered<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
//// return result; {
// } gttic(BayesTree_jointBayesNet);
// // get clique C1 and C2
// /* ************************************************************************* */ sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
// template<class CLIQUE>
// typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::marginalBayesNet( gttic(Lowest_common_ancestor);
// Key j, Eliminate function) const // Find lowest common ancestor clique
// { sharedClique B; {
// return boost::make_shared<BayesNetType>(); // Build two paths to the root
// //gttic(BayesTree_marginalBayesNet); FastList<sharedClique> path1, path2; {
// sharedClique p = C1;
// //// calculate marginal as a factor graph while(p) {
// //FactorGraph<FactorType> fg; path1.push_front(p);
// //fg.push_back(this->marginalFactor(j,function)); p = p->parent();
// }
// //// Reduce the variable indices to start at zero } {
// //gttic(Reduce); sharedClique p = C2;
// //const Permutation reduction = internal::createReducingPermutation(fg.keys()); while(p) {
// //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); path2.push_front(p);
// //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) { p = p->parent();
// // if(factor) factor->reduceWithInverse(inverseReduction); } }
// //gttoc(Reduce); }
// // Find the path intersection
// //// eliminate factor graph marginal to a Bayes net typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
// //boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function); if(*p1 == *p2)
// B = *p1;
// //// Undo the reduction while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
// //gttic(Undo_Reduce); B = *p1;
// //bn->permuteWithInverse(reduction); ++p1;
// //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) { ++p2;
// // if(factor) factor->permuteWithInverse(reduction); } }
// //gttoc(Undo_Reduce); }
// //return bn; gttoc(Lowest_common_ancestor);
// }
// // Compute marginal on lowest common ancestor clique
// /* ************************************************************************* */ gttic(LCA_marginal);
// // Find two cliques, their joint, then marginalizes FactorGraphType p_B = B->marginal2(function);
// /* ************************************************************************* */ gttoc(LCA_marginal);
// template<class CLIQUE>
// typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph // Compute shortcuts of the requested cliques given the lowest common ancestor
// BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, Eliminate function) const { gttic(Clique_shortcuts);
// return boost::make_shared<FactorGraphType>(); BayesNetType p_C1_Bred = C1->shortcut(B, function);
// //gttic(BayesTree_joint); BayesNetType p_C2_Bred = C2->shortcut(B, function);
// gttoc(Clique_shortcuts);
// //// get clique C1 and C2
// //sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; // Factor the shortcuts to be conditioned on the full root
// // Get the set of variables to eliminate, which is C1\B.
// //gttic(Lowest_common_ancestor); gttic(Full_root_factoring);
// //// Find lowest common ancestor clique shared_ptr p_C1_B; {
// //sharedClique B; { std::vector<Index> C1_minus_B; {
// // // Build two paths to the root FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
// // FastList<sharedClique> path1, path2; { BOOST_FOREACH(const Index j, *B->conditional()) {
// // sharedClique p = C1; C1_minus_B_set.erase(j); }
// // while(p) { C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
// // path1.push_front(p); }
// // p = p->parent(); // Factor into C1\B | B.
// // } sharedFactorGraph temp_remaining;
// // } { boost::tie(p_C1_B, temp_remaining) =
// // sharedClique p = C2; FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(OrderingUnordered(C1_minus_B), function);
// // while(p) { }
// // path2.push_front(p); shared_ptr p_C2_B; {
// // p = p->parent(); std::vector<Index> C2_minus_B; {
// // } FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
// // } BOOST_FOREACH(const Index j, *B->conditional()) {
// // // Find the path intersection C2_minus_B_set.erase(j); }
// // B = this->root(); C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
// // typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin(); }
// // while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { // Factor into C2\B | B.
// // B = *p1; sharedFactorGraph temp_remaining;
// // ++p1; boost::tie(p_C2_B, temp_remaining) =
// // ++p2; FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(OrderingUnordered(C2_minus_B), function);
// // } }
// //} gttoc(Full_root_factoring);
// //gttoc(Lowest_common_ancestor);
// gttic(Variable_joint);
// //// Compute marginal on lowest common ancestor clique // Build joint on all involved variables
// //gttic(LCA_marginal); FactorGraphType p_BC1C2;
// //FactorGraph<FactorType> p_B = B->marginal2(this->root(), function); p_BC1C2 += p_B;
// //gttoc(LCA_marginal); p_BC1C2 += *p_C1_B;
// p_BC1C2 += *p_C2_B;
// //// Compute shortcuts of the requested cliques given the lowest common ancestor if(C1 != B)
// //gttic(Clique_shortcuts); p_BC1C2 += C1->conditional();
// //BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function); if(C2 != B)
// //BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function); p_BC1C2 += C2->conditional();
// //gttoc(Clique_shortcuts);
// // now, marginalize out everything that is not variable j1 or j2
// //// Factor the shortcuts to be conditioned on the full root return p_BC1C2.marginalMultifrontalBayesNet(OrderingUnordered(cref_list_of<2,Key>(j1)(j2)), boost::none, function);
// //// Get the set of variables to eliminate, which is C1\B. }
// //gttic(Full_root_factoring);
// //sharedConditional p_C1_B; {
// // std::vector<Index> C1_minus_B; {
// // FastSet<Index> 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<FactorType> temp_remaining;
// // boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
// //}
// //sharedConditional p_C2_B; {
// // std::vector<Index> C2_minus_B; {
// // FastSet<Index> 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<FactorType> temp_remaining;
// // boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
// //}
// //gttoc(Full_root_factoring);
//
// //gttic(Variable_joint);
// //// Build joint on all involved variables
// //FactorGraph<FactorType> 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<FactorType>& factor, p_BC1C2) {
// // if(factor) factor->reduceWithInverse(inverseReduction); }
// //std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
// //gttoc(Reduce);
//
// //// now, marginalize out everything that is not variable j
// //GenericSequentialSolver<FactorType> solver(p_BC1C2);
// //boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function);
//
// //// Undo the reduction
// //gttic(Undo_Reduce);
// //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, *result) {
// // if(factor) factor->permuteWithInverse(reduction); }
// //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
// // if(factor) factor->permuteWithInverse(reduction); }
// //gttoc(Undo_Reduce);
// //return result;
// }
//
// /* ************************************************************************* */
// template<class CLIQUE>
// typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::jointBayesNet(
// Key j1, Key j2, Eliminate function) const
// {
// return boost::make_shared<BayesNetType>();
// //// eliminate factor graph marginal to a Bayes net
// //return GenericSequentialSolver<FactorType> (
// // *this->joint(j1, j2, function)).eliminate(function);
// }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>

44
gtsam/inference/BayesTreeUnordered.h
View File

@ -28,6 +28,9 @@
namespace gtsam { namespace gtsam {
// Forward declarations
template<class FACTOR> class FactorGraphUnordered;
/** /**
* Bayes tree * Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, * @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 typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph; typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate; typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraits EliminationTraits;
/** A convenience class for a list of shared cliques */ /** A convenience class for a list of shared cliques */
struct Cliques : public FastList<sharedClique> { struct Cliques : public FastList<sharedClique> {
@ -135,7 +139,7 @@ namespace gtsam {
const Nodes& nodes() const { return nodes_; } const Nodes& nodes() const { return nodes_; }
/** Access node by variable */ /** 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 */ /** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; } const std::vector<sharedClique>& roots() const { return roots_; }
@ -155,27 +159,24 @@ namespace gtsam {
/** Collect number of cliques with cached separator marginals */ /** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
///** return marginal on any variable */ /** Return marginal on any variable. Note that this actually returns a conditional, for which a
//sharedFactor marginalFactor(Key j, Eliminate function) const; * 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 * return joint on two variables
// * NOTE: this function calls marginal, and then eliminates it into a Bayes Net * Limitation: can only calculate joint if cliques are disjoint or one of them is root
// * This is more expensive than the above function */
// */ sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
//sharedBayesNet marginalBayesNet(Key j, Eliminate function) const;
///** /**
// * return joint on two variables * return joint on two variables as a BayesNet
// * Limitation: can only calculate joint if cliques are disjoint or one of them is root * Limitation: can only calculate joint if cliques are disjoint or one of them is root
// */ */
//sharedFactorGraph joint(Index j1, Index j2, Eliminate function) const; sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) 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;
/** /**
* Read only with side effects * Read only with side effects
@ -226,6 +227,9 @@ namespace gtsam {
/** add a clique (top down) */ /** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** Add all cliques in this BayesTree to the specified factor graph */
void addFactorsToGraph(FactorGraphUnordered<FactorType>& graph) const;
protected: protected:
/** private helper method for saving the Tree to a text file in GraphViz format */ /** private helper method for saving the Tree to a text file in GraphViz format */

52
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -113,5 +113,57 @@ namespace gtsam {
} }
} }
/* ************************************************************************* */
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const OrderingUnordered&, const std::vector<Key>&> 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<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminated =
eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
if(const OrderingUnordered* varsAsOrdering = boost::get<const OrderingUnordered&>(&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<const OrderingUnordered&>(&variables) != 0);
OrderingUnordered totalOrdering =
OrderingUnordered::COLAMDConstrainedLast(*variableIndex,
boost::get<const std::vector<Key>&>(variables), unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = boost::get<const std::vector<Key>&>(variables).size(); // Works because OrderingUnordered derives from std::vector<Key>
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()));
}
}
} }

18
gtsam/inference/EliminateableFactorGraph.h
View File

@ -20,6 +20,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <boost/variant.hpp>
#include <gtsam/inference/OrderingUnordered.h> #include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h> #include <gtsam/inference/VariableIndexUnordered.h>
@ -182,6 +183,23 @@ namespace gtsam {
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; 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<Key> 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<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const OrderingUnordered&, const std::vector<Key>&> variables,
OptionalOrdering marginalizedVariableOrdering = boost::none,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
private: private:
// Access the derived factor graph class // Access the derived factor graph class

20
gtsam/inference/FactorGraphUnordered-inst.h
View File

@ -25,6 +25,7 @@
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h> #include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/BayesTreeUnordered.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -64,25 +65,6 @@ namespace gtsam {
return true; return true;
} }
/* ************************************************************************* */
namespace {
template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraphUnordered<FACTOR>* fg, const boost::shared_ptr<CLIQUE>& clique) {
fg->push_back(clique->conditional_);
return 0;
}
}
/* ************************************************************************* */
template<class FACTOR>
template<class CLIQUE>
void FactorGraphUnordered<FACTOR>::push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree)
{
// Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused
treeTraversal::DepthFirstForest(bayesTree, data, boost::bind(&_pushClique<FACTOR,CLIQUE>, this, _1));
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
size_t FactorGraphUnordered<FACTOR>::nrFactors() const { size_t FactorGraphUnordered<FACTOR>::nrFactors() const {

166
gtsam/inference/FactorGraphUnordered.h
View File

@ -24,15 +24,51 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <type_traits> #include <type_traits>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/inference/BayesTreeUnordered.h>
namespace gtsam { namespace gtsam {
// Forward declarations
template<class CLIQUE> class BayesTreeUnordered;
/** Helper */
template<class C>
class CRefCallPushBack
{
C& obj;
public:
CRefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class RefCallPushBack
{
C& obj;
public:
RefCallPushBack(C& obj) : obj(obj) {}
template<typename A>
void operator()(A& a) { obj.push_back(a); }
};
/** Helper */
template<class C>
class CRefCallAddCopy
{
C& obj;
public:
CRefCallAddCopy(C& obj) : obj(obj) {}
template<typename A>
void operator()(const A& a) { obj.addCopy(a); }
};
/** /**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes. * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around. * In this class, however, only factor nodes are kept around.
@ -44,6 +80,7 @@ namespace gtsam {
public: public:
typedef FACTOR FactorType; ///< factor type typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef sharedFactor value_type;
typedef typename std::vector<sharedFactor>::iterator iterator; typedef typename std::vector<sharedFactor>::iterator iterator;
typedef typename std::vector<sharedFactor>::const_iterator const_iterator; typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
@ -70,7 +107,7 @@ namespace gtsam {
/** Construct from container of factors (shared_ptr or plain objects) */ /** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER> template<class CONTAINER>
explicit FactorGraphUnordered(const CONTAINER& factors) { push_back(factors.begin(), factors.end()); } explicit FactorGraphUnordered(const CONTAINER& factors) { push_back(factors); }
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
@ -112,18 +149,13 @@ namespace gtsam {
/** Add a factor directly using a shared_ptr */ /** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) { typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); } factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid /** Add a factor directly using a shared_ptr */
* the copy). */ void push_back(const sharedFactor& factor) {
template<class DERIVEDFACTOR> factors_.push_back(factor); }
void add(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end()); }
/** push back many factors with an iterator over shared_ptr (factors are not copied) */ /** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR> template<typename ITERATOR>
@ -131,42 +163,92 @@ namespace gtsam {
push_back(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor); } factors_.insert(end(), firstFactor, lastFactor); }
/** push back many factors as shared_ptr's in a container (factors are not copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::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<class CLIQUE>
typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
push_back(const BayesTreeUnordered<CLIQUE>& bayesTree) {
bayesTree.addFactorsToGraph(*this);
}
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
//template<class T>
//boost::assign::list_inserter<boost::function<void(const T&)> >
// operator+=(const T& factorOrContainer)
//{
// return boost::assign::make_list_inserter(
// boost::bind(&This::push_back<T>, this, _1));
//}
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
boost::assign::list_inserter<RefCallPushBack<This> > >::type
operator+=(boost::shared_ptr<DERIVEDFACTOR>& factor) {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this));
}
template<class FACTOR_OR_CONTAINER>
boost::assign::list_inserter<CRefCallPushBack<This> >
operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
}
///** Add a factor directly using a shared_ptr */
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const sharedFactor& factor) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
///** push back many factors as shared_ptr's in a container (factors are not copied) */
//template<typename CONTAINER>
//typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value,
// boost::assign::list_inserter<CRefCallPushBack<This> > >::type
// operator+=(const CONTAINER& container) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*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<class CLIQUE>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator+=(const BayesTreeUnordered<CLIQUE>& bayesTree) {
// return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this));
//}
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
* the copy). */
template<class DERIVEDFACTOR>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor)); }
/** push back many factors with an iterator over plain factors (factors are copied) */ /** push back many factors with an iterator over plain factors (factors are copied) */
template<typename ITERATOR> template<typename ITERATOR>
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
for(ITERATOR f = firstFactor; f != lastFactor; ++f) 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). */ /** push back many factors as non-pointer objects in a container (factors are copied) */
template<class DERIVEDFACTOR> template<typename CONTAINER>
void push_back(const DERIVEDFACTOR& factor) { typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
add(factor); } push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
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<class CLIQUE>
void push_back_bayesTree(const BayesTreeUnordered<CLIQUE>& bayesTree);
public:
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
template<class T>
boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, T>
operator+=(const T& factorOrContainer)
{
return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(factorOrContainer);
} }
/** //template<class FACTOR_OR_CONTAINER>
* @brief Add a vector of derived factors //boost::assign::list_inserter<CRefCallPushBack<This> >
* @param factors to add // operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) {
*/ // return boost::assign::make_list_inserter(CRefCallAddCopy<This>(*this));
//template<typename DERIVEDFACTOR>
//void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
// factors_.insert(end(), factors.begin(), factors.end());
//} //}
/// @} /// @}
@ -246,6 +328,12 @@ namespace gtsam {
/** replace a factor by index */ /** replace a factor by index */
void replace(size_t index, sharedFactor factor) { at(index) = factor; } 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 /// @name Advanced Interface
/// @{ /// @{

4
gtsam/linear/GaussianBayesTreeUnordered.h
View File

@ -33,11 +33,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/** A clique in a GaussianBayesTree */ /** A clique in a GaussianBayesTree */
class GTSAM_EXPORT GaussianBayesTreeCliqueUnordered : class GTSAM_EXPORT GaussianBayesTreeCliqueUnordered :
public BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered> public BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered>
{ {
public: public:
typedef GaussianBayesTreeCliqueUnordered This; typedef GaussianBayesTreeCliqueUnordered This;
typedef BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered> Base; typedef BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr; typedef boost::weak_ptr<This> weak_ptr;
GaussianBayesTreeCliqueUnordered() {} GaussianBayesTreeCliqueUnordered() {}

3
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -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 // check if the size of the parents_ map is the same
if (parents().size() != c.parents().size()) if (parents().size() != c.parents().size())
return false; return false;

6
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

@ -40,12 +40,6 @@ namespace gtsam {
return Base::equals(fg, tol); return Base::equals(fg, tol);
} }
/* ************************************************************************* */
void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree)
{
Base::push_back_bayesTree(bayesTree);
}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const { GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
FastSet<Key> keys; FastSet<Key> keys;

9
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -75,17 +75,13 @@ namespace gtsam {
/** Default constructor */ /** Default constructor */
GaussianFactorGraphUnordered() {} GaussianFactorGraphUnordered() {}
/** Constructor that receives a BayesTree */
GaussianFactorGraphUnordered(const GaussianBayesTreeUnordered& gbt) {
push_back_bayesTree(gbt); }
/** Construct from iterator over factors */ /** Construct from iterator over factors */
template<typename ITERATOR> template<typename ITERATOR>
GaussianFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} GaussianFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
/** Construct from container of factors (shared_ptr or plain objects) */ /** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER> template<class CONTAINER>
GaussianFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} explicit GaussianFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {}
/** Implicit copy/downcast constructor to override explicit template container constructor */ /** Implicit copy/downcast constructor to override explicit template container constructor */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
@ -131,9 +127,6 @@ namespace gtsam {
void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) { void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) {
add(JacobianFactorUnordered(terms,b,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 * Return the set of variables involved in the factors (computes a set
* union). * union).

22
gtsam/symbolic/SymbolicBayesNetUnordered.cpp
View File

@ -16,9 +16,11 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h> #include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h> #include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <fstream>
namespace gtsam { namespace gtsam {
@ -28,5 +30,23 @@ namespace gtsam {
return Base::equals(bn, tol); 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();
}
} }

6
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

@ -18,14 +18,12 @@
#pragma once #pragma once
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h> #include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
namespace gtsam { namespace gtsam {
// Forward declarations
class SymbolicConditionalUnordered;
/** Symbolic Bayes Net /** Symbolic Bayes Net
* \nosubgrouping * \nosubgrouping
*/ */
@ -70,6 +68,8 @@ namespace gtsam {
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
}; };

4
gtsam/symbolic/SymbolicBayesTreeUnordered.h
View File

@ -31,11 +31,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/// A clique in a SymbolicBayesTree /// A clique in a SymbolicBayesTree
class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered : class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered :
public BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered> public BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered>
{ {
public: public:
typedef SymbolicBayesTreeCliqueUnordered This; typedef SymbolicBayesTreeCliqueUnordered This;
typedef BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered> Base; typedef BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr; typedef boost::weak_ptr<This> weak_ptr;
SymbolicBayesTreeCliqueUnordered() {} SymbolicBayesTreeCliqueUnordered() {}

12
gtsam/symbolic/SymbolicConditionalUnordered.cpp
View File

@ -22,4 +22,16 @@ namespace gtsam {
using namespace std; 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);
}
} }

11
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

@ -64,17 +64,17 @@ namespace gtsam {
/** Named constructor from an arbitrary number of keys and frontals */ /** Named constructor from an arbitrary number of keys and frontals */
template<typename ITERATOR> template<typename ITERATOR>
static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) static SymbolicConditionalUnordered FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
{ {
SymbolicConditionalUnordered result; SymbolicConditionalUnordered result;
(BaseFactor&)result = BaseFactor::FromIterator(firstKey, lastKey); (BaseFactor&)result = BaseFactor::FromIterators(firstKey, lastKey);
result.nrFrontals_ = nrFrontals; result.nrFrontals_ = nrFrontals;
return result; } return result; }
/** Named constructor from an arbitrary number of keys and frontals */ /** Named constructor from an arbitrary number of keys and frontals */
template<class CONTAINER> template<class CONTAINER>
static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals) { 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() {} virtual ~SymbolicConditionalUnordered() {}
@ -83,11 +83,10 @@ namespace gtsam {
/// @name Testable /// @name Testable
/** Print with optional formatter */ /** Print with optional formatter */
void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
BaseConditional::print(str, keyFormatter); }
/** Check equality */ /** 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;
/// @} /// @}

17
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

@ -32,22 +32,18 @@ namespace gtsam {
typedef SymbolicEliminationTreeUnordered This; ///< This class typedef SymbolicEliminationTreeUnordered This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
/** /** Build the elimination tree of a factor graph using pre-computed column structure.
* 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 factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not * @param structure The set of factors involving each variable. If this is not precomputed,
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&) * you can call the Create(const FactorGraph<DERIVEDFACTOR>&) named constructor instead.
* named constructor instead. * @return The elimination tree */
* @return The elimination tree
*/
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph,
const VariableIndexUnordered& structure, const OrderingUnordered& order); const VariableIndexUnordered& structure, const OrderingUnordered& order);
/** Build the elimination tree of a factor graph. Note that this has to compute the column /** 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 * structure as a VariableIndex, so if you already have this precomputed, use the other
* constructor instead. * constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree * @param factorGraph The factor graph for which to build the elimination tree */
*/
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph,
const OrderingUnordered& order); const OrderingUnordered& order);
@ -64,6 +60,9 @@ namespace gtsam {
private: private:
/** Private default constructor for testing */
SymbolicEliminationTreeUnordered() {}
friend class ::EliminationTreeUnorderedTester; friend class ::EliminationTreeUnorderedTester;
}; };

5
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -55,11 +55,6 @@ namespace gtsam {
push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3,key4)); push_back(boost::make_shared<SymbolicFactorUnordered>(key1,key2,key3,key4));
} }
/* ************************************************************************* */
void SymbolicFactorGraphUnordered::push_back_bayesTree(const SymbolicBayesTreeUnordered& bayesTree) {
Base::push_back_bayesTree(bayesTree);
}
// /* ************************************************************************* */ // /* ************************************************************************* */
// std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph> // std::pair<SymbolicFactorGraph::sharedConditional, SymbolicFactorGraph>
// SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const // SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const

9
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -69,17 +69,13 @@ namespace gtsam {
/** Construct empty factor graph */ /** Construct empty factor graph */
SymbolicFactorGraphUnordered() {} SymbolicFactorGraphUnordered() {}
/** Constructor from a BayesTree */
SymbolicFactorGraphUnordered(const SymbolicBayesTreeUnordered& bayesTree) {
push_back_bayesTree(bayesTree); }
/** Construct from iterator over factors */ /** Construct from iterator over factors */
template<typename ITERATOR> template<typename ITERATOR>
SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
/** Construct from container of factors (shared_ptr or plain objects) */ /** Construct from container of factors (shared_ptr or plain objects) */
template<class CONTAINER> template<class CONTAINER>
SymbolicFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {} explicit SymbolicFactorGraphUnordered(const CONTAINER& factors) : Base(factors) {}
/** Implicit copy/downcast constructor to override explicit template container constructor */ /** Implicit copy/downcast constructor to override explicit template container constructor */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
@ -109,9 +105,6 @@ namespace gtsam {
/** Push back 4-way factor */ /** Push back 4-way factor */
void push_factor(Key key1, Key key2, Key key3, Key key4); 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);
/// @} /// @}
}; };

4
gtsam/symbolic/SymbolicFactorUnordered.cpp
View File

@ -56,7 +56,7 @@ namespace gtsam {
boost::make_shared<SymbolicConditionalUnordered>( boost::make_shared<SymbolicConditionalUnordered>(
SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)), SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)),
boost::make_shared<SymbolicFactorUnordered>( boost::make_shared<SymbolicFactorUnordered>(
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 SymbolicFactorUnordered::eliminate(const OrderingUnordered& keys) const
{ {
SymbolicFactorGraphUnordered graph; SymbolicFactorGraphUnordered graph;
graph.add(*this); graph += *this; // TODO: Is there a way to avoid copying this factor?
return EliminateSymbolicUnordered(graph, keys); return EliminateSymbolicUnordered(graph, keys);
} }

2
gtsam/symbolic/SymbolicFactorUnordered.h
View File

@ -94,7 +94,7 @@ namespace gtsam {
public: public:
/** Constructor from a collection of keys */ /** Constructor from a collection of keys */
template<typename KEYITERATOR> template<typename KEYITERATOR>
static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) { static SymbolicFactorUnordered FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) {
return SymbolicFactorUnordered(Base::FromIterators(beginKey, endKey)); } return SymbolicFactorUnordered(Base::FromIterators(beginKey, endKey)); }
/** Constructor from a collection of keys - compatible with boost::assign::list_of and /** Constructor from a collection of keys - compatible with boost::assign::list_of and

4
gtsam/symbolic/tests/symbolicExampleGraphs.h
View File

@ -21,6 +21,10 @@
#pragma once #pragma once
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h> #include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>

7
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h> #include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -80,13 +81,13 @@ TEST( SymbolicBayesNet, combine )
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesNet, saveGraph) { TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNetUnordered bn; SymbolicBayesNetUnordered bn;
bn.add(SymbolicConditionalUnordered(_A_, _B_)); bn += SymbolicConditionalUnordered(_A_, _B_);
std::vector<Index> keys; std::vector<Index> keys;
keys.push_back(_B_); keys.push_back(_B_);
keys.push_back(_C_); keys.push_back(_C_);
keys.push_back(_D_); keys.push_back(_D_);
bn.add(SymbolicConditionalUnordered::FromKeys(keys,2)); bn += SymbolicConditionalUnordered::FromKeys(keys,2);
bn.add(SymbolicConditionalUnordered(_D_)); bn += SymbolicConditionalUnordered(_D_);
bn.saveGraph("SymbolicBayesNet.dot"); bn.saveGraph("SymbolicBayesNet.dot");
} }

826
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -18,6 +18,7 @@
*/ */
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h> #include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h> #include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
@ -33,9 +34,10 @@ using namespace gtsam::symbol_shorthand;
static bool debug = false; static bool debug = false;
/* ************************************************************************* */ namespace {
// Conditionals for ASIA example from the tutorial with A and D evidence /* ************************************************************************* */
static SymbolicConditionalUnordered::shared_ptr // Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditionalUnordered::shared_ptr
B(new SymbolicConditionalUnordered(_B_)), B(new SymbolicConditionalUnordered(_B_)),
L(new SymbolicConditionalUnordered(_L_, _B_)), L(new SymbolicConditionalUnordered(_L_, _B_)),
E(new SymbolicConditionalUnordered(_E_, _L_, _B_)), E(new SymbolicConditionalUnordered(_E_, _L_, _B_)),
@ -43,11 +45,22 @@ static SymbolicConditionalUnordered::shared_ptr
T(new SymbolicConditionalUnordered(_T_, _E_, _L_)), T(new SymbolicConditionalUnordered(_T_, _E_, _L_)),
X(new SymbolicConditionalUnordered(_X_, _E_)); X(new SymbolicConditionalUnordered(_X_, _E_));
// Cliques // Cliques
static SymbolicConditionalUnordered::shared_ptr ELB( SymbolicConditionalUnordered::shared_ptr ELB(
boost::make_shared<SymbolicConditionalUnordered>( boost::make_shared<SymbolicConditionalUnordered>(
SymbolicConditionalUnordered::FromKeys(list_of(_E_)(_L_)(_B_), 3))); SymbolicConditionalUnordered::FromKeys(list_of(_E_)(_L_)(_B_), 3)));
/* ************************************************************************* */
// Helper function for below
template<typename KEYS>
SymbolicBayesTreeCliqueUnordered::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals)
{
return boost::make_shared<SymbolicBayesTreeCliqueUnordered>(
boost::make_shared<SymbolicConditionalUnordered>(
SymbolicConditionalUnordered::FromKeys(keys, nrFrontals)));
}
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE( SymbolicBayesTree, constructor ) TEST_UNSAFE( SymbolicBayesTree, constructor )
{ {
@ -204,76 +217,68 @@ TEST(SymbolicBayesTree, clear)
// expectedOrphans += bayesTree[_T_], bayesTree[_X_]; // expectedOrphans += bayesTree[_T_], bayesTree[_X_];
// CHECK(assert_equal(expectedOrphans, orphans)); // CHECK(assert_equal(expectedOrphans, orphans));
//} //}
//
//void getAllCliques(const SymbolicBayesTreeUnordered::sharedClique& subtree, SymbolicBayesTreeUnordered::Cliques& cliques) { void getAllCliques(const SymbolicBayesTreeUnordered::sharedClique& subtree, SymbolicBayesTreeUnordered::Cliques& cliques) {
// // Check if subtree exists // Check if subtree exists
// if (subtree) { if (subtree) {
// cliques.push_back(subtree); cliques.push_back(subtree);
// // Recursive call over all child cliques // Recursive call over all child cliques
// BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& childClique, subtree->children()) { BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& childClique, subtree->children) {
// getAllCliques(childClique,cliques); getAllCliques(childClique,cliques);
// } }
// } }
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( BayesTree, shortcutCheck ) TEST( BayesTree, shortcutCheck )
//{ {
// const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0;
// SymbolicConditionalUnordered::shared_ptr SymbolicFactorGraphUnordered chain = list_of
// A(new SymbolicConditionalUnordered(_A_)), (SymbolicFactorUnordered(_A_))
// B(new SymbolicConditionalUnordered(_B_, _A_)), (SymbolicFactorUnordered(_B_, _A_))
// C(new SymbolicConditionalUnordered(_C_, _A_)), (SymbolicFactorUnordered(_C_, _A_))
// D(new SymbolicConditionalUnordered(_D_, _C_)), (SymbolicFactorUnordered(_D_, _C_))
// E(new SymbolicConditionalUnordered(_E_, _B_)), (SymbolicFactorUnordered(_E_, _B_))
// F(new SymbolicConditionalUnordered(_F_, _E_)), (SymbolicFactorUnordered(_F_, _E_))
// G(new SymbolicConditionalUnordered(_G_, _F_)); (SymbolicFactorUnordered(_G_, _F_));
// SymbolicBayesTreeUnordered bayesTree; SymbolicBayesTreeUnordered bayesTree = *chain.eliminateMultifrontal(
//// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; OrderingUnordered(list_of(_A_)(_B_)(_C_)(_D_)(_E_)(_F_)));
// SymbolicBayesTreeUnordered::insert(bayesTree, A);
// SymbolicBayesTreeUnordered::insert(bayesTree, B); //bayesTree.print("BayesTree");
// SymbolicBayesTreeUnordered::insert(bayesTree, C); //bayesTree.saveGraph("BT1.dot");
// SymbolicBayesTreeUnordered::insert(bayesTree, D);
// SymbolicBayesTreeUnordered::insert(bayesTree, E); SymbolicBayesTreeUnordered::sharedClique rootClique = bayesTree.roots().front();
// SymbolicBayesTreeUnordered::insert(bayesTree, F); //rootClique->printTree();
// SymbolicBayesTreeUnordered::insert(bayesTree, G); SymbolicBayesTreeUnordered::Cliques allCliques;
// getAllCliques(rootClique,allCliques);
// //bayesTree.print("BayesTree");
// //bayesTree.saveGraph("BT1.dot"); BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) {
// //clique->print("Clique#");
// SymbolicBayesTreeUnordered::sharedClique rootClique= bayesTree.root(); SymbolicBayesNetUnordered bn = clique->shortcut(rootClique);
// //rootClique->printTree(); //bn.print("Shortcut:\n");
// SymbolicBayesTreeUnordered::Cliques allCliques; //cout << endl;
// getAllCliques(rootClique,allCliques); }
//
// BayesNet<SymbolicConditionalUnordered> bn; // 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) { // BOOST_FOREACH(SymbolicBayesTreeUnordered::sharedClique& clique, allCliques) {
// //clique->print("Clique#"); // clique->print("Clique#");
// bn = clique->shortcut(rootClique, &EliminateSymbolic); // if(clique->cachedShortcut()){
// //bn.print("Shortcut:\n"); // bn = clique->cachedShortcut().get();
// //cout << endl; // bn.print("Shortcut:\n");
// } // }
// // else
// // Check if all the cached shortcuts are cleared // cout << "Not Initialized" << endl;
// rootClique->deleteCachedShortcuts(); // cout << endl;
// 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 ) //TEST( BayesTree, removeTop )
//{ //{
@ -372,98 +377,6 @@ TEST(SymbolicBayesTree, clear)
// CHECK(orphans.size() == 0); // 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<Index> 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<Index> 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 //// * x2 - x3 - x4 - x5
@ -519,291 +432,306 @@ TEST(SymbolicBayesTree, clear)
//// CHECK(assert_equal(expected, actual)); //// CHECK(assert_equal(expected, actual));
//// ////
////} ////}
//
///* ************************************************************************* */ /* ************************************************************************* */
// TEST( SymbolicBayesTreeUnordered, thinTree ) {
//TEST_UNSAFE( SymbolicBayesTreeUnordered, thinTree ) {
// // create a thin-tree Bayesnet, a la Jean-Guillaume
// // create a thin-tree Bayesnet, a la Jean-Guillaume SymbolicBayesNetUnordered bayesNet;
// SymbolicBayesNet bayesNet; bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(14));
// bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(13, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(13, 14)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(12, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(12, 14));
// bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(11, 13, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(11, 13, 14)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(10, 13, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(10, 13, 14)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(9, 12, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(9, 12, 14)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(8, 12, 14));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(8, 12, 14));
// bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(7, 11, 13));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(7, 11, 13)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(6, 11, 13));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(6, 11, 13)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(5, 10, 13));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(5, 10, 13)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(4, 10, 13));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(4, 10, 13));
// bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(3, 9, 12));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(3, 9, 12)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(2, 9, 12));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(2, 9, 12)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(1, 8, 12));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(1, 8, 12)); bayesNet.push_back(boost::make_shared<SymbolicConditionalUnordered>(0, 8, 12));
// bayesNet.push_front(boost::make_shared<SymbolicConditionalUnordered>(0, 8, 12));
// if (debug) {
// if (debug) { GTSAM_PRINT(bayesNet);
// GTSAM_PRINT(bayesNet); bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
// bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); }
// }
// // create a BayesTree out of a Bayes net
// // create a BayesTree out of a Bayes net OrderingUnordered ordering(bayesNet.keys());
// SymbolicBayesTreeUnordered bayesTree(bayesNet); SymbolicBayesTreeUnordered bayesTree = *SymbolicFactorGraphUnordered(bayesNet).eliminateMultifrontal(ordering);
// if (debug) { bayesTree.print("bayesTree: ");
// GTSAM_PRINT(bayesTree); if (debug) {
// bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); GTSAM_PRINT(bayesTree);
// } bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot");
// }
// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.root();
// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.roots().front();
// {
// // check shortcut P(S9||R) to root {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[9]; // check shortcut P(S9||R) to root
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[9];
// SymbolicBayesNet expected; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// EXPECT(assert_equal(expected, shortcut)); SymbolicBayesNetUnordered expected;
// } EXPECT(assert_equal(expected, shortcut));
// }
// {
// // check shortcut P(S8||R) to root {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[8]; // check shortcut P(S8||R) to root
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[8];
// SymbolicBayesNet expected; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(12, 14)); SymbolicBayesNetUnordered expected;
// EXPECT(assert_equal(expected, shortcut)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(12, 14));
// } EXPECT(assert_equal(expected, shortcut));
// }
// {
// // check shortcut P(S4||R) to root {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; // check shortcut P(S4||R) to root
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4];
// SymbolicBayesNet expected; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(10, 13, 14)); SymbolicBayesNetUnordered expected;
// EXPECT(assert_equal(expected, shortcut)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(10, 13, 14));
// } EXPECT(assert_equal(expected, shortcut));
// }
// {
// // check shortcut P(S2||R) to root {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2]; // check shortcut P(S2||R) to root
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2];
// SymbolicBayesNet expected; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(12, 14)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(9, 12, 14)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(12, 14));
// EXPECT(assert_equal(expected, shortcut)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(9, 12, 14));
// } EXPECT(assert_equal(expected, shortcut));
// }
// {
// // check shortcut P(S0||R) to root {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[0]; // check shortcut P(S0||R) to root
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[0];
// SymbolicBayesNet expected; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(12, 14)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(8, 12, 14)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(12, 14));
// EXPECT(assert_equal(expected, shortcut)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(8, 12, 14));
// } EXPECT(assert_equal(expected, shortcut));
// }
// SymbolicBayesNet::shared_ptr actualJoint;
// SymbolicBayesNetUnordered::shared_ptr actualJoint;
// // Check joint P(8,2)
// if (false) { // TODO, not disjoint // Check joint P(8,2)
// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic); if (false) { // TODO, not disjoint
// SymbolicBayesNet expected; actualJoint = bayesTree.jointBayesNet(8, 2);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(8)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(2, 8)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(8));
// EXPECT(assert_equal(expected, *actualJoint)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2, 8));
// } EXPECT(assert_equal(expected, *actualJoint));
// }
// // Check joint P(1,2)
// if (false) { // TODO, not disjoint // Check joint P(1,2)
// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic); if (false) { // TODO, not disjoint
// SymbolicBayesNet expected; actualJoint = bayesTree.jointBayesNet(1, 2);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(2)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(1, 2)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2));
// EXPECT(assert_equal(expected, *actualJoint)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(1, 2));
// } EXPECT(assert_equal(expected, *actualJoint));
// }
// // Check joint P(2,6)
// if (true) { // Check joint P(2,6)
// actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic); if (true) {
// SymbolicBayesNet expected; actualJoint = bayesTree.jointBayesNet(2, 6);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(6)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(2, 6)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(6));
// EXPECT(assert_equal(expected, *actualJoint)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2, 6));
// } EXPECT(assert_equal(expected, *actualJoint));
// }
// // Check joint P(4,6)
// if (false) { // TODO, not disjoint // Check joint P(4,6)
// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic); if (false) { // TODO, not disjoint
// SymbolicBayesNet expected; actualJoint = bayesTree.jointBayesNet(4, 6);
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(6)); SymbolicBayesNetUnordered expected;
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(4, 6)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(6));
// EXPECT(assert_equal(expected, *actualJoint)); expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4, 6));
// } EXPECT(assert_equal(expected, *actualJoint));
//} }
// }
///* ************************************************************************* *
// Bayes tree for smoother with "natural" ordering: /* ************************************************************************* */
// C1 5 6 TEST(SymbolicBayesTreeUnordered, forest_joint)
// C2 4 : 5 {
// C3 3 : 4 // Create forest
// C4 2 : 3 SymbolicBayesTreeCliqueUnordered::shared_ptr root1 = MakeClique(list_of(1), 1);
// C5 1 : 2 SymbolicBayesTreeCliqueUnordered::shared_ptr root2 = MakeClique(list_of(2), 2);
// C6 0 : 1 SymbolicBayesTreeUnordered bayesTree;
// **************************************************************************** */ bayesTree.insertRoot(root1);
// bayesTree.insertRoot(root2);
//TEST_UNSAFE( SymbolicBayesTreeUnordered, linear_smoother_shortcuts ) {
// // Create smoother with 7 nodes // Check joint
// SymbolicFactorGraph smoother; SymbolicBayesNetUnordered expected = list_of
// smoother.push_factor(0); (SymbolicConditionalUnordered(1))
// smoother.push_factor(0, 1); (SymbolicConditionalUnordered(2));
// smoother.push_factor(1, 2); SymbolicBayesNetUnordered actual = *bayesTree.jointBayesNet(1, 2);
// smoother.push_factor(2, 3);
// smoother.push_factor(3, 4); EXPECT(assert_equal(expected, actual));
// smoother.push_factor(4, 5); }
// smoother.push_factor(5, 6);
// /* ************************************************************************* *
// BayesNet<SymbolicConditionalUnordered> bayesNet = Bayes tree for smoother with "natural" ordering:
// *SymbolicSequentialSolver(smoother).eliminate(); C1 5 6
// C2 4 : 5
// if (debug) { C3 3 : 4
// GTSAM_PRINT(bayesNet); C4 2 : 3
// bayesNet.saveGraph("/tmp/symbolicBayesNet.dot"); C5 1 : 2
// } C6 0 : 1
// **************************************************************************** */
// // create a BayesTree out of a Bayes net
// SymbolicBayesTreeUnordered bayesTree(bayesNet); TEST_UNSAFE( SymbolicBayesTreeUnordered, linear_smoother_shortcuts ) {
// if (debug) { // Create smoother with 7 nodes
// GTSAM_PRINT(bayesTree); SymbolicFactorGraphUnordered smoother;
// bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); smoother.push_factor(0);
// } smoother.push_factor(0, 1);
// smoother.push_factor(1, 2);
// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.root(); smoother.push_factor(2, 3);
// smoother.push_factor(3, 4);
// { smoother.push_factor(4, 5);
// // check shortcut P(S2||R) to root smoother.push_factor(5, 6);
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); // Eliminate in numerical order 0..6
// SymbolicBayesNet expected; OrderingUnordered ordering(smoother.keys());
// EXPECT(assert_equal(expected, shortcut)); SymbolicBayesNetUnordered bayesNet = *smoother.eliminateSequential(ordering);
// }
// if (debug) {
// { GTSAM_PRINT(bayesNet);
// // check shortcut P(S3||R) to root bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 }
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic);
// SymbolicBayesNet expected; // create a BayesTree
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(4, 5)); SymbolicBayesTreeUnordered bayesTree = *smoother.eliminateMultifrontal(ordering);
// EXPECT(assert_equal(expected, shortcut)); if (debug) {
// } GTSAM_PRINT(bayesTree);
// bayesTree.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot");
// { }
// // check shortcut P(S4||R) to root
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 SymbolicBayesTreeUnordered::Clique::shared_ptr R = bayesTree.roots().front();
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic);
// SymbolicBayesNet expected; {
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(3, 5)); // check shortcut P(S2||R) to root
// EXPECT(assert_equal(expected, shortcut)); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2
// } SymbolicBayesNetUnordered shortcut = c->shortcut(R);
//} SymbolicBayesNetUnordered expected;
// EXPECT(assert_equal(expected, shortcut));
///* ************************************************************************* */ }
//// from testSymbolicJunctionTree, which failed at one point
//TEST(SymbolicBayesTreeUnordered, complicatedMarginal) { {
// // check shortcut P(S3||R) to root
// // Create the conditionals to go in the BayesTree SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3
// list<Index> L; SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// L = list_of(1)(2)(5); SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(4, 5));
// SymbolicConditionalUnordered::shared_ptr R_1_2(new SymbolicConditionalUnordered(L, 2)); EXPECT(assert_equal(expected, shortcut));
// 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)); // check shortcut P(S4||R) to root
// L = list_of(7)(8)(11); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4
// SymbolicConditionalUnordered::shared_ptr R_7_8(new SymbolicConditionalUnordered(L, 2)); SymbolicBayesNetUnordered shortcut = c->shortcut(R);
// L = list_of(9)(10)(11)(12); SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(3, 5));
// SymbolicConditionalUnordered::shared_ptr R_9_10(new SymbolicConditionalUnordered(L, 2)); EXPECT(assert_equal(expected, shortcut));
// L = list_of(11)(12); }
// SymbolicConditionalUnordered::shared_ptr R_11_12(new SymbolicConditionalUnordered(L, 2)); }
//
// // Symbolic Bayes Tree /* ************************************************************************* */
// typedef SymbolicBayesTreeUnordered::Clique Clique; // from testSymbolicJunctionTree, which failed at one point
// typedef SymbolicBayesTreeUnordered::sharedClique sharedClique; TEST(SymbolicBayesTreeUnordered, complicatedMarginal)
// {
// // Create Bayes Tree // Create the conditionals to go in the BayesTree
// SymbolicBayesTreeUnordered bt; SymbolicBayesTreeCliqueUnordered::shared_ptr cur;
// bt.insert(sharedClique(new Clique(R_11_12))); SymbolicBayesTreeCliqueUnordered::shared_ptr root = MakeClique(list_of(11)(12), 2);
// bt.insert(sharedClique(new Clique(R_9_10))); cur = root;
// bt.insert(sharedClique(new Clique(R_7_8)));
// bt.insert(sharedClique(new Clique(R_5_6))); cur->children += MakeClique(list_of(9)(10)(11)(12), 2);
// bt.insert(sharedClique(new Clique(R_3_4))); cur->children.front()->parent_ = cur;
// bt.insert(sharedClique(new Clique(R_1_2))); cur = root->children.front();
// if (debug) {
// GTSAM_PRINT(bt); cur->children += MakeClique(list_of(7)(8)(11), 2);
// bt.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot"); cur->children.front()->parent_ = cur;
// } cur = root->children.front();
//
// SymbolicBayesTreeUnordered::Clique::shared_ptr R = bt.root(); cur->children += MakeClique(list_of(5)(6)(7)(8), 2);
// SymbolicBayesNet empty; cur->children.front()->parent_ = cur;
// cur = root->children.front();
// // Shortcut on 9
// { cur->children += MakeClique(list_of(3)(4)(6), 2);
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[9]; cur->children.front()->parent_ = cur;
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); cur = root->children.front();
// EXPECT(assert_equal(empty, shortcut));
// } cur->children += MakeClique(list_of(1)(2)(5), 2);
// cur->children.front()->parent_ = cur;
// // Shortcut on 7 cur = root->children.front();
// {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[7]; // Create Bayes Tree
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesTreeUnordered bt;
// EXPECT(assert_equal(empty, shortcut)); bt.insertRoot(root);
// } if (debug) {
// GTSAM_PRINT(bt);
// // Shortcut on 5 bt.saveGraph("/tmp/SymbolicBayesTreeUnordered.dot");
// { }
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[5];
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); // Shortcut on 9
// SymbolicBayesNet expected; {
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(8, 11)); SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[9];
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(7, 8, 11)); SymbolicBayesNetUnordered shortcut = c->shortcut(root);
// EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(SymbolicBayesNetUnordered(), shortcut));
// } }
//
// // Shortcut on 3 // Shortcut on 7
// { {
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[3]; SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[7];
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); SymbolicBayesNetUnordered shortcut = c->shortcut(root);
// SymbolicBayesNet expected; EXPECT(assert_equal(SymbolicBayesNetUnordered(), shortcut));
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(6, 11)); }
// EXPECT(assert_equal(expected, shortcut));
// } // Shortcut on 5
// {
// // Shortcut on 1 SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[5];
// { SymbolicBayesNetUnordered shortcut = c->shortcut(root);
// SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[1]; SymbolicBayesNetUnordered expected = list_of
// SymbolicBayesNet shortcut = c->shortcut(R, EliminateSymbolic); (SymbolicConditionalUnordered(8, 11))
// SymbolicBayesNet expected; (SymbolicConditionalUnordered(7, 8, 11));
// expected.push_front(boost::make_shared<SymbolicConditionalUnordered>(5, 11)); EXPECT(assert_equal(expected, shortcut));
// EXPECT(assert_equal(expected, shortcut)); }
// }
// // Shortcut on 3
// // Marginal on 5 {
// { SymbolicBayesTreeUnordered::Clique::shared_ptr c = bt[3];
// IndexFactor::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic); SymbolicBayesNetUnordered shortcut = c->shortcut(root);
// EXPECT(assert_equal(IndexFactor(5), *actual, 1e-1)); SymbolicBayesNetUnordered expected = list_of(SymbolicConditionalUnordered(6, 11));
// } EXPECT(assert_equal(expected, shortcut));
// }
// // Shortcut on 6
// { // Shortcut on 1
// IndexFactor::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic); {
// EXPECT(assert_equal(IndexFactor(6), *actual, 1e-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() { int main() {
TestResult tr; TestResult tr;

9
gtsam/symbolic/tests/testSymbolicFactor.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicFactorUnordered.h> #include <gtsam/symbolic/SymbolicFactorUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h> #include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
@ -52,10 +53,10 @@ TEST(SymbolicFactor, eliminate) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactor, EliminateSymbolic) TEST(SymbolicFactor, EliminateSymbolic)
{ {
const vector<SymbolicFactorUnordered::shared_ptr> factors = list_of const SymbolicFactorGraphUnordered factors = list_of
(boost::make_shared<SymbolicFactorUnordered>(2,4,6)) (SymbolicFactorUnordered(2,4,6))
(boost::make_shared<SymbolicFactorUnordered>(1,2,5)) (SymbolicFactorUnordered(1,2,5))
(boost::make_shared<SymbolicFactorUnordered>(0,3)); (SymbolicFactorUnordered(0,3));
const SymbolicFactorUnordered expectedFactor(4,5,6); const SymbolicFactorUnordered expectedFactor(4,5,6);
const SymbolicConditionalUnordered expectedConditional = const SymbolicConditionalUnordered expectedConditional =

40
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -49,13 +49,13 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
const OrderingUnordered order = list_of(0)(1); const OrderingUnordered order = list_of(0)(1);
const SymbolicBayesNetUnordered expectedBayesNet = list_of const SymbolicBayesNetUnordered expectedBayesNet = list_of
(boost::make_shared<SymbolicConditionalUnordered>(0,1,2)) (SymbolicConditionalUnordered(0,1,2))
(boost::make_shared<SymbolicConditionalUnordered>(1,2,3,4)); (SymbolicConditionalUnordered(1,2,3,4));
const SymbolicFactorGraphUnordered expectedSfg = list_of const SymbolicFactorGraphUnordered expectedSfg = list_of
(boost::make_shared<SymbolicFactorUnordered>(2,3)) (SymbolicFactorUnordered(2,3))
(boost::make_shared<SymbolicFactorUnordered>(4,5)) (SymbolicFactorUnordered(4,5))
(boost::make_shared<SymbolicFactorUnordered>(2,3,4)); (SymbolicFactorUnordered(2,3,4));
SymbolicBayesNetUnordered::shared_ptr actualBayesNet; SymbolicBayesNetUnordered::shared_ptr actualBayesNet;
SymbolicFactorGraphUnordered::shared_ptr actualSfg; SymbolicFactorGraphUnordered::shared_ptr actualSfg;
@ -88,11 +88,11 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeCliqueUnordered>(root)); expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeCliqueUnordered>(root));
SymbolicFactorGraphUnordered expectedFactorGraph = list_of SymbolicFactorGraphUnordered expectedFactorGraph = list_of
(boost::make_shared<SymbolicFactorUnordered>(0,1)) (SymbolicFactorUnordered(0,1))
(boost::make_shared<SymbolicFactorUnordered>(0,2)) (SymbolicFactorUnordered(0,2))
(boost::make_shared<SymbolicFactorUnordered>(1,3)) (SymbolicFactorUnordered(1,3))
(boost::make_shared<SymbolicFactorUnordered>(2,3)) (SymbolicFactorUnordered(2,3))
(boost::make_shared<SymbolicFactorUnordered>(1)); (SymbolicFactorUnordered(1));
SymbolicBayesTreeUnordered::shared_ptr actualBayesTree; SymbolicBayesTreeUnordered::shared_ptr actualBayesTree;
SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph; SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph;
@ -103,6 +103,20 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); 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) { TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
SymbolicFactorGraphUnordered fg; SymbolicFactorGraphUnordered fg;
@ -219,9 +233,9 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
// create Bayes Net // create Bayes Net
SymbolicBayesNetUnordered bayesNet; SymbolicBayesNetUnordered bayesNet;
bayesNet.add(SymbolicConditionalUnordered(0, 1, 2)); bayesNet += SymbolicConditionalUnordered(0, 1, 2);
bayesNet.add(SymbolicConditionalUnordered(1, 2)); bayesNet += SymbolicConditionalUnordered(1, 2);
bayesNet.add(SymbolicConditionalUnordered(1)); bayesNet += SymbolicConditionalUnordered(1);
// create actual factor graph from a Bayes Net // create actual factor graph from a Bayes Net
SymbolicFactorGraphUnordered actual(bayesNet); SymbolicFactorGraphUnordered actual(bayesNet);