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

42
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

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

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

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

44
gtsam/inference/BayesTreeUnordered.h
View File

@ -28,6 +28,9 @@
namespace gtsam {
// Forward declarations
template<class FACTOR> class FactorGraphUnordered;
/**
* Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
@ -59,6 +62,7 @@ namespace gtsam {
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
typedef typename CLIQUE::EliminationTraits EliminationTraits;
/** A convenience class for a list of shared cliques */
struct Cliques : public FastList<sharedClique> {
@ -135,7 +139,7 @@ namespace gtsam {
const Nodes& nodes() const { return nodes_; }
/** Access node by variable */
const Node::shared_ptr operator[](Key j) const { return nodes_.at(j); }
const sharedNode operator[](Key j) const { return nodes_.at(j); }
/** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; }
@ -155,27 +159,24 @@ namespace gtsam {
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
///** return marginal on any variable */
//sharedFactor marginalFactor(Key j, Eliminate function) const;
/** Return marginal on any variable. Note that this actually returns a conditional, for which a
* solution may be directly obtained by calling .solve() on the returned object.
* Alternatively, it may be directly used as its factor base class. For example, for Gaussian
* systems, this returns a GaussianConditional, which inherits from JacobianFactor and
* GaussianFactor. */
sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
///**
// * return marginal on any variable, as a Bayes Net
// * NOTE: this function calls marginal, and then eliminates it into a Bayes Net
// * This is more expensive than the above function
// */
//sharedBayesNet marginalBayesNet(Key j, Eliminate function) const;
/**
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedFactorGraph joint(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
///**
// * return joint on two variables
// * Limitation: can only calculate joint if cliques are disjoint or one of them is root
// */
//sharedFactorGraph joint(Index j1, Index j2, Eliminate function) const;
///**
// * return joint on two variables as a BayesNet
// * Limitation: can only calculate joint if cliques are disjoint or one of them is root
// */
//sharedBayesNet jointBayesNet(Index j1, Index j2, Eliminate function) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
sharedBayesNet jointBayesNet(Index j1, Index j2, const Eliminate& function = EliminationTraits::DefaultEliminate) const;
/**
* Read only with side effects
@ -226,6 +227,9 @@ namespace gtsam {
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** Add all cliques in this BayesTree to the specified factor graph */
void addFactorsToGraph(FactorGraphUnordered<FactorType>& graph) const;
protected:
/** 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/function.hpp>
#include <boost/variant.hpp>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
@ -182,6 +183,23 @@ namespace gtsam {
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** Compute the marginal of the requested variables and return the result as a Bayes net.
* @param variables Determines the variables whose marginal to compute, if provided as an
* Ordering they will be ordered in the returned BayesNet as specified, and if provided
* as a vector<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:
// 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/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/BayesTreeUnordered.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
@ -64,25 +65,6 @@ namespace gtsam {
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>
size_t FactorGraphUnordered<FACTOR>::nrFactors() const {

166
gtsam/inference/FactorGraphUnordered.h
View File

@ -24,15 +24,51 @@
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <type_traits>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/BayesTreeUnordered.h>
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.
* In this class, however, only factor nodes are kept around.
@ -44,6 +80,7 @@ namespace gtsam {
public:
typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef sharedFactor value_type;
typedef typename std::vector<sharedFactor>::iterator 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) */
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
@ -112,18 +149,13 @@ namespace gtsam {
/** Add a factor directly using a shared_ptr */
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)); }
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
* the copy). */
template<class DERIVEDFACTOR>
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()); }
/** Add a factor directly using a shared_ptr */
void push_back(const sharedFactor& factor) {
factors_.push_back(factor); }
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR>
@ -131,42 +163,92 @@ namespace gtsam {
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor); }
/** push back many factors as shared_ptr's in a container (factors are not copied) */
template<typename 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) */
template<typename ITERATOR>
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
for(ITERATOR f = firstFactor; f != lastFactor; ++f)
add(*f);
push_back(*f);
}
/** Add a factor by value, will be copy-constructed (pass a shared_ptr to avoid the copy). */
template<class DERIVEDFACTOR>
void push_back(const DERIVEDFACTOR& factor) {
add(factor); }
protected:
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
* classes in favor of a type-specialized version that calls this templated function. */
template<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);
/** push back many factors as non-pointer objects in a container (factors are copied) */
template<typename CONTAINER>
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
/**
* @brief Add a vector of derived factors
* @param factors to add
*/
//template<typename DERIVEDFACTOR>
//void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
// factors_.insert(end(), factors.begin(), factors.end());
//template<class FACTOR_OR_CONTAINER>
//boost::assign::list_inserter<CRefCallPushBack<This> >
// operator*=(const FACTOR_OR_CONTAINER& factorOrContainer) {
// return boost::assign::make_list_inserter(CRefCallAddCopy<This>(*this));
//}
/// @}
@ -246,6 +328,12 @@ namespace gtsam {
/** replace a factor by index */
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** Erase factor and rearrange other factors to take up the empty space */
void erase(const_iterator item) { factors_.erase(item); }
/** Erase factors and rearrange other factors to take up the empty space */
void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); }
/// @}
/// @name Advanced Interface
/// @{

4
gtsam/linear/GaussianBayesTreeUnordered.h
View File

@ -33,11 +33,11 @@ namespace gtsam {
/* ************************************************************************* */
/** A clique in a GaussianBayesTree */
class GTSAM_EXPORT GaussianBayesTreeCliqueUnordered :
public BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered>
public BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered>
{
public:
typedef GaussianBayesTreeCliqueUnordered This;
typedef BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered, GaussianBayesNetUnordered> Base;
typedef BayesTreeCliqueBaseUnordered<GaussianBayesTreeCliqueUnordered, GaussianFactorGraphUnordered> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
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
if (parents().size() != c.parents().size())
return false;

6
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

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

9
gtsam/linear/GaussianFactorGraphUnordered.h
View File

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

22
gtsam/symbolic/SymbolicBayesNetUnordered.cpp
View File

@ -16,9 +16,11 @@
* @author Richard Roberts
*/
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <fstream>
namespace gtsam {
@ -28,5 +30,23 @@ namespace gtsam {
return Base::equals(bn, tol);
}
/* ************************************************************************* */
void SymbolicBayesNetUnordered::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
SymbolicConditionalUnordered::Frontals frontals = conditional->frontals();
Key me = frontals.front();
SymbolicConditionalUnordered::Parents parents = conditional->parents();
BOOST_FOREACH(Key p, parents)
of << p << "->" << me << std::endl;
}
of << "}";
of.close();
}
}

6
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

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

4
gtsam/symbolic/SymbolicBayesTreeUnordered.h
View File

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

12
gtsam/symbolic/SymbolicConditionalUnordered.cpp
View File

@ -22,4 +22,16 @@ namespace gtsam {
using namespace std;
/* ************************************************************************* */
void SymbolicConditionalUnordered::print(const std::string& str, const KeyFormatter& keyFormatter) const
{
BaseConditional::print(str, keyFormatter);
}
/* ************************************************************************* */
bool SymbolicConditionalUnordered::equals(const This& c, double tol) const
{
return BaseFactor::equals(c) && BaseConditional::equals(c);
}
}

11
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

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

17
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

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

5
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -55,11 +55,6 @@ namespace gtsam {
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>
// SymbolicFactorGraph::eliminateFrontals(size_t nFrontals) const

9
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

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

4
gtsam/symbolic/SymbolicFactorUnordered.cpp
View File

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

2
gtsam/symbolic/SymbolicFactorUnordered.h
View File

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

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

@ -21,6 +21,10 @@
#pragma once
#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 <boost/assign/list_of.hpp>

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

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

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

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

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

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

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

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