From da732ced40ff1b221a728930ad55fbe4b2def28d Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Thu, 16 Mar 2023 18:46:13 +0100
Subject: [PATCH] debugging labelling in not analyzed areas
---
src/kf_based_terrain_analysis.cpp | 143 +++++++++++++++---------------
1 file changed, 71 insertions(+), 72 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index ea46cf7..1e3f454 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -893,9 +893,9 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
float min_std_dev = 1000.0;
float max_std_dev = -1.0;
- for (float i = -20.0; i < 20.0; i += 0.2)
+ for (float i = -40.0; i < 40.0; i += 0.2)
{
- for (float j = -20.0; j < 20.0; j += 0.2)
+ for (float j = -40.0; j < 40.0; j += 0.2)
{
pcl::PointXYZRGBNormal point;
point.x = i;
@@ -1038,9 +1038,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND;
-// 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_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
pcl::PointXYZRGBNormal point_to_evaluate = pcl_cloud_ptr->points[*point_iterator];
@@ -1090,12 +1090,12 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// 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;
+ 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
@@ -1109,12 +1109,12 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// 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].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;
}
@@ -1224,51 +1224,59 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] =
(float)predicted_class + 0.0;
+
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_OBSTACLE;
}
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];
+
+ 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;
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_GROUND;
}
//std::cout << "Class + score = " << pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] << std::endl;
- 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;
- //std::cout << "NN says point is road!" << std::endl;
- 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;
- std::cout << "NN says point is sidewalk!" << std::endl;
- 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;
- std::cout << "NN says point is terrain!" << std::endl;
- 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;
- std::cout << "NN says point is vegetation!" << std::endl;
- 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;
- std::cout << "NN says point is Obstacle!" << std::endl;
- break;
- }
+// 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;
+// //std::cout << "NN says point is road!" << std::endl;
+// 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;
+// //std::cout << "NN says point is sidewalk!" << std::endl;
+// 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;
+// //std::cout << "NN says point is terrain!" << std::endl;
+// 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;
+// //std::cout << "NN says point is vegetation!" << std::endl;
+// 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;
+// //std::cout << "NN says point is Obstacle!" << std::endl;
+// break;
+// }
}
}
@@ -1400,12 +1408,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// 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;
+ pcl_cloud_ptr->points[*point_iterator].r = 255;//R_CLASS_GROUND; // We use instead the r g b channels directly
+ pcl_cloud_ptr->points[*point_iterator].g = 255;//G_CLASS_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].b = 0;//B_CLASS_GROUND;
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;
@@ -1416,12 +1421,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// 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;
+ pcl_cloud_ptr->points[*point_iterator].r = 255;//R_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = 255;//G_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = 0;//B_CLASS_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + score;
@@ -1431,12 +1433,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
// which is the one storing RGB values packed as a float, so colour is disabled at the moment
-// 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].r = 255;//R_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = 255;//G_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = 0;//B_CLASS_OVERHANGING_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE
--
GitLab