From 2dbed92ba21ae26aea4a0b9885e7074c2ac70149 Mon Sep 17 00:00:00 2001
From: Nils Schulte <git@nilsschulte.de>
Date: Thu, 30 May 2024 21:33:10 +0200
Subject: [PATCH] orthtree wip

---
 CMakeLists.txt   |   4 +-
 package.xml      |   3 +-
 src/orthtree.hpp | 144 +++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 149 insertions(+), 2 deletions(-)
 create mode 100644 src/orthtree.hpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 90151bc..8d8fbe4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,10 +26,12 @@ find_package(Ceres REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 find_package(Eigen3 REQUIRED NO_MODULE)
+find_package(tbb REQUIRED)
+find_package(sophus REQUIRED)
 
 add_executable(simpleslam src/simpleslam_node.cpp )
 ament_target_dependencies(simpleslam rclcpp sensor_msgs nav_msgs)
-target_link_libraries(simpleslam Ceres::ceres Eigen3::Eigen)
+target_link_libraries(simpleslam Ceres::ceres Eigen3::Eigen TBB::tbb Sophus::Sophus)
 
 
 # include_directories(include lib/karto_sdk/include
diff --git a/package.xml b/package.xml
index 8734496..104a4ce 100644
--- a/package.xml
+++ b/package.xml
@@ -22,7 +22,8 @@
 
   <build_depend>liblapack-dev</build_depend>
   <build_depend>libceres-dev</build_depend>
-  <build_depend>tbb</build_depend>b
+  <build_depend>tbb</build_depend>
+  <build_depend>sophus</build_depend>
   
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/src/orthtree.hpp b/src/orthtree.hpp
new file mode 100644
index 0000000..642b6c4
--- /dev/null
+++ b/src/orthtree.hpp
@@ -0,0 +1,144 @@
+#include <Eigen/Dense>
+#include <algorithm>
+#include <array>
+#include <bitset>
+#include <execution>
+#include <functional>
+#include <map>
+#include <memory>
+#include <mutex>
+
+template <unsigned int dim, typename number, typename T> class Orthtree {
+  typedef Eigen::Vector<number, dim> Point;
+  typedef std::function<bool(std::unique_ptr<T> &)> data_access_f;
+
+  class BBox {
+  public:
+    enum Relation {
+      NO_OVERLAP,
+      OVERLAP,
+      CONTAINS,
+      CONTAINED,
+    };
+    Point min, max;
+    Point center() { return (min + max) / 2; }
+    Relation relation(const BBox &b) {
+      if (b.min > max || b.max < min)
+        return NO_OVERLAP;
+      if (b.min > min && b.max < max)
+        return CONTAINS;
+      if (b.min < min && b.max > max)
+        return CONTAINED;
+      return OVERLAP;
+    };
+    BBox(const Point &min, const Point &max) : min(min), max(max) {}
+  };
+  class Node {
+  public:
+    typedef std::bitset<dim> coord;
+    Point center;
+    std::unique_ptr<std::array<Node, (2 << dim)>> children;
+    std::unique_ptr<T> data;
+
+    mutable std::mutex mtx;
+
+    void create_children(const Point &parent_center, number parent_width) {
+      children = std::make_unique<std::array<Node, (2 << dim)>>();
+      for (unsigned int i = 0; i < (2 << dim); ++i) {
+        Point offset = Point::Ones() * (parent_width / 4);
+        for (unsigned int j = 0; j < dim; ++j)
+          if (!coord(i)[j])
+            offset(j) *= -1;
+        (*children)[i].center = parent_center + offset;
+      }
+    };
+
+    unsigned int get_child_coord(const Point &p) const {
+      coord child_coord;
+      for (unsigned int i = 0; i < dim; ++i)
+        child_coord[i] = center[i] < p[i];
+      return child_coord.to_ulong();
+    };
+
+    void depth(const Point &point, int depth, data_access_f f, number width) {
+      if (depth == 0) {
+        std::lock_guard<std::mutex> guard(mtx);
+        f(data);
+      } else {
+        {
+          std::lock_guard<std::mutex> guard(mtx);
+          if (!children)
+            create_children(center, width);
+        }
+        (*children)[get_child_coord(point)].depth(point, depth - 1, f,
+                                                  width / 2);
+      }
+    };
+    BBox bb(number width) {
+      Point offset = Point::Ones() * (width / 2);
+      return BBox(center - offset, center + offset);
+    };
+    void all_data(data_access_f f) const {
+      std::lock_guard<std::mutex> guard(mtx);
+      if (data) {
+        f(data);
+      }
+      if (children) {
+        std::for_each(std::execution::par_unseq, std::begin(*children),
+                      std::end(*children),
+                      [f](const auto &c) { c.all_data(f); });
+      }
+    }
+    Node &is_leaf() {
+      std::lock_guard<std::mutex> guard(mtx);
+      return bool(children);
+    }
+  };
+  std::unique_ptr<Node> root;
+  number root_width;
+
+  mutable std::mutex mtx;
+  Orthtree(const Point &root_center, number root_width)
+      : root_width(root_width), root(new Node()) {
+    root->center = root_center;
+  };
+
+  void depth(const Point &point, int depth, data_access_f f) {
+    root->depth(point, depth, f, root_width);
+  }
+
+  void data_in_bb(const BBox &bb, data_access_f f) const {
+    number curr_width = root_width;
+    std::vector<const Node *const> to_visit{root.get()};
+    std::vector<const Node *const> to_visit_children;
+    bool stop = false;
+    while (!to_visit.empty() && !stop) {
+      auto bbrel = to_visit.back()->bb(curr_width).relation(bb);
+      switch (bbrel) {
+      case (Orthtree::BBox::CONTAINED):
+        to_visit.back()->all_data(f);
+        break;
+      case (Orthtree::BBox::OVERLAP):
+      case Orthtree::BBox::CONTAINS:
+        to_visit.back()->depth(to_visit->back()->center, 0, [&](auto &p) {
+          if (f(p))
+            stop = true;
+          return false;
+        });
+        if (!to_visit.back()->is_leaf())
+          std::transform(to_visit.back()->children->cbegin(),
+                         to_visit.back()->children->cend(),
+                         to_visit_children.begin(), std::addressof);
+        break;
+      default:
+        break;
+      }
+      to_visit.pop_back();
+      if (to_visit.empty()) {
+        to_visit.swap(to_visit_children);
+        to_visit.clear();
+        curr_width /= 2;
+      }
+    }
+  }
+}