118 lines
4.0 KiB
C++
118 lines
4.0 KiB
C++
/*
|
|
* JunctionTree-inl.h
|
|
*
|
|
* Created on: Feb 4, 2010
|
|
* Author: nikai
|
|
* Description: the junction tree
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/optional.hpp>
|
|
#include "Pose2.h"
|
|
#include "BayesTree-inl.h"
|
|
#include "SymbolicFactorGraph.h"
|
|
|
|
#include "JunctionTree.h"
|
|
|
|
#define DEBUG(i) \
|
|
if (verboseLevel>i) cout
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* Linear JunctionTree
|
|
*/
|
|
template <class Conditional, class FG>
|
|
void JunctionTree<Conditional, FG>::SubFG::printTree(const string& indent) const {
|
|
print(indent);
|
|
BOOST_FOREACH(const shared_ptr& child, children_)
|
|
child->printTree(indent+" ");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Conditional, class FG>
|
|
pair<FG, typename BayesTree<Conditional>::sharedClique>
|
|
JunctionTree<Conditional, FG>::eliminateOneClique(sharedSubFG current, BayesTree<Conditional>& bayesTree) {
|
|
FG fg; // factor graph will be assembled from local factors and marginalized children
|
|
list<sharedClique> children;
|
|
fg.push_back(*current); // add the local factor graph
|
|
BOOST_FOREACH(sharedSubFG& child, current->children_) {
|
|
// receive the factors from the child and its clique point
|
|
FG fgChild; sharedClique cliqueChild;
|
|
boost::tie(fgChild, cliqueChild) = eliminateOneClique(child, bayesTree);
|
|
if (!cliqueChild.get()) throw runtime_error("eliminateOneClique: child clique is invalid!");
|
|
|
|
fg.push_back(fgChild);
|
|
children.push_back(cliqueChild);
|
|
}
|
|
|
|
// eliminate the combined factors
|
|
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
|
// BayesNet<Conditional> bn = fg.eliminate(current->frontal_);
|
|
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
|
|
|
|
// create a new clique corresponding the combined factors
|
|
sharedClique new_clique = bayesTree.insert(bn, children);
|
|
|
|
return make_pair(fg, new_clique);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Conditional, class FG>
|
|
BayesTree<Conditional> JunctionTree<Conditional, FG>::eliminate() {
|
|
BayesTree<Conditional> bayesTree;
|
|
eliminateOneClique(rootFG_, bayesTree);
|
|
return bayesTree;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Conditional, class FG>
|
|
void JunctionTree<Conditional, FG>::iterSubGraphsDFS(VisitorSubFG visitor, sharedSubFG current) {
|
|
if (!current.get()) current = rootFG_;
|
|
// iterateBFS<SubFG>(visitor, current);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Conditional, class FG>
|
|
void JunctionTree<Conditional, FG>::iterSubGraphsBFS(VisitorSubFG visitor) {
|
|
// iterateDFS<SubFG>(visitor, rootFG_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* Linear JunctionTree
|
|
*/
|
|
template <class Conditional, class FG>
|
|
void LinearJunctionTree<Conditional, FG>::btreeBackSubstitue(typename BayesTree<Conditional>::sharedClique current, VectorConfig& config) {
|
|
// solve the bayes net in the current node
|
|
typename BayesNet<Conditional>::const_reverse_iterator it = current->rbegin();
|
|
for (; it!=current->rend(); it++) {
|
|
Vector x = (*it)->solve(config); // Solve for that variable
|
|
config.insert((*it)->key(),x); // store result in partial solution
|
|
}
|
|
|
|
// solve the bayes nets in the child nodes
|
|
BOOST_FOREACH(sharedClique child, current->children_) {
|
|
btreeBackSubstitue(child, config);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template <class Conditional, class FG>
|
|
VectorConfig LinearJunctionTree<Conditional, FG>::optimize() {
|
|
// eliminate from leaves to the root
|
|
BayesTree<Conditional> bayesTree = JunctionTree<Conditional, FG>::eliminate();
|
|
|
|
VectorConfig result;
|
|
btreeBackSubstitue(bayesTree.root(), result);
|
|
return result;
|
|
}
|
|
|
|
} //namespace gtsam
|