12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pulled GaussianBayesTree optimize algorithm into a template function

release/4.3a0
Richard Roberts 2013-08-09 21:35:40 +00:00
parent 356351db75
commit 1a4fe360ee
2 changed files with 105 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/linear/GaussianBayesTree.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/BayesTree-inst.h> #include <gtsam/inference/BayesTree-inst.h>
#include <gtsam/inference/BayesTreeCliqueBase-inst.h> #include <gtsam/inference/BayesTreeCliqueBase-inst.h>
#include <gtsam/linear/linearAlgorithms-inst.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -33,63 +34,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal namespace internal
{ {
/* ************************************************************************* */
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
//VectorValues ancestorResults;
//VectorValues results;
};
/* ************************************************************************* */
/** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()()
* optimizes the clique given the solution for the parents, and returns the solution for the
* clique's frontal variables. In addition, it adds the solution to a global collected
* solution that will finally be returned to the user. The reason we pass the individual
* clique solutions between nodes is to avoid log(n) lookups over all variables, they instead
* then are only over a node's parent variables. */
struct OptimizeClique
{
VectorValues collectedResult;
OptimizeData operator()(
const GaussianBayesTreeClique::shared_ptr& clique,
OptimizeData& parentData)
{
OptimizeData myData;
myData.parentData = parentData;
// Take any ancestor results we'll need
//BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// Solve and store in our results
VectorValues result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/);
collectedResult.insert(result);
//myData.ancestorResults.insert(result);
return myData;
}
};
/* ************************************************************************* */
//OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData)
//{
// // Create data - holds a pointer to our parent, a copy of parent solution, and our results
// OptimizeData myData;
// myData.parentData = parentData;
// // Take any ancestor results we'll need
// BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // Solve and store in our results
// myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
// myData.ancestorResults.insert(myData.results);
// return myData;
//}
/* ************************************************************************* */
//void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData)
//{
// // Conglomerate our results to the parent
// myData.parentData->results.insert(myData.results);
//}
/* ************************************************************************* */ /* ************************************************************************* */
double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum) double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum)
{ {
@ -108,14 +52,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues GaussianBayesTree::optimize() const VectorValues GaussianBayesTree::optimize() const
{ {
gttic(GaussianBayesTree_optimize); return internal::linearAlgorithms::optimizeBayesTree(*this);
//internal::OptimizeData rootData; // Will hold final solution
//treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
//return rootData.results;
internal::OptimizeData rootData;
internal::OptimizeClique preVisitor;
treeTraversal::DepthFirstForest(*this, rootData, preVisitor);
return preVisitor.collectedResult;
} }
/* ************************************************************************* */ /* ************************************************************************* */

103
gtsam/linear/linearAlgorithms-inst.h Normal file
View File

@ -0,0 +1,103 @@
/* ----------------------------------------------------------------------------
* 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 linearAlgorithms-inst.h
* @brief Templated algorithms that are used in multiple places in linear
* @author Richard Roberts
*/
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
namespace gtsam
{
namespace internal
{
namespace linearAlgorithms
{
/* ************************************************************************* */
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
//VectorValues ancestorResults;
//VectorValues results;
};
/* ************************************************************************* */
/** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()()
* optimizes the clique given the solution for the parents, and returns the solution for the
* clique's frontal variables. In addition, it adds the solution to a global collected
* solution that will finally be returned to the user. The reason we pass the individual
* clique solutions between nodes is to avoid log(n) lookups over all variables, they instead
* then are only over a node's parent variables. */
template<class CLIQUE>
struct OptimizeClique
{
VectorValues collectedResult;
OptimizeData operator()(
const boost::shared_ptr<CLIQUE>& clique,
OptimizeData& parentData)
{
OptimizeData myData;
myData.parentData = parentData;
// Take any ancestor results we'll need
//BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// Solve and store in our results
VectorValues result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/);
collectedResult.insert(result);
//myData.ancestorResults.insert(result);
return myData;
}
};
/* ************************************************************************* */
//OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData)
//{
// // Create data - holds a pointer to our parent, a copy of parent solution, and our results
// OptimizeData myData;
// myData.parentData = parentData;
// // Take any ancestor results we'll need
// BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // Solve and store in our results
// myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
// myData.ancestorResults.insert(myData.results);
// return myData;
//}
/* ************************************************************************* */
//void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData)
//{
// // Conglomerate our results to the parent
// myData.parentData->results.insert(myData.results);
//}
/* ************************************************************************* */
template<class BAYESTREE>
VectorValues optimizeBayesTree(const BAYESTREE& bayesTree)
{
gttic(linear_optimizeBayesTree);
//internal::OptimizeData rootData; // Will hold final solution
//treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
//return rootData.results;
OptimizeData rootData;
OptimizeClique<typename BAYESTREE::Clique> preVisitor;
treeTraversal::DepthFirstForest(bayesTree, rootData, preVisitor);
return preVisitor.collectedResult;
}
}
}
}