diff --git a/CMakeLists.txt b/CMakeLists.txt
index 90151bc..887faf5 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
@@ -43,15 +45,13 @@ install(TARGETS
 
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(simpleslam_test test/orthtree_test.cpp)
+  target_include_directories(simpleslam_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  target_link_libraries(simpleslam_test Ceres::ceres Eigen3::Eigen TBB::tbb Sophus::Sophus)
 endif()
 
 ament_package()
diff --git a/package.xml b/package.xml
index 8734496..58a6c85 100644
--- a/package.xml
+++ b/package.xml
@@ -22,11 +22,13 @@
 
   <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>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/orthtree.hpp b/src/orthtree.hpp
new file mode 100644
index 0000000..c90af18
--- /dev/null
+++ b/src/orthtree.hpp
@@ -0,0 +1,171 @@
+#include <Eigen/Dense>
+#include <algorithm>
+#include <array>
+#include <bitset>
+#include <execution>
+#include <functional>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <mutex>
+
+template <unsigned int dim, typename number, typename T> class Orthtree {
+public:
+  typedef Eigen::Vector<number, dim> Point;
+  typedef std::function<bool(std::unique_ptr<T> &, const Point &)>
+      data_access_f;
+  typedef std::function<bool(const std::unique_ptr<T> &, const Point &)>
+      cdata_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.array() > max.array()).any() ||
+          (b.max.array() < min.array()).any())
+        return NO_OVERLAP;
+      if ((b.min.array() > min.array()).all() &&
+          (b.max.array() < max.array()).all())
+        return CONTAINS;
+      if ((b.min.array() < min.array()).all() &&
+          (b.max.array() > max.array()).all())
+        return CONTAINED;
+      return OVERLAP;
+    };
+    BBox(const Point &min, const Point &max) : min(min), max(max) {
+      assert((min.array() < max.array()).all());
+    }
+  };
+  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, center);
+      } 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) const {
+      Point offset = Point::Ones() * (width / 2);
+      return BBox(center - offset, center + offset);
+    };
+    void access_data(cdata_access_f f) const {
+      std::lock_guard<std::mutex> guard(mtx);
+      if (data) {
+        f(data, center);
+      }
+    }
+    void all_data(cdata_access_f f) const {
+      std::lock_guard<std::mutex> guard(mtx);
+      if (data) {
+        f(data, center);
+      }
+      if (children) {
+        std::for_each(std::execution::par_unseq, std::begin(*children),
+                      std::end(*children),
+                      [f](const auto &c) { c.all_data(f); });
+      }
+    }
+    bool is_leaf() const {
+      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(new Node()), root_width(root_width) {
+    root->center = root_center;
+  };
+
+  void all_data(cdata_access_f f) const { root->all_data(f); };
+
+  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, cdata_access_f f) const {
+    number curr_width = root_width;
+    std::vector<const Node *> to_visit{root.get()};
+    std::vector<const Node *> to_visit_children;
+    bool stop = false;
+    while (!to_visit.empty() && !stop) {
+      auto cbb = to_visit.back()->bb(curr_width);
+      auto bbrel = cbb.relation(bb);
+      // std::cout << to_visit.back()->center.transpose() << " - "
+                // << cbb.min.transpose() << " - " << bbrel << std::endl;
+      switch (bbrel) {
+      case (Orthtree::BBox::CONTAINED):
+        to_visit.back()->all_data(f);
+        break;
+      case (Orthtree::BBox::OVERLAP):
+      case Orthtree::BBox::CONTAINS:
+        to_visit.back()->access_data([&](auto &p, auto c) {
+          if (!f(p, c))
+            stop = true;
+          return false;
+        });
+        if (!to_visit.back()->is_leaf()) {
+          // std::transform(std::execution::par_unseq,to_visit.back()->children->cbegin(),
+          //                to_visit.back()->children->cend(),
+          //                to_visit_children.begin(), std::addressof<const
+          //                Node>);
+          for (const auto &n : *to_visit.back()->children) {
+            to_visit_children.push_back(&n);
+          }
+          // std::cout << "leaf "<<to_visit_children.size() << std::endl;
+        }
+        break;
+      default:
+        break;
+      }
+      to_visit.pop_back();
+      if (to_visit.empty()) {
+        to_visit.swap(to_visit_children);
+        to_visit_children.clear();
+        curr_width /= 2;
+      }
+    }
+  }
+};
diff --git a/src/simpleslam_node.cpp b/src/simpleslam_node.cpp
index 418b159..496cd0f 100644
--- a/src/simpleslam_node.cpp
+++ b/src/simpleslam_node.cpp
@@ -13,6 +13,7 @@
 
 #include "simpleslam.hpp"
 #include "utils.hpp"
+#include "orthtree.hpp"
 
 class MinimalSubscriber : public rclcpp::Node {
 public:
diff --git a/test/orthtree_test.cpp b/test/orthtree_test.cpp
new file mode 100644
index 0000000..cc0c6ec
--- /dev/null
+++ b/test/orthtree_test.cpp
@@ -0,0 +1,89 @@
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "../src/orthtree.hpp"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+TEST(orthtree, insert1) {
+  typedef Orthtree<2, float, bool> O;
+  Orthtree<2, float, bool> t(O::Point::Zero(), 16);
+  auto p = O::Point(1, -7);
+  t.depth(p, 3, [&](auto &tp, auto c) {
+    tp = std::make_unique<bool>(true);
+    return true;
+  });
+
+  t.all_data([&](auto &tp, auto c) -> bool {
+    EXPECT_TRUE(tp && *tp) << c.transpose();
+    EXPECT_TRUE(c == p) << c.transpose();
+    return true;
+  });
+}
+TEST(orthtree, bbox1) {
+  typedef Orthtree<2, float, bool> O;
+  EXPECT_EQ(O::BBox(O::Point(0, 0), O::Point(10, 10))
+                .relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
+            O::BBox::OVERLAP);
+
+  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(10, 10))
+                .relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
+            O::BBox::OVERLAP);
+
+  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
+                .relation(O::BBox(O::Point(2, 2), O::Point(10, 11))),
+            O::BBox::NO_OVERLAP);
+  
+  EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
+                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
+            O::BBox::CONTAINED);
+  
+  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(100, 100))
+                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
+            O::BBox::CONTAINS);
+
+  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
+                .relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
+            O::BBox::OVERLAP);
+
+  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
+                .relation(O::BBox(O::Point(-10, -2), O::Point(10, 11))),
+            O::BBox::OVERLAP);
+
+  EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
+                .relation(O::BBox(O::Point(-10, -1), O::Point(10, 11))),
+            O::BBox::NO_OVERLAP);
+}
+
+TEST(orthtree, insert2) {
+  typedef Orthtree<2, float, Eigen::Vector2f> O;
+  O t(O::Point::Zero(), 16);
+  for (int i = 0; i <= 360; i += 90) {
+
+    Eigen::Rotation2D<float> rot(float(i) / 180.0 * M_PI);
+    O::Point p = O::Point(-4, 4) + (rot * O::Point(1.9, 0));
+    t.depth(p, 3, [&](auto &tp, auto c) {
+      tp = std::make_unique<O::Point>(p);
+      return true;
+    });
+  }
+  // t.all_data([&](auto &tp, auto c) -> bool {
+    // EXPECT_TRUE(false) << c.transpose() << " - " << tp->transpose();
+    // return true;
+  // });
+
+  t.data_in_bb(O::BBox(O::Point(-2, 0), O::Point(0, 32)),
+               [&](auto &tp, auto c) -> bool {
+                 EXPECT_TRUE(tp) << c.transpose();
+                 EXPECT_TRUE(false) << c.transpose();
+                 return true;
+               });
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}