diff --git a/slam_lib/include/LidarSlam/RollingGrid.h b/slam_lib/include/LidarSlam/RollingGrid.h
index 50358578f82ff7506851c0c5846d18bfee725d14..550858b1d4dd4b00adc51d0b9adb882ca95f0214 100644
--- a/slam_lib/include/LidarSlam/RollingGrid.h
+++ b/slam_lib/include/LidarSlam/RollingGrid.h
@@ -80,6 +80,9 @@ public:
   //! Extract all points in map lying in given bounding box
   PointCloud::Ptr Get(const Eigen::Array3d& minPoint, const Eigen::Array3d& maxPoint) const;
 
+  //! Extract all points in map that are close to points from given cloud
+  PointCloud::Ptr Get(const PointCloud& pcToMatch) const;
+
   //! Get all points
   PointCloud::Ptr Get() const;
 
diff --git a/slam_lib/src/RollingGrid.cxx b/slam_lib/src/RollingGrid.cxx
index e1d792eb9e474e5970e7ded301df2efc639ea4b1..9455c016e361eae705afcd3fee513cd6d3459990 100644
--- a/slam_lib/src/RollingGrid.cxx
+++ b/slam_lib/src/RollingGrid.cxx
@@ -20,6 +20,8 @@
 #include "LidarSlam/RollingGrid.h"
 #include "LidarSlam/Utilities.h"
 
+#include <unsupported/Eigen/CXX11/Tensor>
+
 // A new PCL Point is added so we need to recompile PCL to be able to use
 // filters (pcl::VoxelGrid) with this new type
 #ifndef PCL_NO_PRECOMPILE
@@ -135,6 +137,74 @@ RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Array3d& minPoint, co
   return intersection;
 }
 
+//------------------------------------------------------------------------------
+RollingGrid::PointCloud::Ptr RollingGrid::Get(const PointCloud& pcToMatch) const
+{
+  // Identify voxels in which lie input points, and the bounding box of the non-empty voxels
+  Eigen::Tensor<float, 3> pointsInVoxels(this->GridSize, this->GridSize, this->GridSize);
+  pointsInVoxels.setZero();
+  Eigen::Array3i voxelGridOrigin = this->PositionToVoxel(this->VoxelGridPosition) - this->GridSize / 2;
+  Eigen::Array3i minCell, maxCell;
+  minCell.setConstant(this->GridSize - 1);
+  maxCell.setConstant(0);
+  for (const Point& point : pcToMatch)
+  {
+    // Find the voxel containing this point
+    Eigen::Array3i cubeIdx = this->PositionToVoxel(point.getArray3fMap()) - voxelGridOrigin;
+    // Notify the voxel if it is within grid
+    if (((0 <= cubeIdx) && (cubeIdx < this->GridSize)).all())
+    {
+      pointsInVoxels(cubeIdx.x(), cubeIdx.y(), cubeIdx.z())++;
+      minCell = minCell.min(cubeIdx);
+      maxCell = maxCell.max(cubeIdx);
+    }
+  }
+
+  // Check if maxPoint is greater than minPoint.
+  // If not, this means that no point from pcToMatch lies in rolling grid.
+  if ((minCell > maxCell).any())
+    return PointCloud::Ptr(new PointCloud);
+
+  // Extract non empty part of rolling grid using bounding box
+  // We do that to to save time by avoiding convolving the entire rolling grid
+  Eigen::array<int, 3> offsets = {minCell.x(), minCell.y(), minCell.z()};
+  Eigen::array<int, 3> extents = {maxCell.x() - minCell.x() + 1, maxCell.y() - minCell.y() + 1, maxCell.z() - minCell.z() + 1};
+  Eigen::Tensor<float, 3> nonEmptyVoxels = pointsInVoxels.slice(offsets, extents);
+
+  // Dilate the votes by convolving with blur kernel
+  Eigen::TensorFixedSize<float, Eigen::Sizes<3, 3, 3>> kernel;
+  constexpr float centerWeight = 1.;
+  constexpr float orthoWeight  = 0.4;
+  constexpr float diagWeight   = 0.2;
+  constexpr float cornerWeight = 0.1;
+  kernel.setValues({{{cornerWeight,   diagWeight,  cornerWeight},
+                     {  diagWeight,  orthoWeight,    diagWeight},
+                     {cornerWeight,   diagWeight,  cornerWeight}},
+
+                    {{  diagWeight,  orthoWeight,    diagWeight},
+                     { orthoWeight, centerWeight,   orthoWeight},
+                     {  diagWeight,  orthoWeight,    diagWeight}},
+
+                    {{cornerWeight,   diagWeight,  cornerWeight},
+                     {  diagWeight,  orthoWeight,    diagWeight},
+                     {cornerWeight,   diagWeight,  cornerWeight}}});
+  // Perform SAME convolution by adding padding
+  Eigen::array<std::pair<int, int>, 3> padding = {{{1, 1}, {1, 1}, {1, 1}}};
+  Eigen::array<int, 3> dims = {0, 1, 2};
+  Eigen::Tensor<float, 3> voxelsToUse = nonEmptyVoxels.pad(padding).eval().convolve(kernel, dims);
+
+  // Extract all points from voxels to use
+  constexpr float THRESHOLD = 1.;
+  PointCloud::Ptr intersection(new PointCloud);
+  for (int x = 0; x < voxelsToUse.dimension(0); x++)
+    for (int y = 0; y < voxelsToUse.dimension(1); y++)
+      for (int z = 0; z < voxelsToUse.dimension(2); z++)
+        if (voxelsToUse(x, y, z) >= THRESHOLD)
+          *intersection += *(this->Grid[x + minCell.x()][y + minCell.y()][z + minCell.z()]);
+
+  return intersection;
+}
+
 //------------------------------------------------------------------------------
 RollingGrid::PointCloud::Ptr RollingGrid::Get() const
 {
diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx
index 6f7d15fbb39481df2706c559f474876c105dc68c..06dd5b05177f1d6cdcd2246be7d6c950cbd38ef8 100644
--- a/slam_lib/src/Slam.cxx
+++ b/slam_lib/src/Slam.cxx
@@ -939,14 +939,12 @@ void Slam::Localization()
 
   auto extractMapKeypointsAndBuildKdTree = [this](const PointCloud::Ptr& currKeypoints, RollingGrid& map, PointCloud::Ptr& prevKeypoints, KDTree& kdTree)
   {
-    // Estimate current keypoints bounding box
+    // Estimate current keypoints WORLD positions
     PointCloud currWordKeypoints;
     pcl::transformPointCloud(*currKeypoints, currWordKeypoints, this->Tworld.matrix());
-    Eigen::Vector4f minPoint, maxPoint;
-    pcl::getMinMax3D(currWordKeypoints, minPoint, maxPoint);
 
-    // Extract all points in maps lying in this bounding box
-    prevKeypoints = map.Get(minPoint.head<3>().cast<double>().array(), maxPoint.head<3>().cast<double>().array());
+    // Extract all points in maps lying near these points
+    prevKeypoints = map.Get(currWordKeypoints);
     kdTree.Reset(prevKeypoints);
   };