From 7ea32a3b0e68cacc1e4d2e315d4808d340893d87 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Fri, 7 Oct 2022 15:51:00 +0200
Subject: [PATCH] cleaning old functions and removing unused parameters
---
include/kf_based_terrain_analysis.h | 12 -
include/structs_definitions.h | 2 -
src/kf_based_terrain_analysis.cpp | 491 ++--------------------------
3 files changed, 23 insertions(+), 482 deletions(-)
diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index 03c9f65..d388976 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -51,11 +51,6 @@ private:
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr);
- void 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);
-
void 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,
@@ -91,13 +86,6 @@ private:
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr);
- void labelPointcloudUsingGroundModel(
- 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 fastLabelPointcloudUsingGroundModel(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index f853cff..d772605 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -151,12 +151,10 @@ struct FilteringConfiguration
float mahalanobis_threshold;
- int number_of_references_used_for_labelling;
float max_pred_std_dev_for_labelling;
float score_threshold;
bool classify_not_labeled_points_as_obstacles;
- bool discard_not_connected_references_for_labelling;
bool measure_performance;
bool show_dense_reconstruction;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index f705148..6303bd1 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -379,7 +379,6 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// then, we analize the vertices in the ROI and make edges to those that are seen as ground from the currently processed vertex. The CONNECTED
// _TO_ROOT flag is set in those to which we connect (it is unset by default)
-
//std::cout << "Starting labelLocalGroundPoints!!" << std::endl;
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
@@ -696,116 +695,6 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
// getchar();
}
-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_);
-
- pcl::PointCloud<pcl::PointXY>::Ptr pcl_pointcloud_2D_ptr(new pcl::PointCloud<pcl::PointXY>);
- pcl::PointXY point;
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = pcl_cloud_ptr->begin(); it != pcl_cloud_ptr->end();
- ++it)
- {
- point.x = it->x;
- point.y = it->y;
- 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;
-
- 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;
- 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;
-
- const float DISTANCE_FROM_CELL_CENTER_TO_CELL_CORNER = 0.7071067812
- * filtering_configuration.elevation_grid_resolution; //sqrt(2)/2.0
-
- int i = 0;
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it = elevation_cloud->begin(); it != elevation_cloud->end();
- ++it)
- {
- point.x = it->x;
- point.y = it->y;
-
- std::vector<int> pointIdxRadiusSearch;
- std::vector<float> pointRadiusSearchSquaredDistance;
-
- kdtree_2D.radiusSearch(point, DISTANCE_FROM_CELL_CENTER_TO_CELL_CORNER, pointIdxRadiusSearch,
- pointRadiusSearchSquaredDistance);
-
- 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)
- {
- 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)
- {
- 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
- {
- 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;
- }
-
- 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::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];
-
- // Filling remaining fields
- 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_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;
- //std::cout << "z_coordinates.size() = " << z_coordinates.size() << std::endl;
-
- correspondence_indexes.push_back(pointIdxRadiusSearch);
- }
-}
-
void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
@@ -956,7 +845,6 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
// that are not connected with the root (the root position is same of the sensor but at ground level, so not connected to root means the same
// as unreachable for the robot
-
//std::cout << "Generating root vertex" << std::endl;
// The first reference point (root vertex) is the ground in (0,0) approximately known by sensor extrinsic calibration
pcl::PointXYZRGBNormal root_vertex;
@@ -1005,7 +893,7 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
//std::cout << "setting input cloud" << std::endl;
pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
kdtree.setInputCloud(ground_reference_cloud_ptr);
- int K = filtering_configuration.number_of_references_used_for_labelling;
+ int K = 1;
std::vector<int> pointIdxKNNSearch(K);
std::vector<float> pointKNNSquaredDistance(K);
@@ -1086,9 +974,7 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
}
}
}
-
//std::cout << "Dense reconstruction min_std_dev = " << min_std_dev << " max_std_dev = " << max_std_dev << std::endl;
-
//std::cout << "Finished dense cloud creation!!" << std::endl;
}
@@ -1261,332 +1147,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
//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,
- 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;
-
-// 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;
- std::vector<int> pointIdxKNNSearch(K);
- std::vector<float> pointKNNSquaredDistance(K);
-
- int num_of_references_found = kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch,
- pointKNNSquaredDistance);
- int best_reference_index = -1;
- int best_reference_index_predicting_ground = -1;
- int best_reference_index_predicting_obstacle = -1;
-
- float min_ground_pred_std_dev = 10000.0;
- float min_obstacle_pred_std_dev = 10000.0;
- float best_pred_std_dev = 10000.0;
-
- for (int reference_iterator = 0; reference_iterator < num_of_references_found; ++reference_iterator)
- {
- pcl::PointXYZRGBNormal reference_in_sensor_frame =
- ground_reference_cloud_ptr->points[pointIdxKNNSearch[reference_iterator]];
-
- if (!filtering_configuration.discard_not_connected_references_for_labelling
- || (reference_in_sensor_frame.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT
- && edges[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]].size() > 0))
- {
- bool observation_z_is_below_prediction = false;
- float prediction_std_dev = -1.0;
- float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame, point_in_sensor_frame,
- prediction_std_dev, observation_z_is_below_prediction);
-
- // if we have enough confidence to label something
- if (prediction_std_dev < filtering_configuration.max_pred_std_dev_for_labelling)
- {
- // if we think the point could be ground
- if (mahalanobis_distance < filtering_configuration.mahalanobis_threshold)
- {
- if (prediction_std_dev < min_ground_pred_std_dev)
- {
- min_ground_pred_std_dev = prediction_std_dev;
- best_reference_index_predicting_ground = reference_iterator;
- }
- }
- else
- {
- if (prediction_std_dev < min_obstacle_pred_std_dev)
- {
- min_obstacle_pred_std_dev = prediction_std_dev;
- best_reference_index_predicting_obstacle = reference_iterator;
- }
- }
- }
- }
- }
-
- bool elevation_cloud_point_is_ground = false;
- bool elevation_cloud_point_is_not_predictable_using_the_model = false;
- if (best_reference_index_predicting_ground != -1.0)
- {
- best_reference_index = best_reference_index_predicting_ground;
- best_pred_std_dev = min_ground_pred_std_dev;
- elevation_cloud_point_is_ground = true;
- }
- else if (best_reference_index_predicting_obstacle != -1.0)
- {
- best_reference_index = best_reference_index_predicting_obstacle;
- best_pred_std_dev = min_obstacle_pred_std_dev;
- elevation_cloud_point_is_ground = false;
- }
- else
- {
- 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[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!!!
- {
- //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
- i->data_n[DATA_N_1_Z_GROUND] = i->z;
-
- // set the class as ground
- float score = 0.99 - (best_pred_std_dev / filtering_configuration.max_pred_std_dev_for_labelling);
- if (score < 0.0)
- score = 0.0;
- i->data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
-
- // colour the point properly
- i->r = R_CLASS_GROUND;
- i->g = G_CLASS_GROUND;
- i->b = B_CLASS_GROUND;
-
- // 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!
- }
- }
- }
- if (flag_not_overhanging_obstacle_found)
- {
- i->r = R_CLASS_OBSTACLE;
- i->g = G_CLASS_OBSTACLE;
- i->b = B_CLASS_OBSTACLE;
- }
- }
- else // If the lowest point in cell is already an obstacle, the remaining points are also obstacles
- {
- // if it is an obstacle point we use the ground reference z prediction as z ground
- float z_ground = predictZ(reference_in_sensor_frame, point_in_sensor_frame.x, point_in_sensor_frame.y);
-
- // If the point from the elevation cloud is an obstacle below the prediction
- // we assume that the ground is at the level of the elevation cloud point
- if (point_in_sensor_frame.z < z_ground)
- z_ground = point_in_sensor_frame.z;
-
- i->data_n[DATA_N_1_Z_GROUND] = z_ground;
-
- // set the class to OBSTACLE
- i->data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.99;
-
- // and colour the point
- i->r = R_CLASS_OBSTACLE;
- i->g = G_CLASS_OBSTACLE;
- i->b = B_CLASS_OBSTACLE;
-
- //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)
- 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::ensureThatConnectionsAreReflectedInBothEnds(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr, std::vector<std::vector<int>> edges)
{
@@ -1617,37 +1177,40 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
//std::cout << "Starting ground segmentation!!" << std::endl;
//std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
+ // We copy the input in a pointer fashion (maybe it could be optimized)
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
*pcl_cloud_ptr = pcl_cloud;
-// We create a pointcloud that will contain only trusted subsampled ground points, these points will be used
-// as references to compute local planes to label near points
- pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-
+// We create a new representation for the input cloud: Using a grid, we extract the lowest point of each cell
+// as representative point of its area. Then we extract some statistics and using an indexes vector
+// we keep track of all the points represented by each lowest point, so the original point cloud can be recovered
+// at any time. The lowest points (those in elevation cloud) will be classified as being ground or obstacles using our
+// prior knowledge and the ground points will be used to update the prior in a Kalman Filter fashion
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector < std::vector<int> > 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);
- }
+// The parameters estimated using the Kalman filter are the slopes and a z coordinate defining small local planes.
+// Those planes along with their connectivities (if a plane sees other planes as ground are considered connected, otherwise
+// (a plane sees other plane as obstacle) they are considered not-connected) are stored in a graph structure
+// (ground_reference_cloud_ptr contains the vertices and the vector of int vectors named edges contains the edgesof the graph)
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+// This is an optimized version of a previous function (thats why the "fast" is appended at the begginning of the name),
+// but it became the standard so the previous one was deleted for sake of code clarity. This function generates the
+// representation using the grid to keep only the lowest points in each cell and a vector with indexes to the rest of points
+ fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
//std::cout << "Elevation cloud generated! Npoints = " << elevation_cloud_ptr->points.size() << std::endl;
+ // This function juit create the first vertex in the graph, using the prior information in the lidar and filtering configuration
+ // structures.
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;
-//getchar();
-
// We run the algorithm, starting from the reference at sensor origin it will try to find others trusty local
// ground references, which will increase the value of ground_reference_cloud->points.size(), evaluating
-// local planes using these reference points, once no new references appear. The algorithm ends after evaluating
+// local planes using these reference points, until no new references appear. The algorithm ends after evaluating
// all of them
-
int reference_index = 0;
while (reference_index < ground_reference_cloud_ptr->points.size())
{
@@ -1674,17 +1237,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// 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)
- {
- 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);
- }
+ 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
@@ -1699,6 +1253,7 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
}
else
{
+ // TODO: Make this interface more clear
ground_dense_reconstruction_pcl_cloud.clear();
ground_dense_reconstruction_pcl_cloud = *elevation_cloud_ptr;
}
--
GitLab