diff --git a/gtsam/linear/GaussianBayesTreeUnordered.cpp b/gtsam/linear/GaussianBayesTreeUnordered.cpp
index 7e21fe3a5..42cd08b22 100644
--- a/gtsam/linear/GaussianBayesTreeUnordered.cpp
+++ b/gtsam/linear/GaussianBayesTreeUnordered.cpp
@@ -29,6 +29,13 @@ namespace gtsam {
   /* ************************************************************************* */
   namespace internal
   {
+    /* ************************************************************************* */
+    struct OptimizeData {
+      boost::optional<OptimizeData&> parentData;
+      VectorValuesUnordered ancestorResults;
+      VectorValuesUnordered 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
@@ -36,34 +43,75 @@ 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
-    {
-      VectorValuesUnordered collectedResult;
+    //struct OptimizeClique
+    //{
+    //  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;
-      }
-    };
+    //  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;
+    //  }
+    //};
+
+    /* ************************************************************************* */
+    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);
+    }
 
     /* ************************************************************************* */
     double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
     {
       parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
+      assert(false);
+      return 0;
     }
   }
 
   /* ************************************************************************* */
-  VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const {
-    internal::OptimizeClique visitor;
-    VectorValuesUnordered empty;
-    treeTraversal::DepthFirstForest(*this, empty, visitor);
-    return visitor.collectedResult;
+  GaussianBayesTreeUnordered::GaussianBayesTreeUnordered(const GaussianBayesTreeUnordered& other) :
+    Base(other) {}
+
+  /* ************************************************************************* */
+  GaussianBayesTreeUnordered& GaussianBayesTreeUnordered::operator=(const GaussianBayesTreeUnordered& other)
+  {
+    (void) Base::operator=(other);
+    return *this;
+  }
+
+  /* ************************************************************************* */
+  bool GaussianBayesTreeUnordered::equals(const This& other, double tol) const
+  {
+    return Base::equals(other, tol);
+  }
+
+  /* ************************************************************************* */
+  VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const
+  {
+    internal::OptimizeData rootData; // Will hold final solution
+    treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
+    return rootData.results;
   }
 
   ///* ************************************************************************* */