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

improved fast labelling, now achieving even better results than with the slow method!

parent 51a4257e
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
+ 48
− 157
View file @ 800eb229
......@@ -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,
......
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