99 lines
3.2 KiB
C++
99 lines
3.2 KiB
C++
/*
|
|
* EliminationTree-inl.h
|
|
* Created on: Feb 4, 2010
|
|
* @Author: Kai Ni
|
|
* @Author: Frank Dellaert
|
|
* @brief: The elimination tree, template bodies
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <stdexcept>
|
|
#include <functional>
|
|
#include <boost/foreach.hpp>
|
|
#include "EliminationTree.h"
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
template<class FG>
|
|
void EliminationTree<FG>::add(const FG& fg, const Symbol& key,
|
|
const IndexTable<Symbol>& indexTable) {
|
|
|
|
// Make a node and put it in the nodes_ array:
|
|
sharedNode node(new Node(fg, key));
|
|
size_t j = indexTable(key);
|
|
nodes_[j] = node;
|
|
|
|
// if the separator is empty, this is the root
|
|
if (node->separator_.empty()) {
|
|
this->root_ = node;
|
|
}
|
|
else {
|
|
// find parent by iterating over all separator keys, and taking the lowest
|
|
// one in the ordering. That is the index of the parent clique.
|
|
size_t parentIndex = nrVariables_;
|
|
BOOST_FOREACH(const Symbol& j, node->separator_) {
|
|
size_t index = indexTable(j);
|
|
if (index<parentIndex) parentIndex = index;
|
|
}
|
|
// attach to parent
|
|
sharedNode& parent = nodes_[parentIndex];
|
|
if (!parent) throw
|
|
invalid_argument("EliminationTree::add: parent clique does not exist");
|
|
node->parent_ = parent;
|
|
parent->children_.push_back(node);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class FG>
|
|
EliminationTree<FG>::EliminationTree(const OrderedGraphs& graphs) :
|
|
nrVariables_(graphs.size()), nodes_(nrVariables_) {
|
|
|
|
// Get ordering by (map first graphs)
|
|
Ordering ordering;
|
|
transform(graphs.begin(), graphs.end(), back_inserter(ordering),
|
|
_Select1st<typename OrderedGraphs::value_type> ());
|
|
|
|
// Create a temporary map from key to ordering index
|
|
IndexTable<Symbol> indexTable(ordering);
|
|
|
|
// Go over the collection in reverse elimination order
|
|
// and add one node for every of the n variables.
|
|
BOOST_REVERSE_FOREACH(const NamedGraph& namedGraph, graphs)
|
|
add(namedGraph.second, namedGraph.first, indexTable);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class FG>
|
|
EliminationTree<FG>::EliminationTree(FG& fg, const Ordering& ordering) :
|
|
nrVariables_(ordering.size()), nodes_(nrVariables_) {
|
|
|
|
// Loop over all variables and get factors that are connected
|
|
OrderedGraphs graphs;
|
|
BOOST_FOREACH(const Symbol& key, ordering) {
|
|
// TODO: a collection of factors is a factor graph and this should be returned
|
|
// below rather than having to copy. GaussianFactorGraphSet should go...
|
|
vector<typename FG::sharedFactor> found = fg.findAndRemoveFactors(key);
|
|
FG fragment;
|
|
NamedGraph namedGraph(key,fragment);
|
|
BOOST_FOREACH(const typename FG::sharedFactor& factor, found)
|
|
namedGraph.second.push_back(factor);
|
|
graphs.push_back(namedGraph);
|
|
}
|
|
|
|
// Create a temporary map from key to ordering index
|
|
IndexTable<Symbol> indexTable(ordering);
|
|
|
|
// Go over the collection in reverse elimination order
|
|
// and add one node for every of the n variables.
|
|
BOOST_REVERSE_FOREACH(const NamedGraph& namedGraph, graphs)
|
|
add(namedGraph.second, namedGraph.first, indexTable);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
} //namespace gtsam
|