87 lines
2.9 KiB
C++
87 lines
2.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* GaussianJunctionTree.cpp
|
|
* Created on: Jul 12, 2010
|
|
* @author Kai Ni
|
|
* @author Frank Dellaert
|
|
* @brief: the Gaussian junction tree
|
|
*/
|
|
|
|
#include <gtsam/inference/ClusterTree-inl.h>
|
|
#include <gtsam/inference/JunctionTree-inl.h>
|
|
#include <gtsam/linear/GaussianJunctionTree.h>
|
|
|
|
#include <vector>
|
|
|
|
#include <boost/foreach.hpp>
|
|
|
|
namespace gtsam {
|
|
|
|
// explicit template instantiation
|
|
template class JunctionTree<GaussianFactorGraph>;
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* GaussianJunctionTree
|
|
*/
|
|
void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr<const BayesTree::Clique>& current, VectorValues& config) const {
|
|
// solve the bayes net in the current node
|
|
GaussianBayesNet::const_reverse_iterator it = current->rbegin();
|
|
for (; it!=current->rend(); ++it) {
|
|
Vector x = (*it)->solve(config); // Solve for that variable
|
|
config[(*it)->key()] = x; // store result in partial solution
|
|
}
|
|
|
|
// solve the bayes nets in the child nodes
|
|
BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) {
|
|
btreeBackSubstitute(child, config);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void countDims(const boost::shared_ptr<const BayesTree<GaussianConditional>::Clique>& clique, vector<size_t>& dims) {
|
|
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional>& cond, *clique) {
|
|
// There should be no two conditionals on the same variable
|
|
assert(dims[cond->key()] == 0);
|
|
dims[cond->key()] = cond->dim();
|
|
}
|
|
BOOST_FOREACH(const boost::shared_ptr<const BayesTree<GaussianConditional>::Clique>& child, clique->children()) {
|
|
countDims(child, dims);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
VectorValues GaussianJunctionTree::optimize() const {
|
|
tic("GJT optimize 1: eliminate");
|
|
// eliminate from leaves to the root
|
|
boost::shared_ptr<const BayesTree::Clique> rootClique(this->eliminate());
|
|
toc("GJT optimize 1: eliminate");
|
|
|
|
// Allocate solution vector
|
|
tic("GJT optimize 2: allocate VectorValues");
|
|
vector<size_t> dims(rootClique->back()->key() + 1, 0);
|
|
countDims(rootClique, dims);
|
|
VectorValues result(dims);
|
|
toc("GJT optimize 2: allocate VectorValues");
|
|
|
|
// back-substitution
|
|
tic("GJT optimize 3: back-substitute");
|
|
btreeBackSubstitute(rootClique, result);
|
|
toc("GJT optimize 3: back-substitute");
|
|
return result;
|
|
}
|
|
|
|
} //namespace gtsam
|