From 1d06f1e7667d3f9c9761161cbbf9e47aa3955559 Mon Sep 17 00:00:00 2001
From: "Kuangyuan.Sun" <kuangyuan.sun@tusimple.ai>
Date: Wed, 25 Jan 2023 22:44:43 -0800
Subject: [PATCH] try fixing stack overflow issue for large tree

---
 gtsam/inference/BayesTreeCliqueBase.h | 25 ++++++++++++++++++++++++-
 gtsam/inference/ClusterTree.h         | 26 +++++++++++++++++++++++++-
 gtsam/inference/EliminationTree.h     | 24 ++++++++++++++++++++++++
 3 files changed, 73 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index 976d1a7f2..f72412590 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -25,6 +25,8 @@
 #include <string>
 #include <mutex>
 #include <optional>
+#include <queue>
+#include <deque>
 
 namespace gtsam {
 
@@ -97,7 +99,28 @@ namespace gtsam {
     }
 
     // Virtual destructor.
-    virtual ~BayesTreeCliqueBase() {}
+    virtual ~BayesTreeCliqueBase() {
+      // default destructor will cause stack overflow for deletion of large trees
+      // so we delete the tree explicitly by first BFSing the tree to determine the topological order
+      // and then clearing the children of each node in topological order
+      // Only work in single threaded mode
+      std::queue<This*> bfs_queue;
+      std::deque<This*> topological_order;
+      bfs_queue.push(this);
+      while (!bfs_queue.empty()) {
+        auto node = bfs_queue.front();
+        bfs_queue.pop();
+        topological_order.push_front(node);
+        for (auto& child : node->children) {
+          if (child.use_count() == 1) {
+            bfs_queue.push(child.get());
+          }
+        }
+      }
+      for (auto node : topological_order) {
+        node->children.clear();
+      }
+    }
 
     /// @}
 
diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h
index 98a86b3f8..a517951a0 100644
--- a/gtsam/inference/ClusterTree.h
+++ b/gtsam/inference/ClusterTree.h
@@ -9,6 +9,8 @@
 
 #pragma once
 
+#include <deque>
+#include <queue>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Ordering.h>
@@ -46,7 +48,8 @@ class ClusterTree {
 
     Cluster() : problemSize_(0) {}
 
-    virtual ~Cluster() {}
+    virtual ~Cluster() {
+    }
 
     const Cluster& operator[](size_t i) const {
       return *(children[i]);
@@ -164,6 +167,27 @@ class ClusterTree {
     return *(roots_[i]);
   }
 
+  virtual ~ClusterTree() {
+    // use default destructor which recursively deletes all nodes with shared_ptr causes stack overflow.
+    // so for each tree, we do a BFS to get sequence of nodes to delete, and clear their children first.
+    for (auto&& root : roots_) {
+      std::queue<Cluster*> q;
+      std::deque<Cluster*> nodes;
+      q.push(root.get());
+      while (!q.empty()) {
+        auto node = q.front();
+        nodes.push_front(node);
+        q.pop();
+        for (auto&& child : node->children) {
+          q.push(child.get());
+        }
+      }
+      for (auto&& node : nodes) {
+        node->children.clear();
+      }
+    }
+  }
+
   /// @}
 
  protected:
diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h
index 2a3abdcf6..dcd561fd2 100644
--- a/gtsam/inference/EliminationTree.h
+++ b/gtsam/inference/EliminationTree.h
@@ -19,6 +19,8 @@
 
 #include <utility>
 #include <memory>
+#include <deque>
+#include <queue>
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/FastVector.h>
@@ -118,6 +120,27 @@ namespace gtsam {
     /// @}
 
   public:
+    ~EliminationTree() {
+      // default destructor will cause stack overflow for deletion of large trees
+      // so we delete the tree explicitly by first BFSing the tree to determine the topological order
+      // and then clearing the children of each node in topological order
+      for (auto&& root : roots_) {
+        std::queue<Node*> bfs_queue;
+        std::deque<Node*> topological_order;
+        bfs_queue.push(root.get());
+        while (!bfs_queue.empty()) {
+          Node* node = bfs_queue.front();
+          bfs_queue.pop();
+          topological_order.push_front(node);
+          for (auto&& child : node->children) {
+            bfs_queue.push(child.get());
+          }
+        }
+        for (auto&& node : topological_order) {
+          node->children.clear();
+        }
+      }
+    }
     /// @name Standard Interface
     /// @{
 
@@ -156,6 +179,7 @@ namespace gtsam {
     /** Swap the data of this tree with another one, this operation is very fast. */
     void swap(This& other);
 
+
   protected:
     /// Protected default constructor
     EliminationTree() {}