diff --git a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp
index dec023d..6f2bde1 100644
--- a/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp
+++ b/cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp
@@ -26,6 +26,7 @@
 #include <Eigen/Core>
 #include <array>
 #include <bonxai/bonxai.hpp>
+#include <cstdint>
 #include <sophus/se3.hpp>
 
 #include "bonxai/grid_coord.hpp"
@@ -43,7 +44,10 @@ static constexpr std::array<Bonxai::CoordT, 27> shifts{
     Bonxai::CoordT{1, -1, -1},  Bonxai::CoordT{1, -1, 0},  Bonxai::CoordT{1, -1, 1},
     Bonxai::CoordT{1, 0, -1},   Bonxai::CoordT{1, 0, 0},   Bonxai::CoordT{1, 0, 1},
     Bonxai::CoordT{1, 1, -1},   Bonxai::CoordT{1, 1, 0},   Bonxai::CoordT{1, 1, 1}};
-}
+
+static constexpr uint8_t inner_grid_log2_size = 2;
+static constexpr uint8_t leaf_grid_log2_size = 2;
+}  // namespace
 
 namespace kinematic_icp {
 
@@ -53,7 +57,7 @@ SparseVoxelGrid::SparseVoxelGrid(const double voxel_size,
     : voxel_size_(voxel_size),
       clipping_distance_(clipping_distance),
       max_points_per_voxel_(max_points_per_voxel),
-      map_(voxel_size),
+      map_(voxel_size, inner_grid_log2_size, leaf_grid_log2_size),
       accessor_(map_.createAccessor()) {}
 
 std::tuple<Eigen::Vector3d, double> SparseVoxelGrid::GetClosestNeighbor(