106 lines
3.7 KiB
C++
106 lines
3.7 KiB
C++
/*
|
|
* JunctionTree-inl.h
|
|
* Created on: Feb 4, 2010
|
|
* @Author: Kai Ni
|
|
* @Author: Frank Dellaert
|
|
* @brief: The junction tree, template bodies
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/foreach.hpp>
|
|
|
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
|
#include <gtsam/inference/BayesTree-inl.h>
|
|
#include <gtsam/inference/JunctionTree.h>
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
template <class FG>
|
|
JunctionTree<FG>::JunctionTree(FG& fg, const Ordering& ordering) {
|
|
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
|
|
// -> SymbolicBayesNet -> SymbolicBayesTree
|
|
SymbolicFactorGraph sfg(fg);
|
|
SymbolicBayesNet sbn = sfg.eliminate(ordering);
|
|
BayesTree<SymbolicConditional> sbt(sbn);
|
|
|
|
// distribtue factors
|
|
this->root_ = distributeFactors(fg, sbt.root());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class FG>
|
|
typename JunctionTree<FG>::sharedClique JunctionTree<FG>::distributeFactors(
|
|
FG& fg, const BayesTree<SymbolicConditional>::sharedClique bayesClique) {
|
|
// create a new clique in the junction tree
|
|
sharedClique clique(new Clique());
|
|
clique->frontal_ = bayesClique->ordering();
|
|
clique->separator_.insert(bayesClique->separator_.begin(),
|
|
bayesClique->separator_.end());
|
|
|
|
// recursively call the children
|
|
BOOST_FOREACH(const BayesTree<SymbolicConditional>::sharedClique bayesChild, bayesClique->children()) {
|
|
sharedClique child = distributeFactors(fg, bayesChild);
|
|
clique->children_.push_back(child);
|
|
child->parent_ = clique;
|
|
}
|
|
|
|
// collect the factors
|
|
typedef vector<typename FG::sharedFactor> Factors;
|
|
BOOST_FOREACH(const Symbol& frontal, clique->frontal_) {
|
|
Factors factors = fg.template findAndRemoveFactors(frontal);
|
|
BOOST_FOREACH(const typename FG::sharedFactor& factor_, factors)
|
|
clique->push_back(factor_);
|
|
}
|
|
|
|
return clique;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class FG> template <class Conditional>
|
|
pair<FG, typename BayesTree<Conditional>::sharedClique>
|
|
JunctionTree<FG>::eliminateOneClique(sharedClique current) {
|
|
|
|
typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
|
|
FG fg; // factor graph will be assembled from local factors and marginalized children
|
|
list<sharedBtreeClique> children;
|
|
fg.push_back(*current); // add the local factor graph
|
|
|
|
BOOST_FOREACH(sharedClique& child, current->children_) {
|
|
// receive the factors from the child and its clique point
|
|
FG fgChild; sharedBtreeClique childRoot;
|
|
boost::tie(fgChild, childRoot) = eliminateOneClique<Conditional>(child);
|
|
|
|
fg.push_back(fgChild);
|
|
children.push_back(childRoot);
|
|
}
|
|
|
|
// eliminate the combined factors
|
|
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
|
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
|
|
|
|
// create a new clique corresponding the combined factors
|
|
// BayesTree<Conditional> bayesTree(bn, children);
|
|
sharedBtreeClique new_clique(new typename BayesTree<Conditional>::Clique(bn));
|
|
new_clique->children_ = children;
|
|
|
|
BOOST_FOREACH(sharedBtreeClique& childRoot, children)
|
|
childRoot->parent_ = new_clique;
|
|
|
|
return make_pair(fg, new_clique);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class FG> template <class Conditional>
|
|
typename BayesTree<Conditional>::sharedClique JunctionTree<FG>::eliminate() {
|
|
pair<FG, typename BayesTree<Conditional>::sharedClique> ret = this->eliminateOneClique<Conditional>(this->root());
|
|
if (ret.first.nrFactors() != 0)
|
|
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
|
|
return ret.second;
|
|
}
|
|
|
|
} //namespace gtsam
|