From 800eb229b27f00d24eb1c3c34ee2ab7c63ce7567 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Wed, 13 Jul 2022 08:30:49 +0200
Subject: [PATCH] improved fast labelling, now achieving even better results
than with the slow method!
---
src/kf_based_terrain_analysis.cpp | 205 +++++++-----------------------
1 file changed, 48 insertions(+), 157 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index dd3c622..66baf53 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -367,30 +367,40 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]]);
}
- //std::cout << "Updating elevation cloud point to represent ground data" << std::endl;
- if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND) // if the point was seen as obstacle from another POV
- // or was unexplored, with set it to ground class
- {
- //std::cout << "Storing z point as z ground" << std::endl;
- // as now the point is classified as ground, we store its own z value as z ground
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] =
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].z;
+ // now we compute the score
+ float score = 0.99 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold);
+ if (score < 0.0)
+ score = 0.0;
- // set the class as ground
- float score = 0.99 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold);
- if (score < 0.0)
- score = 0.0;
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
- (float)CLASS_GROUND + score;
-
- // colour the point properly
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].r = R_CLASS_GROUND;
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].g = G_CLASS_GROUND;
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].b = B_CLASS_GROUND;
+ float index_of_labelling_ref =
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL];
- // and store the reference for the later full pointcloud labelling step
+ //std::cout << "Updating elevation cloud point to represent ground data" << std::endl;
+ if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND
+ || std::floor((int)index_of_labelling_ref) == INDEX_UNKNOWN
+ || index_of_labelling_ref - std::floor((int)index_of_labelling_ref) < score) // if the point was seen as obstacle from another POV
+ // or was unexplored, with set it to ground class
+ {
+ // and store the reference for the later full pointcloud labelling step (only if the score is better than the previous label
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] =
- reference_index;
+ reference_index + score;
+
+ if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND)
+ {
+ //std::cout << "Storing z point as z ground" << std::endl;
+ // as now the point is classified as ground, we store its own z value as z ground
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_n[DATA_N_1_Z_GROUND] =
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].z;
+
+ // set the class as ground
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
+ (float)CLASS_GROUND + score;
+
+ // colour the point properly
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].r = R_CLASS_GROUND;
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].g = G_CLASS_GROUND;
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].b = B_CLASS_GROUND;
+ }
}
}
else
@@ -399,9 +409,23 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << " z = " << point_in_sensor_frame.z << std::endl << "mahalanobis_distance = " << mahalanobis_distance
// << " mahalanobis_threshold = " << filtering_configuration.mahalanobis_threshold << std::endl;
- if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we only store the data if it has not been done
- // previously
+ if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) != CLASS_GROUND) // we don't want to convert ground points to obstacles
+ // because of the POV (one thing can be seem as obstacle or ground depending on the POV)
{
+ // Now we compute the score
+ float score = (mahalanobis_distance / filtering_configuration.mahalanobis_threshold) - 1.0;
+ if (score > 0.99)
+ score = 0.99;
+
+ // Now we check the score of the previous detection
+ float index_of_labelling_ref =
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL];
+
+ if (std::floor((int)index_of_labelling_ref) == INDEX_UNKNOWN
+ || index_of_labelling_ref - std::floor((int)index_of_labelling_ref) < score)
+ elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] =
+ reference_index + score;
+
// if it is an obstacle point we use the ground reference z prediction as z ground
float z_ground = predictZ(ground_reference_cloud_ptr->points[reference_index], point_in_sensor_frame.x,
point_in_sensor_frame.y);
@@ -410,16 +434,13 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// set the class to OBSTACLE
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_1_ID_CLASS] =
- (float)CLASS_OBSTACLE + 0.99;
+ (float)CLASS_OBSTACLE + score; //0.99;
// and colour the point
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].r = R_CLASS_OBSTACLE;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].g = G_CLASS_OBSTACLE;
elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].b = B_CLASS_OBSTACLE;
}
- // and store the reference for the later full pointcloud labelling step
- elevation_cloud_ptr->points[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] =
- reference_index;
}
}
//std::cout << "Observations for local tangent plane estimation extracted!" << std::endl;
@@ -594,136 +615,6 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
// getchar();
}
-// Just a try to optimize this function, it turned out to be slower :(
-//void CKf_Based_Terrain_Analysis::generateElevationCloud(
-// const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
-// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
-// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud, std::vector<std::vector<int>> &correspondence_indexes)
-//{
-// //if (filtering_configuration.measure_performance)
-// CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
-//
-// int i = 0;
-// for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
-// ++it)
-// it->data_c[DATA_C_3_ORIGINAL_INDEX] = i++;
-//
-// // We want to find the x and y values, but not interested in z
-// //std::cout << "Elevation grid resolution = " << filtering_configuration.elevation_grid_resolution << std::endl;
-// pcl::VoxelGrid < pcl::PointXYZRGBNormal > voxel_grid;
-// voxel_grid.setInputCloud(pcl_cloud_ptr);
-// voxel_grid.setLeafSize(filtering_configuration.elevation_grid_resolution,
-// filtering_configuration.elevation_grid_resolution, OUT_OF_RANGE);
-// voxel_grid.filter(*elevation_cloud);
-// //std::cout << "Number of points after voxelization = " << elevation_cloud->points.size() << std::endl;
-//
-// pcl::PassThrough < pcl::PointXYZRGBNormal > pass;
-// pass.setInputCloud(pcl_cloud_ptr);
-//
-// i = 0;
-// for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it = elevation_cloud->begin(); it != elevation_cloud->end();
-// ++it)
-// {
-// float x_min, x_max, y_min, y_max, x_search_range, y_search_range;
-//
-// x_search_range = filtering_configuration.elevation_grid_resolution;
-// y_search_range = filtering_configuration.elevation_grid_resolution;
-//
-// x_min = it->x - x_search_range;
-// y_min = it->y - y_search_range;
-// x_max = it->x + x_search_range;
-// y_max = it->y + y_search_range;
-//
-// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_cell(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-//
-// // Extracting lidar points
-// pass.setFilterFieldName("x");
-// pass.setFilterLimits(x_min, x_max);
-// pass.filter(*points_in_cell);
-// pass.setInputCloud(points_in_cell);
-// pass.setFilterFieldName("y");
-// pass.setFilterLimits(y_min, y_max);
-// pass.filter(*points_in_cell);
-//
-// std::vector<int> pointIdxRadiusSearch;
-// for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator cell_point_iterator = points_in_cell->begin();
-// cell_point_iterator != points_in_cell->end(); ++cell_point_iterator)
-// {
-// //std::cout << "id = " << cell_point_iterator->data_c[DATA_C_3_ORIGINAL_INDEX] << std::endl;
-// pointIdxRadiusSearch.push_back(cell_point_iterator->data_c[DATA_C_3_ORIGINAL_INDEX]);
-// }
-// //std::cout << "Total number of points in cell = " << pointIdxRadiusSearch.size() << std::endl;
-//
-// std::vector<float> z_coordinates;
-// std::vector<float> x_coordinates;
-// std::vector<float> y_coordinates;
-//
-// bool outlier_detected = false;
-// for (std::vector<int>::const_iterator iterator = pointIdxRadiusSearch.begin();
-// iterator != pointIdxRadiusSearch.end(); ++iterator)
-// {
-// //std::cout << "storing coordinates of point in cell with id = " << *iterator << std::endl;
-// z_coordinates.push_back(pcl_cloud_ptr->points[*iterator].z);
-// x_coordinates.push_back(pcl_cloud_ptr->points[*iterator].x);
-// y_coordinates.push_back(pcl_cloud_ptr->points[*iterator].y);
-//
-// if (pcl_cloud_ptr->points[*iterator].data_c[DATA_C_1_ID_CLASS] == OUTLIER)
-// outlier_detected = true;
-// }
-//
-// if (outlier_detected)
-// {
-// //std::cout << "Outlier detected!" << std::endl;
-// it->data_c[DATA_C_1_ID_CLASS] = OUTLIER;
-// it->r = R_CLASS_OUTLIER;
-// it->g = G_CLASS_OUTLIER;
-// it->b = B_CLASS_OUTLIER;
-// }
-// else
-// {
-// //std::cout << "Not outlier, setting points to unknown class" << std::endl;
-// it->data_c[DATA_C_1_ID_CLASS] = CLASS_UNKNOWN;
-// it->r = R_CLASS_UNKNOWN;
-// it->g = G_CLASS_UNKNOWN;
-// it->b = B_CLASS_UNKNOWN;
-// }
-//
-// //std::cout << "Computing cell statistics!" << std::endl;
-// float sum = std::accumulate(z_coordinates.begin(), z_coordinates.end(), 0.0);
-// float z_mean = sum / (float)z_coordinates.size();
-// float var = 0;
-// for (int n = 0; n < z_coordinates.size(); ++n)
-// var += ((z_coordinates[n] - z_mean) * (z_coordinates[n] - z_mean));
-//
-// var /= (float)z_coordinates.size();
-//
-// //std::cout << "Retrieving the position of the lowest point in cell..." << std::endl;
-// std::vector<float>::iterator lowest_point_in_cell = std::min_element(z_coordinates.begin(), z_coordinates.end());
-// int position_in_vector = std::distance(z_coordinates.begin(), lowest_point_in_cell);
-//
-// it->z = z_coordinates[position_in_vector];
-// it->x = x_coordinates[position_in_vector];
-// it->y = y_coordinates[position_in_vector];
-//
-// //std::cout << "Filling remaining fields" << std::endl;
-// it->data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
-// it->data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
-// it->data_n[DATA_N_2_Z_MEAN] = z_mean;
-// it->data_n[DATA_N_3_Z_VARIANCE] = var;
-//
-// it->data_c[DATA_C_2_CAR_PROB] = PROB_UNKNOWN;
-//
-// //std::cout << "storing the index for later in labelling step" << std::endl;
-// it->data_c[DATA_C_3_ORIGINAL_INDEX] = i++;
-//
-// //std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
-// //std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
-//
-// //std::cout << "storing the vector of indexes" << std::endl;
-// correspondence_indexes.push_back(pointIdxRadiusSearch);
-// }
-//}
-
void CKf_Based_Terrain_Analysis::generateElevationCloud(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
--
GitLab