diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index c05b933195cba85d91ba9f40d8303a83f43b7b53..231983975baafefe2b2fe512bbda4fe4bc16a866 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -652,10 +652,10 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
   std::cout << "Starting detectLowObstacles function!" << std::endl;
   float res = res_deg * M_PI / 180.0;
   float r_max = h / alpha;
-  float K = 1 / sin(res);
+  float K = 1.0 / sin(res);
   float d_max = K * r_max;
-  float cell_side = r_max / sqrt(8);
-  int num_of_cells = (d_max * 2 / cell_side);
+  float cell_side = r_max / sqrt(8.0);
+  int num_of_cells = (d_max * 2.0 / cell_side);
 
   std::cout << "r_max = "       << r_max << std::endl;
   std::cout << "K = "            << K << std::endl;
@@ -670,8 +670,8 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
     if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
     {
       //std::cout << "Analyzing ground point!" << std::endl;
-      int x = i->x / cell_side;
-      int y = i->y / cell_side;
+      int x = i->x / cell_side + num_of_cells / 2.0;
+      int y = i->y / cell_side + num_of_cells / 2.0;
       //std::cout << "x cell number = " << x << "  y cell number = " << y << std::endl;
       if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
       {
@@ -718,8 +718,8 @@ void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float
   {
     if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
     {
-      int x = i->x / cell_side;
-      int y = i->y / cell_side;
+      int x = i->x / cell_side + num_of_cells / 2.0;
+      int y = i->y / cell_side + num_of_cells / 2.0;
       if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
       {
         if (grid[x][y].label == 1)