From 6f1695f9e3b47812de6324cb673699da8c8cdb25 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 12:17:35 +0200
Subject: [PATCH] improved optimized elevation cloud generation to avoid
running out of memory
---
src/kf_based_terrain_analysis.cpp | 242 ++++++++++++++++++++++--------
1 file changed, 176 insertions(+), 66 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 978c863..0ff31b1 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -734,97 +734,206 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
int number_of_rows_and_columns = std::ceil(2.0 * MAX_RANGE / filtering_configuration.elevation_grid_resolution);
//std::cout << "number_of_rows_and_columns = " << number_of_rows_and_columns << std::endl;
- kf_based_terrain_analysis_lib::ElevationCloudCell elevation_map[number_of_rows_and_columns][number_of_rows_and_columns];
+ int elevation_map[number_of_rows_and_columns][number_of_rows_and_columns];
+ for (int i = 0; i < number_of_rows_and_columns; ++i)
+ for (int j = 0; j < number_of_rows_and_columns; ++j)
+ elevation_map[i][j] = INDEX_UNKNOWN;
//std::cout << "elevation_map created! " << std::endl;
+ std::vector<kf_based_terrain_analysis_lib::ElevationCloudCell> elevation_cell_vector;
+
int i = 0;
//std::cout << "Starting to fill the elevation_map! " << std::endl;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
++it)
{
- int row = std::round((number_of_rows_and_columns / 2.0) + (it->x / filtering_configuration.elevation_grid_resolution));
- int col = std::round((number_of_rows_and_columns / 2.0) + (it->y / filtering_configuration.elevation_grid_resolution));
+ int row = std::round(
+ (number_of_rows_and_columns / 2.0) + (it->x / filtering_configuration.elevation_grid_resolution));
+ int col = std::round(
+ (number_of_rows_and_columns / 2.0) + (it->y / filtering_configuration.elevation_grid_resolution));
- if(elevation_map[row][col].indexes_of_points_in_cell.empty())
+ if (elevation_map[row][col] == INDEX_UNKNOWN)
{
+ elevation_map[row][col] = elevation_cell_vector.size();
+ kf_based_terrain_analysis_lib::ElevationCloudCell new_cell;
//std::cout << "Pointing to an empty cell, storing current point as lowest, and keeping its index! " << std::endl;
//std::cout << "Point number " << i << " row = " << row << " col = " << col << std::endl;
- elevation_map[row][col].lowest_point_in_cell = *it;
- elevation_map[row][col].index_of_lowest_point = i;
+ new_cell.lowest_point_in_cell = *it;
+ new_cell.index_of_lowest_point = i;
+ elevation_cell_vector.push_back(new_cell);
}
else
{
//std::cout << "Pointing to a NON-empty cell! checking z!" << std::endl;
- if(it->z < elevation_map[row][col].lowest_point_in_cell.z)
+ if (it->z < elevation_cell_vector[elevation_map[row][col]].lowest_point_in_cell.z)
{
//std::cout << "Current point is the lowest, storing it as it is!" << std::endl;
- elevation_map[row][col].lowest_point_in_cell = *it;
- elevation_map[row][col].index_of_lowest_point = i;
+ elevation_cell_vector[elevation_map[row][col]].lowest_point_in_cell = *it;
+ elevation_cell_vector[elevation_map[row][col]].index_of_lowest_point = i;
}
}
//std::cout << "Storing z coordinate and index in the indexes vector" << std::endl;
- elevation_map[row][col].z_coordinates.push_back(it->z);
- elevation_map[row][col].indexes_of_points_in_cell.push_back(i);
+ elevation_cell_vector[elevation_map[row][col]].z_coordinates.push_back(it->z);
+ elevation_cell_vector[elevation_map[row][col]].indexes_of_points_in_cell.push_back(i);
i++;
}
//std::cout << "Now generating the elevation cloud from the elevation map!" << std::endl;
//DEBUG
//int total_number_of_points_stored = 0;
- int index_respect_elevation_cloud = 0;
- for(int row = 0; row < number_of_rows_and_columns; ++row)
+
+ for (int i = 0; i < elevation_cell_vector.size(); ++i)
{
- for(int col = 0; col < number_of_rows_and_columns; ++col)
+ pcl::PointXYZRGBNormal elevation_cloud_point = elevation_cell_vector[i].lowest_point_in_cell;
+ if (elevation_cloud_point.data_c[DATA_C_1_ID_CLASS] == OUTLIER)
{
- if(!elevation_map[row][col].indexes_of_points_in_cell.empty())
- {
- pcl::PointXYZRGBNormal elevation_cloud_point = elevation_map[row][col].lowest_point_in_cell;
- if (elevation_cloud_point.data_c[DATA_C_1_ID_CLASS] == OUTLIER)
- {
- elevation_cloud_point.r = R_CLASS_OUTLIER;
- elevation_cloud_point.g = G_CLASS_OUTLIER;
- elevation_cloud_point.b = B_CLASS_OUTLIER;
- }
- else
- {
- elevation_cloud_point.r = R_CLASS_UNKNOWN;
- elevation_cloud_point.g = G_CLASS_UNKNOWN;
- elevation_cloud_point.b = B_CLASS_UNKNOWN;
- }
- float sum = std::accumulate(elevation_map[row][col].z_coordinates.begin(), elevation_map[row][col].z_coordinates.end(), 0.0);
- float z_mean = sum / (float)elevation_map[row][col].z_coordinates.size();
- float var = 0;
- for (int n = 0; n < elevation_map[row][col].z_coordinates.size(); ++n)
- var += ((elevation_map[row][col].z_coordinates[n] - z_mean) * (elevation_map[row][col].z_coordinates[n] - z_mean));
+ elevation_cloud_point.r = R_CLASS_OUTLIER;
+ elevation_cloud_point.g = G_CLASS_OUTLIER;
+ elevation_cloud_point.b = B_CLASS_OUTLIER;
+ }
+ else
+ {
+ elevation_cloud_point.r = R_CLASS_UNKNOWN;
+ elevation_cloud_point.g = G_CLASS_UNKNOWN;
+ elevation_cloud_point.b = B_CLASS_UNKNOWN;
+ }
+ float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(),
+ elevation_cell_vector[i].z_coordinates.end(), 0.0);
+ float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size();
+ float var = 0;
+ for (int n = 0; n < elevation_cell_vector[i].z_coordinates.size(); ++n)
+ var += ((elevation_cell_vector[i].z_coordinates[n] - z_mean)
+ * (elevation_cell_vector[i].z_coordinates[n] - z_mean));
- var /= (float)elevation_map[row][col].z_coordinates.size();
+ var /= (float)elevation_cell_vector[i].z_coordinates.size();
- // Filling remaining fields
- elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
- elevation_cloud_point.data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
- elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean;
- elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var;
+ // Filling remaining fields
+ elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
+ elevation_cloud_point.data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
+ elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean;
+ elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var;
- elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
- elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX] = index_respect_elevation_cloud++; //storing the index for later in labelling step
+ elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
+ elevation_cloud_point.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;
- //std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
+ //std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
+ //std::cout << "elevation_cell_vector[i].z_coordinates.size() = " << z_coordinates.size() << std::endl;
- //DEBUG!
- //total_number_of_points_stored += elevation_map[row][col].indexes_of_points_in_cell.size();
+ //DEBUG!
+ //total_number_of_points_stored += elevation_map[row][col].indexes_of_points_in_cell.size();
- correspondence_indexes.push_back(elevation_map[row][col].indexes_of_points_in_cell);
- //std::cout << "Storing point into elevation cloud!" << std::endl;
- elevation_cloud->push_back(elevation_cloud_point);
- }
- }
+ correspondence_indexes.push_back(elevation_cell_vector[i].indexes_of_points_in_cell);
+ //std::cout << "Storing point into elevation cloud!" << std::endl;
+ elevation_cloud->push_back(elevation_cloud_point);
}
-// std::cout << "elevation_cloud generated!!" << std::endl;
+ //std::cout << "elevation_cloud generated!!" << std::endl;
// //std::cout << "total_number_of_points_stored = " << total_number_of_points_stored << std::endl;
// //std::cout << "original number of points = " << pcl_cloud_ptr->points.size() << std::endl;
}
+/*
+ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
+ 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 number_of_rows_and_columns = std::ceil(2.0 * MAX_RANGE / filtering_configuration.elevation_grid_resolution);
+ //std::cout << "number_of_rows_and_columns = " << number_of_rows_and_columns << std::endl;
+
+ kf_based_terrain_analysis_lib::ElevationCloudCell elevation_map[number_of_rows_and_columns][number_of_rows_and_columns];
+ //std::cout << "elevation_map created! " << std::endl;
+
+ int i = 0;
+ //std::cout << "Starting to fill the elevation_map! " << std::endl;
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
+ ++it)
+ {
+ int row = std::round((number_of_rows_and_columns / 2.0) + (it->x / filtering_configuration.elevation_grid_resolution));
+ int col = std::round((number_of_rows_and_columns / 2.0) + (it->y / filtering_configuration.elevation_grid_resolution));
+
+ if(elevation_map[row][col].indexes_of_points_in_cell.empty())
+ {
+ //std::cout << "Pointing to an empty cell, storing current point as lowest, and keeping its index! " << std::endl;
+ //std::cout << "Point number " << i << " row = " << row << " col = " << col << std::endl;
+ elevation_map[row][col].lowest_point_in_cell = *it;
+ elevation_map[row][col].index_of_lowest_point = i;
+ }
+ else
+ {
+ //std::cout << "Pointing to a NON-empty cell! checking z!" << std::endl;
+ if(it->z < elevation_map[row][col].lowest_point_in_cell.z)
+ {
+ //std::cout << "Current point is the lowest, storing it as it is!" << std::endl;
+ elevation_map[row][col].lowest_point_in_cell = *it;
+ elevation_map[row][col].index_of_lowest_point = i;
+ }
+ }
+ //std::cout << "Storing z coordinate and index in the indexes vector" << std::endl;
+ elevation_map[row][col].z_coordinates.push_back(it->z);
+ elevation_map[row][col].indexes_of_points_in_cell.push_back(i);
+ i++;
+ }
+
+ //std::cout << "Now generating the elevation cloud from the elevation map!" << std::endl;
+ //DEBUG
+ //int total_number_of_points_stored = 0;
+ int index_respect_elevation_cloud = 0;
+ for(int row = 0; row < number_of_rows_and_columns; ++row)
+ {
+ for(int col = 0; col < number_of_rows_and_columns; ++col)
+ {
+ if(!elevation_map[row][col].indexes_of_points_in_cell.empty())
+ {
+ pcl::PointXYZRGBNormal elevation_cloud_point = elevation_map[row][col].lowest_point_in_cell;
+ if (elevation_cloud_point.data_c[DATA_C_1_ID_CLASS] == OUTLIER)
+ {
+ elevation_cloud_point.r = R_CLASS_OUTLIER;
+ elevation_cloud_point.g = G_CLASS_OUTLIER;
+ elevation_cloud_point.b = B_CLASS_OUTLIER;
+ }
+ else
+ {
+ elevation_cloud_point.r = R_CLASS_UNKNOWN;
+ elevation_cloud_point.g = G_CLASS_UNKNOWN;
+ elevation_cloud_point.b = B_CLASS_UNKNOWN;
+ }
+ float sum = std::accumulate(elevation_map[row][col].z_coordinates.begin(), elevation_map[row][col].z_coordinates.end(), 0.0);
+ float z_mean = sum / (float)elevation_map[row][col].z_coordinates.size();
+ float var = 0;
+ for (int n = 0; n < elevation_map[row][col].z_coordinates.size(); ++n)
+ var += ((elevation_map[row][col].z_coordinates[n] - z_mean) * (elevation_map[row][col].z_coordinates[n] - z_mean));
+
+ var /= (float)elevation_map[row][col].z_coordinates.size();
+
+ // Filling remaining fields
+ elevation_cloud_point.data_n[DATA_N_0_INTENSITY] = INTENSITY_UNKNOWN;
+ elevation_cloud_point.data_n[DATA_N_1_Z_GROUND] = Z_GROUND_UNKNOWN;
+ elevation_cloud_point.data_n[DATA_N_2_Z_MEAN] = z_mean;
+ elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE] = var;
+
+ elevation_cloud_point.data_c[DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL] = INDEX_UNKNOWN;
+ elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX] = index_respect_elevation_cloud++; //storing the index for later in labelling step
+
+ //std::cout << "var z = " << it->data_n[DATA_N_3_Z_VARIANCE] << std::endl;
+ //std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
+
+ //DEBUG!
+ //total_number_of_points_stored += elevation_map[row][col].indexes_of_points_in_cell.size();
+
+ correspondence_indexes.push_back(elevation_map[row][col].indexes_of_points_in_cell);
+ //std::cout << "Storing point into elevation cloud!" << std::endl;
+ elevation_cloud->push_back(elevation_cloud_point);
+ }
+ }
+ }
+ //std::cout << "elevation_cloud generated!!" << std::endl;
+ // //std::cout << "total_number_of_points_stored = " << total_number_of_points_stored << std::endl;
+ // //std::cout << "original number of points = " << pcl_cloud_ptr->points.size() << std::endl;
+ }*/
+
void CKf_Based_Terrain_Analysis::generateRootVertex(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
@@ -1198,7 +1307,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
{
pcl::PointXYZRGBNormal point_in_sensor_frame = *i;
- // 0) K nearest neighbor search
+// 0) K nearest neighbor search
pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
kdtree.setInputCloud(ground_reference_cloud_ptr);
int K = filtering_configuration.number_of_references_used_for_labelling;
@@ -1535,8 +1644,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("groundSegmentation", performance_supervisor_ptr_);
- //std::cout << "Starting ground segmentation!!" << std::endl;
- //std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
+//std::cout << "Starting ground segmentation!!" << std::endl;
+//std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
*pcl_cloud_ptr = pcl_cloud;
@@ -1548,15 +1657,16 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector < std::vector<int> > correspondence_indexes;
- fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
+ if (filtering_configuration.number_of_references_used_for_labelling > 0)
+ {
+ generateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
+ }
+ else
+ {
+ fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
+ }
//std::cout << "Elevation cloud generated! Npoints = " << elevation_cloud_ptr->points.size() << std::endl;
-// pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr fast_elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-// std::vector < std::vector<int> > fast_correspondence_indexes;
-//
-// fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, fast_elevation_cloud_ptr, fast_correspondence_indexes);
-// std::cout << "Fast Elevation cloud generated! Npoints = " << fast_elevation_cloud_ptr->points.size() << std::endl;
-
generateRootVertex(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr);
//std::cout << "Root vertex generated! Num of points = " << ground_reference_cloud_ptr->points.size() << std::endl;
// << "Number of edges = " << edges[0].size() << std::endl;
@@ -1589,8 +1699,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
//std::cout << "Total number of references = " << ground_reference_cloud_ptr->points.size() << std::endl;
//getchar();
- // We can discard ground references if they are not connected with any other, to test this, we need
- // to be sure that connections are reflected in both ends
+// We can discard ground references if they are not connected with any other, to test this, we need
+// to be sure that connections are reflected in both ends
ensureThatConnectionsAreReflectedInBothEnds(ground_reference_cloud_ptr, edges);
if (filtering_configuration.number_of_references_used_for_labelling > 0)
@@ -1600,7 +1710,7 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
}
else
{
- //std::cout << "Using fast labelling function!" << std::endl;
+//std::cout << "Using fast labelling function!" << std::endl;
fastLabelPointcloudUsingGroundModel(filtering_configuration, elevation_cloud_ptr, correspondence_indexes,
ground_reference_cloud_ptr, edges, pcl_cloud_ptr);
}
@@ -1609,7 +1719,7 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
if (filtering_configuration.show_dense_reconstruction)
{
- //std::cout << "Ready to create dense reconstruction" << std::endl;
+//std::cout << "Ready to create dense reconstruction" << std::endl;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
createDenseGroundCloud(filtering_configuration, ground_reference_cloud_ptr, lidar_configuration,
pcl_dense_cloud_ptr);
--
GitLab