diff --git a/src/hole_detection_alg.cpp b/src/hole_detection_alg.cpp
index f151cfaf0ecf67a5e30a64199101da8f82942770..2ff16529bbf2381a26f08270af362f9ac57a1037 100644
--- a/src/hole_detection_alg.cpp
+++ b/src/hole_detection_alg.cpp
@@ -37,10 +37,17 @@ int c=0;
 int r=0;
 
 int p [num_cells] = {};
+float threshold [num_cells+1] = {};
 
 //////////////////////////////////////////////////////////////////////////////
 // Hole detection box generation
 //////////////////////////////////////////////////////////////////////////////
+for (int i=0; i<=num_cells; ++i)
+{
+  threshold[i] = (box_y/2)-((num_cells-i)*(box_y/num_cells));
+}
+
+
 for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
 {
   for (size_t colIndex=0; colIndex<cloud_in.width; ++colIndex, ++pointIndex)
@@ -52,8 +59,8 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_in.height; ++rowIndex)
     {
       for (int cell=0; cell<num_cells; ++cell)
       {
-        if ( (box_y/2)-((num_cells-cell)*(box_y/num_cells)) < cloud_in.points[pointIndex].y &&
-          cloud_in.points[pointIndex].y < (box_y/2)-((num_cells-cell-1)*(box_y/num_cells)) )
+        if ( threshold[cell] < cloud_in.points[pointIndex].y &&
+          cloud_in.points[pointIndex].y < threshold[cell+1] )
         {
           cloud_in.points[pointIndex].r=0;
           cloud_in.points[pointIndex].g=int(255*(float(cell)/float(max(1,num_cells-1))));
@@ -136,7 +143,9 @@ for (size_t rowIndex=0, pointIndex=0; rowIndex<5; ++rowIndex)
     {
       if (p[cell] < hole_min_p)
       {
-        cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, (box_y/2)-((num_cells-cell)*(box_y/num_cells)) + 0.05*colIndex, Z));
+        //cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, (box_y/2)-((num_cells-cell)*(box_y/num_cells)) + 0.05*colIndex, Z));
+        //cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, threshold[cell] + 0.01*colIndex, Z));
+        cloud_for->points.push_back (pcl::PointXYZ(box_x_ini, threshold[cell] + (threshold[cell+1]-threshold[cell])*colIndex/5, Z));
       }
     }
   }