60 lines
1.7 KiB
C++
60 lines
1.7 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file GaussianJunctionTree.cpp
|
|
* @date Jul 12, 2010
|
|
* @author Kai Ni
|
|
* @author Frank Dellaert
|
|
* @brief: the Gaussian junction tree
|
|
*/
|
|
|
|
#include <gtsam/inference/ClusterTree.h>
|
|
#include <gtsam/inference/JunctionTree.h>
|
|
#include <gtsam/linear/GaussianJunctionTree.h>
|
|
#include <gtsam/linear/GaussianBayesTree.h>
|
|
|
|
#include <vector>
|
|
|
|
#include <boost/foreach.hpp>
|
|
|
|
namespace gtsam {
|
|
|
|
// explicit template instantiation
|
|
template class JunctionTree<GaussianFactorGraph>;
|
|
template class ClusterTree<GaussianFactorGraph>;
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
VectorValues GaussianJunctionTree::optimize(Eliminate function) const {
|
|
tic(GJT_eliminate);
|
|
// eliminate from leaves to the root
|
|
BTClique::shared_ptr rootClique(this->eliminate(function));
|
|
toc(GJT_eliminate);
|
|
|
|
// Allocate solution vector and copy RHS
|
|
tic(allocate_VectorValues);
|
|
vector<size_t> dims(rootClique->conditional()->back()+1, 0);
|
|
countDims(rootClique, dims);
|
|
VectorValues result(dims);
|
|
toc(allocate_VectorValues);
|
|
|
|
// back-substitution
|
|
tic(backsubstitute);
|
|
internal::optimizeInPlace<GaussianBayesTree>(rootClique, result);
|
|
toc(backsubstitute);
|
|
return result;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
} //namespace gtsam
|