diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index fca0392e92b737c7a1963fd16fde3b79fef91dbf..5f5d265c10fe7c1782a0e6214332c2c8f2055aa1 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1089,6 +1089,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
// We don't use score with obstacle points, if they are considered obstacles, we believe that their
// probability of being ground is zero, the class label is used to distinguish between general obstacles
@@ -1108,6 +1111,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OVERHANGING_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.0;
}
@@ -1183,28 +1189,38 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
features(12, 0) = var_of_scores;
int predicted_class = nn_.predict(features);
- pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] = predicted_class;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] = (float)predicted_class + (float)scores[n];
switch(predicted_class)
{
case ROAD:
-
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_ROAD;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_ROAD;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_ROAD;
break;
case SIDEWALK:
-
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_SIDEWALK;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_SIDEWALK;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_SIDEWALK;
break;
case TERRAIN:
-
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_TERRAIN;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_TERRAIN;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_TERRAIN;
break;
case VEGETATION:
-
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_VEGETATION;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_VEGETATION;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_VEGETATION;
break;
case CLASS_OBSTACLE:
-
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_KITTI_OBSTACLE;
break;
}
}
@@ -1335,6 +1351,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND; // We use instead the r g b channels directly
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_ROAD;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_ROAD;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_ROAD;
float score = 0.0; // we don't have too much confidence in these points, because they are not analyzed
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
@@ -1348,6 +1367,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + score;
@@ -1360,6 +1382,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OVERHANGING_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE