From da10417cfc9bde76070a9c4cb8cab2dd41136b01 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 26 Jul 2013 18:00:37 +0000
Subject: [PATCH] Some speedups in back-substitution

---
 gtsam/linear/GaussianBayesTreeUnordered.cpp | 84 ++++++++++++---------
 1 file changed, 47 insertions(+), 37 deletions(-)

diff --git a/gtsam/linear/GaussianBayesTreeUnordered.cpp b/gtsam/linear/GaussianBayesTreeUnordered.cpp
index 641385c94..e56645a77 100644
--- a/gtsam/linear/GaussianBayesTreeUnordered.cpp
+++ b/gtsam/linear/GaussianBayesTreeUnordered.cpp
@@ -33,7 +33,7 @@ namespace gtsam {
     struct OptimizeData {
       boost::optional<OptimizeData&> parentData;
       VectorValuesUnordered ancestorResults;
-      VectorValuesUnordered results;
+      //VectorValuesUnordered results;
     };
 
     /* ************************************************************************* */
@@ -43,42 +43,48 @@ namespace gtsam {
      *  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
+    struct OptimizeClique
+    {
+      VectorValuesUnordered collectedResult;
+
+      OptimizeData operator()(
+        const GaussianBayesTreeCliqueUnordered::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
+        VectorValuesUnordered result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/);
+        collectedResult.insert(result);
+        //myData.ancestorResults.insert(result);
+        return myData;
+      }
+    };
+
+    /* ************************************************************************* */
+    //OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& parentData)
     //{
-    //  VectorValuesUnordered collectedResult;
-
-    //  VectorValuesUnordered operator()(
-    //    const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
-    //    const VectorValuesUnordered& parentSolution)
-    //  {
-    //    // parents are assumed to already be solved and available in result
-    //    VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution);
-    //    collectedResult.insert(cliqueSolution);
-    //    return cliqueSolution;
-    //  }
-    //};
+    //  // 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;
+    //}
 
     /* ************************************************************************* */
-    OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::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 GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData)
-    {
-      // Conglomerate our results to the parent
-      myData.parentData->results.insert(myData.results);
-    }
+    //void OptimizePostVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData)
+    //{
+    //  // Conglomerate our results to the parent
+    //  myData.parentData->results.insert(myData.results);
+    //}
 
     /* ************************************************************************* */
     double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
@@ -110,9 +116,13 @@ namespace gtsam {
   VectorValuesUnordered GaussianBayesTreeUnordered::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; // 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;
   }
 
   ///* ************************************************************************* */