From 1a4fe360eedf78dfaa8c140c14ea6a9f64bfc928 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:40 +0000 Subject: [PATCH] Pulled GaussianBayesTree optimize algorithm into a template function --- gtsam/linear/GaussianBayesTree.cpp | 67 +---------------- gtsam/linear/linearAlgorithms-inst.h | 103 +++++++++++++++++++++++++++ 2 files changed, 105 insertions(+), 65 deletions(-) create mode 100644 gtsam/linear/linearAlgorithms-inst.h diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index bb25c9a45..0ceecdfd2 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -33,63 +34,6 @@ namespace gtsam { /* ************************************************************************* */ namespace internal { - /* ************************************************************************* */ - struct OptimizeData { - boost::optional 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) { @@ -108,14 +52,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianBayesTree::optimize() const { - gttic(GaussianBayesTree_optimize); - //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; + return internal::linearAlgorithms::optimizeBayesTree(*this); } /* ************************************************************************* */ diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h new file mode 100644 index 000000000..93276be59 --- /dev/null +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -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 +#include + +#include +#include + +namespace gtsam +{ + namespace internal + { + namespace linearAlgorithms + { + /* ************************************************************************* */ + struct OptimizeData { + boost::optional 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 + struct OptimizeClique + { + VectorValues collectedResult; + + OptimizeData operator()( + const boost::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); + //} + + /* ************************************************************************* */ + template + 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 preVisitor; + treeTraversal::DepthFirstForest(bayesTree, rootData, preVisitor); + return preVisitor.collectedResult; + } + } + } +} \ No newline at end of file