Revert VoxelHashMap change -> Allocations go in a separate PR

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -31,11 +31,18 @@
 
 namespace {
 using kiss_icp::Voxel;
-static const std::array<Voxel, 27> shifts{
-    {{0, 0, 0},   {1, 0, 0},   {-1, 0, 0}, {0, 1, 0},   {0, -1, 0},  {0, 0, 1},   {0, 0, -1},
-     {1, 1, 0},   {1, -1, 0},  {-1, 1, 0}, {-1, -1, 0}, {1, 0, 1},   {1, 0, -1},  {-1, 0, 1},
-     {-1, 0, -1}, {0, 1, 1},   {0, 1, -1}, {0, -1, 1},  {0, -1, -1}, {1, 1, 1},   {1, 1, -1},
-     {1, -1, 1},  {1, -1, -1}, {-1, 1, 1}, {-1, 1, -1}, {-1, -1, 1}, {-1, -1, -1}}};
+
+std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
+    std::vector<Voxel> voxel_neighborhood;
+    for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
+        for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) {
+            for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) {
+                voxel_neighborhood.emplace_back(i, j, k);
+            }
+        }
+    }
+    return voxel_neighborhood;
+}
 }  // namespace
 
 namespace kiss_icp {
@@ -44,11 +51,12 @@ std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
     const Eigen::Vector3d &query) const {
     // Convert the point to voxel coordinates
     const auto &voxel = PointToVoxel(query, voxel_size_);
+    // Get nearby voxels on the map
+    const auto &query_voxels = GetAdjacentVoxels(voxel);
     // Find the nearest neighbor
     Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
     double closest_distance = std::numeric_limits<double>::max();
-    std::for_each(shifts.cbegin(), shifts.cend(), [&](const auto &voxel_shift) {
-        const auto &query_voxel = voxel + voxel_shift;
+    std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) {
         auto search = map_.find(query_voxel);
         if (search != map_.end()) {
             const auto &points = search.value();