Skip to content
Snippets Groups Projects
Commit 6f1695f9 authored by Iván del Pino's avatar Iván del Pino
Browse files

improved optimized elevation cloud generation to avoid running out of memory

parent b4d5e09c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/kf_based_terrain_analysis.cpp
+ 176
− 66
View file @ 6f1695f9
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment