From 51a4257e9f08809ca47af22307790df914f28d9d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Tue, 12 Jul 2022 19:04:07 +0200
Subject: [PATCH] added fast labelling mode, to enable it just set the
number_of_reference_used_for_labelling to zero
---
include/kf_based_terrain_analysis.h | 8 +
include/structs_definitions.h | 4 +-
src/kf_based_terrain_analysis.cpp | 400 ++++++++++++++++++++++++++--
3 files changed, 395 insertions(+), 17 deletions(-)
diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index 6136571..6a1f953 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -87,6 +87,14 @@ private:
const std::vector<std::vector<int>> &edges,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr);
+ void fastLabelPointcloudUsingGroundModel(
+ const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
+ const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
+ const std::vector<std::vector<int> > &correspondence_indexes,
+ const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
+ const std::vector<std::vector<int>> &edges,
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr);
+
void ensureThatConnectionsAreReflectedInBothEnds(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
std::vector<std::vector<int>> edges);
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 0d59f13..00feb6e 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -20,9 +20,11 @@ const int DATA_N_3_Z_VARIANCE = 3;
const int DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
const int DATA_C_1_ID_CLASS = 1;
-const int DATA_C_2_CAR_PROB = 2;
+const int DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL = 2;
const int DATA_C_3_ORIGINAL_INDEX = 3;
+const int DATA_C_2_CAR_PROB = 2; // TODO: remove this
+
const int GRND_REF_DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
const int GRND_REF_DATA_C_1_ROLL = 1;
const int GRND_REF_DATA_C_2_PITCH = 2;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index a9f266a..dd3c622 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -24,8 +24,8 @@ void CKf_Based_Terrain_Analysis::extractPointsInROI(
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("extractPointsInROI", performance_supervisor_ptr_);
- //std::cout << "Extracting points in ROI: reference.x = " << reference.x << " reference.y = " << reference.y
- // << " reference.z = " << reference.z << std::endl;
+//std::cout << "Extracting points in ROI: reference.x = " << reference.x << " reference.y = " << reference.y
+// << " reference.z = " << reference.z << std::endl;
float x_min, x_max, y_min, y_max, x_search_range, y_search_range;
@@ -387,6 +387,10 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
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;
+
+ // 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;
}
}
else
@@ -413,6 +417,9 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
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;
@@ -560,7 +567,7 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
std::vector<std::vector<kf_based_terrain_analysis_lib::Edge>> &discarded_edges)
{
//if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("labelGroundPoints", performance_supervisor_ptr_);
+ //CFunctionMonitor performance_monitor("labelGroundPoints", performance_supervisor_ptr_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr references_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
@@ -587,13 +594,143 @@ 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,
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_);
+ //CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
pcl::PointCloud<pcl::PointXY>::Ptr pcl_pointcloud_2D_ptr(new pcl::PointCloud<pcl::PointXY>);
pcl::PointXY point;
@@ -605,19 +742,19 @@ void CKf_Based_Terrain_Analysis::generateElevationCloud(
pcl_pointcloud_2D_ptr->points.push_back(point);
}
-//std::cout << "Number of points in pcl_pointcloud_2D_ptr = " << pcl_pointcloud_2D_ptr->points.size() << std::endl;
+ //std::cout << "Number of points in pcl_pointcloud_2D_ptr = " << pcl_pointcloud_2D_ptr->points.size() << std::endl;
pcl::KdTreeFLANN < pcl::PointXY > kdtree_2D;
kdtree_2D.setInputCloud(pcl_pointcloud_2D_ptr);
-// 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;
+ // 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;
+ //std::cout << "Number of points after voxelization = " << elevation_cloud->points.size() << std::endl;
const float DISTANCE_FROM_CELL_CENTER_TO_CELL_CORNER = 0.7071067812
* filtering_configuration.elevation_grid_resolution; //sqrt(2)/2.0
@@ -687,7 +824,7 @@ void CKf_Based_Terrain_Analysis::generateElevationCloud(
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;
+ it->data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
it->data_c[DATA_C_3_ORIGINAL_INDEX] = i++; //storing the index for later in labelling step
//std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
@@ -739,7 +876,7 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr)
{
//if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("createDenseGroundCloud", performance_supervisor_ptr_);
+ CFunctionMonitor performance_monitor("createDenseGroundCloud", performance_supervisor_ptr_);
//std::cout << "setting input cloud" << std::endl;
pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
@@ -831,6 +968,229 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
//std::cout << "Finished dense cloud creation!!" << std::endl;
}
+void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
+ const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
+ const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
+ const std::vector<std::vector<int> > &correspondence_indexes,
+ const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
+ const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr)
+{
+ //if (filtering_configuration.measure_performance)
+ //CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
+ //std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
+ // << std::endl;
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = elevation_cloud_ptr->begin();
+ i != elevation_cloud_ptr->end(); ++i)
+ {
+ pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
+
+ bool elevation_cloud_point_is_ground = false;
+ if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
+ elevation_cloud_point_is_ground = true;
+
+ bool elevation_cloud_point_is_not_predictable_using_the_model = false;
+ int reference_index = (int)std::floor(
+ point_in_sensor_frame.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL]);
+ if (reference_index == INDEX_UNKNOWN)
+ elevation_cloud_point_is_not_predictable_using_the_model = true;
+
+ if (!elevation_cloud_point_is_not_predictable_using_the_model)
+ {
+ pcl::PointXYZRGBNormal reference_in_sensor_frame = ground_reference_cloud_ptr->points[reference_index];
+ if (elevation_cloud_point_is_ground) //&& sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) //TODO!!!
+ {
+ // If the lowest point is ground we need to check one by one the rest of points in the cell
+ // In case we find not-overhanging obstacles in its cell, we will classify the lowest point in cell also as obstacle
+ bool flag_not_overhanging_obstacle_found = false;
+
+ // 1) We point to the vector of indices related to the elevation_cloud point under evaluation
+ std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ + point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
+
+ // and iterate over this vector
+ for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
+ point_iterator != index_iterator->end(); ++point_iterator)
+ {
+ bool observation_z_is_below_prediction = false;
+ float prediction_std_dev = -1.0;
+ float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame,
+ pcl_cloud_ptr->points[*point_iterator], prediction_std_dev,
+ observation_z_is_below_prediction);
+ if (std::isnan(mahalanobis_distance))
+ {
+ std::cout << "posterior pred sigma = " << prediction_std_dev << " uncertainty = "
+ << prediction_std_dev * filtering_configuration.mahalanobis_threshold << " mahalanobis distance = "
+ << mahalanobis_distance << std::endl;
+
+ std::getchar();
+ }
+
+ if (mahalanobis_distance < filtering_configuration.mahalanobis_threshold)
+ {
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = pcl_cloud_ptr->points[*point_iterator].z;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
+ point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
+
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ 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;
+
+ float score = 0.99 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold);
+ if (score < 0.0)
+ score = 0.0;
+ pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
+ // data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
+ // so if there are others segmentators working out there, we do not erase their job!
+
+ // data_c[3] is reserved to store the original index, so we do not want to change it!
+ }
+ else
+ {
+ //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
+
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
+ point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
+
+ //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
+ if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
+ < filtering_configuration.robot_height)
+ {
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
+
+ flag_not_overhanging_obstacle_found = true;
+ }
+ else
+ {
+ //std::cout << "Overhanging obstacle detected!" << std::endl;
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE
+ + 0.99;
+ // data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
+ // so if there are others segmentators working out there, we do not erase their job!
+
+ // data_c[3] is reserved to store the original index, so we do not want to change it!
+ }
+ }
+ }
+ }
+ else // If the lowest point in cell is already an obstacle, the remaining points are also obstacles
+ {
+ //std::cout << "Labeling all points in cell as obstacle!" << std::endl;
+ // 1) We point to the vector of indices related to the elevation_cloud point under evaluation
+ std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ + point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
+
+ for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
+ point_iterator != index_iterator->end(); ++point_iterator)
+ {
+ // Intensity is already in the pointcloud so we do not copy data_n[0] (in fact this value is not present in elevation cloud)
+ float z_ground = i->z;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = z_ground;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
+ point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
+
+ //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
+ if ((pcl_cloud_ptr->points[*point_iterator].z - z_ground) < filtering_configuration.robot_height)
+ {
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
+ }
+ else
+ {
+ //std::cout << "Overhanging obstacle detected!" << std::endl;
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.99;
+ // data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
+ // so if there are others segmentators working out there, we do not erase their job!
+
+ // data_c[3] is reserved to store the original index, so we do not want to change it!
+ }
+ }
+ }
+ }
+ else // if we don't have enough information to try to predict the ground level at this point
+ {
+ std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ + point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
+
+ if (filtering_configuration.classify_not_labeled_points_as_obstacles)
+ {
+ for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
+ point_iterator != index_iterator->end(); ++point_iterator)
+ {
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] =
+ point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
+ pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
+ point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
+
+ //std::cout << "obstacle z distance from ground = " << pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z << std::endl;
+ if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
+ < filtering_configuration.ground_threshold_in_not_analyzed_areas)
+ {
+ 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;
+
+ float score = 0.01; // 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;
+ }
+ else if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
+ < filtering_configuration.robot_height)
+ {
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
+ }
+ else
+ {
+ //std::cout << "Overhanging obstacle detected!" << std::endl;
+ // data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE; // We use instead the r g b channels directly
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.99;
+ // data_c[2] is reserved for Car probability, as we do not estimate it in this algorithm we leave this value untouched
+ // so if there are others segmentators working out there, we do not erase their job!
+
+ // data_c[3] is reserved to store the original index, so we do not want to change it!
+ }
+ }
+ }
+ }
+ }
+ //std::cout << "Labeling concluded!" << std::endl;
+}
+
void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
@@ -839,7 +1199,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr)
{
//if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
+ //CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
//std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
// << std::endl;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = elevation_cloud_ptr->begin();
@@ -925,7 +1285,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
{
pcl::PointXYZRGBNormal reference_in_sensor_frame =
ground_reference_cloud_ptr->points[pointIdxKNNSearch[best_reference_index]];
- if (elevation_cloud_point_is_ground)//&& sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) //TODO!!!
+ if (elevation_cloud_point_is_ground) //&& sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) //TODO!!!
{
//std::cout << "std_dev_z = " << sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) << std::endl;
// We classify the elevation cloud point as ground
@@ -1126,7 +1486,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
float score = 0.01; // 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;
}
- else if((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
+ else if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
< filtering_configuration.robot_height)
{
// data_c[0] is reserved to cast the RGB values into a float (PCL convention) so we do not use it
@@ -1236,9 +1596,17 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// to be sure that connections are reflected in both ends
ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges);
- labelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
- ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
-
+ if (filtering_configuration.number_of_references_used_for_labelling > 0)
+ {
+ labelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
+ ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
+ }
+ else
+ {
+ //std::cout << "Using fast labelling function!" << std::endl;
+ fastLabelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
+ ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
+ }
// 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
--
GitLab