diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 5263d124b4b8300858ec0558726c206c14e0da5d..6461a52c7ff747fba69526bafb59bb51d41614d6 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -17,8 +17,9 @@ const int KATA_EVALUATION = 2;
 const float OUT_OF_RANGE = 1000.0;
 const float MAX_RANGE = 100.0;
 
+const int DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD = 0;
+
 const int DATA_N_0_INTENSITY = 0;
-const int DATA_C_2_MEAN_INTENSITY = 2;
 const int DATA_N_1_INTENSITY_VARIANCE = 1;
 const int DATA_N_2_Z_MEAN = 2;
 const int DATA_N_3_Z_VARIANCE = 3;
@@ -147,9 +148,9 @@ const uint8_t R_VERTEX_NOT_CONNECTED_TO_ROOT = 255;
 const uint8_t G_VERTEX_NOT_CONNECTED_TO_ROOT = 0;
 const uint8_t B_VERTEX_NOT_CONNECTED_TO_ROOT = 0;
 
-const uint8_t R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255;
-const uint8_t G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0;
-const uint8_t B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0;
+const uint8_t R_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 0;
+const uint8_t G_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255;
+const uint8_t B_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE = 255;
 
 namespace kf_based_terrain_analysis_lib
 {
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index d8ab35d0a83ba8629a6f37b44cf251433de9b737..b4647309d6aa7c9f80113c7c050df9a1db8cf0bd 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -202,12 +202,12 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm
   {
     Eigen::Vector3d partial_derivatives;
     partial_derivatives(0) = 1.0;
-    partial_derivatives(1) = (point_in_sensor_frame.y - reference_in_sensor_frame.y);
-    partial_derivatives(2) = (point_in_sensor_frame.x - reference_in_sensor_frame.x);
+    partial_derivatives(1) = (point_in_sensor_frame.x - reference_in_sensor_frame.x);
+    partial_derivatives(2) = (point_in_sensor_frame.y - reference_in_sensor_frame.y);
 
-    prediction_std_fast = partial_derivatives.transpose()
+    prediction_std_fast = sqrt(partial_derivatives.transpose()
         * references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]]
-        * partial_derivatives;
+        * partial_derivatives);
   }
 
   float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast;
@@ -489,6 +489,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
                                                                                               // because of the POV (one thing can be seem as obstacle or ground depending on the POV)
         {
           // Now we compute the score
+          // ERROR!! Check this! it should be 1.0 - maha / thresh, isnt it?
           float score = (mahalanobis_distance / filtering_configuration.mahalanobis_threshold) - 1.0;
           if (score > 0.99)
             score = 0.99;
@@ -774,33 +775,14 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
       elevation_cloud_point.b = B_CLASS_UNKNOWN;
     }
 
-    float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(),
-                                elevation_cell_vector[i].z_coordinates.end(), 0.0);
-    float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size();
-    float var = 0;
-    for (int n = 0; n < elevation_cell_vector[i].z_coordinates.size(); ++n)
-      var += ((elevation_cell_vector[i].z_coordinates[n] - z_mean)
-          * (elevation_cell_vector[i].z_coordinates[n] - z_mean));
-
-    var /= (float)elevation_cell_vector[i].z_coordinates.size();
-
-    float sum_intensity = std::accumulate(elevation_cell_vector[i].intensities.begin(),
-                                          elevation_cell_vector[i].intensities.end(), 0.0);
-    float intensity_mean = sum_intensity / (float)elevation_cell_vector[i].intensities.size();
-    float var_intensity = 0;
-    for (int n = 0; n < elevation_cell_vector[i].intensities.size(); ++n)
-      var_intensity += ((elevation_cell_vector[i].intensities[n] - intensity_mean)
-          * (elevation_cell_vector[i].intensities[n] - intensity_mean));
-
-    var_intensity /= (float)elevation_cell_vector[i].intensities.size();
-
     //std::cout << "Intensity of elevation point: mean = " << intensity_mean << "    std = " << sqrt(var_intensity) << std::endl;
 
     // Filling remaining fields
-    elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = intensity_mean;
-    elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = var_intensity;
-    elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean;
-    elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var;
+    elevation_cloud_point.data_n[DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD] =
+        elevation_cell_vector[i].index_of_lowest_point;
+    elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE] = 0.0;
+    elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = 0.0;
+    elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = 0.0;
 
     elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
     elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX] = i; //storing the index for later in labelling step
@@ -844,9 +826,12 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
   root_vertex.y = 0.0;
   root_vertex.z = -1.0 * lidar_configuration.sensor_height;
 
-  root_vertex.r = 0;
-  root_vertex.g = 0;
-  root_vertex.b = 0;
+  // We will keep the correspondence between ground references and original points in the lidar cloud to take advantage of the segmentation
+  // for planning
+  root_vertex.data_c[GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT] = (float)INDEX_UNKNOWN;
+//  root_vertex.r = 0;
+//  root_vertex.g = 0;
+//  root_vertex.b = 0;
 
   root_vertex.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] = VERTEX_CONNECTED_TO_ROOT;
 
@@ -950,8 +935,8 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
         if (std_dev > max_std_dev)
           max_std_dev = std_dev;
 
-        if (std_dev > MAX_STD_DEV)
-          std_dev = MAX_STD_DEV;
+        //if (std_dev > MAX_STD_DEV)
+        //  std_dev = MAX_STD_DEV;
 
         float red = 0.0;
         float green = 255.0 * std_dev / MAX_STD_DEV;
@@ -961,8 +946,11 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
         point.g = (int)green;
         point.b = (int)blue;
 
-        //std::cout << "Storing the point!" << std::endl;
-        pcl_dense_cloud_ptr->push_back(point);
+        if (std_dev < MAX_STD_DEV)
+        {
+          //std::cout << "Storing the point!" << std::endl;
+          pcl_dense_cloud_ptr->push_back(point);
+        }
       }
     }
   }
@@ -1293,7 +1281,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
                 break;
 
               default:
-                assert("ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel: Unrecognized number of outputs in Shallow Neural Network" && false);
+                assert(
+                    "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel: Unrecognized number of outputs in Shallow Neural Network"
+                        && false);
                 break;
             }
           }
@@ -1409,14 +1399,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
             elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE];
 
-        // TODO: we copy the mean intensity value in the channel reserved for the color, we do this because
-        // we have detected a problem with pcl conversions: when passing the point cloud through a ROS topic,
-        // channels c2 and c3 get corrupted, so we avoid to use them, we will investigate this problem in the
-        // future (hopefully)
-        //        pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
-        //            elevation_cloud_point.data_n[DATA_N_0_INTENSITY];
-//          pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] =
-//              elevation_cloud_point.data_n[DATA_N_0_INTENSITY];
         if (filtering_configuration.classify_not_labeled_points_as_obstacles)
         {
           if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
@@ -1473,14 +1455,22 @@ void CKf_Based_Terrain_Analysis::ensureThatConnectionsAreReflectedInBothEnds(
 {
   for (int vertex_iterator = 0; vertex_iterator < ground_reference_cloud_ptr->points.size(); ++vertex_iterator)
   {
+//    if (ground_reference_cloud_ptr->points[vertex_iterator].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS]
+//        != VERTEX_NOT_CONNECTED_TO_ROOT)
+//    {
     for (int connected_vertices_iterator = 0; connected_vertices_iterator < edges[vertex_iterator].size();
         ++connected_vertices_iterator)
     {
       int connected_vertex = edges[vertex_iterator][connected_vertices_iterator];
+//        if (ground_reference_cloud_ptr->points[connected_vertex].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS]
+//            != VERTEX_NOT_CONNECTED_TO_ROOT)
+//        {
       if (std::find(edges[connected_vertex].begin(), edges[connected_vertex].end(), vertex_iterator)
           == edges[connected_vertex].end())
         edges[connected_vertex].push_back(vertex_iterator);
+//        }
     }
+//    }
   }
 }
 
@@ -1756,13 +1746,25 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
 //  std::cout << "Total number of covariance matrices = " << references_covariance_matrix_vector_.size() << std::endl;
 //getchar();
 
-// We can discard ground references if they are not connected with any other, to test this, we need
-// to be sure that connections are reflected in both ends
-  ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges);
-
   fastLabelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
                                       ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
 
+  for (int vertex_iterator = 0; vertex_iterator < ground_reference_cloud_ptr->points.size(); ++vertex_iterator)
+  {
+    // If the point in the original cloud has been classified as obstacle we will want to exclude it from the graph search that might be used for planning by other algorithms
+    if (pcl_cloud_ptr->points[ground_reference_cloud_ptr->points[vertex_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT]].data_c[DATA_C_1_ID_CLASS]
+        == CLASS_OBSTACLE)
+    {
+      ground_reference_cloud_ptr->points[vertex_iterator].data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] =
+          VERTEX_NOT_CONNECTED_TO_ROOT;
+      //edges[vertex_iterator].clear();
+    }
+  }
+
+  // We can discard ground references if they are not connected with any other, to test this, we need
+  // to be sure that connections are reflected in both ends
+  ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges);
+
   // Dense reconstruction aims to serve as debugging or visualizing tool, it generates an XY regular grid with predicted
   // Z values using the closest reference point
   if (filtering_configuration.show_dense_reconstruction)
diff --git a/src/observation_downsampler.cpp b/src/observation_downsampler.cpp
index e6b4a813b490935ea8b6901a6cdfaa14ec266cb6..c49960f5215db441bd191154660b2bd4760044d0 100644
--- a/src/observation_downsampler.cpp
+++ b/src/observation_downsampler.cpp
@@ -98,6 +98,7 @@ void CObservationDownsampler::addGroundObservationInSensorFrame(
   num_points_[index]++;
 
   pcl::PointXYZRGBNormal aux = ground_observation_in_sensor_frame;
+  // TODO: rename this to clarify the process
   aux.data_c[GRND_REF_DATA_C_3_Z_VARIANCE] = azimuth_angle;
   observations_[index].push_back(aux);
 }
@@ -149,9 +150,10 @@ void CObservationDownsampler::generateNewGroundReferences(
       //    << "    Centroid with accums = " << z_centroid << "    variance = " << z_centroid_variance << std::endl;
       //assert(z_centroid == point.z && "something goes wrong!");
 
-      point.r = 0; // just to not leave uninitialized values
-      point.g = 0;
-      point.b = 0;
+      point.data_c[GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT] = (float)median_point.data_n[DATA_N_0_ELEVATION_CLOUD_INDEX_OF_POINT_IN_ORIGINAL_CLOUD];
+//      point.r = 0; // just to not leave uninitialized values
+//      point.g = 0;
+//      point.b = 0;
 
       // Warning! the index is computed as size() because we haven't added the point yet!
       point.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX] = (float)ground_reference_cloud_ptr->points.size();