diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 8ad9cb0557b39e17d68ba449a9651c2d8da10698..47ee66f031446ec61f612344bb6fec25d8d54adf 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1253,7 +1253,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
                 if (predicted_class == CLASS_OBSTACLE)
                 {
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] =
-                      (float)predicted_class + 0.0;
+                      (float)CLASS_OBSTACLE + 0.0;
 
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_NON_TRAVERSABLE;
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_NON_TRAVERSABLE;
@@ -1262,7 +1262,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
                 else
                 {
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] =
-                      (float)predicted_class + (float)scores[n];
+                      (float)CLASS_GROUND + (float)scores[n];
 
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_GROUND;
                   pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_GROUND;