From 7369e2ab06443d48d33175d80ddeb96a1d9beda0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Fri, 13 May 2022 14:27:39 +0200
Subject: [PATCH] first full implementation achieved!
---
src/ground_segmentation_alg_node.cpp | 208 +++++++++++++++++++++++----
1 file changed, 182 insertions(+), 26 deletions(-)
diff --git a/src/ground_segmentation_alg_node.cpp b/src/ground_segmentation_alg_node.cpp
index 4d0f2df..db07423 100644
--- a/src/ground_segmentation_alg_node.cpp
+++ b/src/ground_segmentation_alg_node.cpp
@@ -473,8 +473,8 @@ void transformPointCloud(const Eigen::Matrix<float, 6, 1> pose,
}
void transformInversePointCloud(const Eigen::Matrix<float, 6, 1> pose,
- const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud,
- pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud)
+ const pcl::PointCloud<pcl::PointXYZRGBNormal> input_pcl_cloud,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &output_pcl_cloud)
{
Eigen::Matrix<float, 4, 4> rigid_transform = Eigen::Matrix<float, 4, 4>::Identity();
@@ -508,24 +508,26 @@ void transformInversePointCloud(const Eigen::Matrix<float, 6, 1> pose,
pcl::transformPointCloud(input_pcl_cloud, output_pcl_cloud, rigid_transform);
}
-void discardPointsOutOfROI(const Eigen::Matrix<float, 6, 1> pose, const float ground_grid_size, pcl::PointCloud<pcl::PointXYZRGBNormal> &point_cloud_in_pose_frame)
+void discardPointsOutOfROI(const Eigen::Matrix<float, 6, 1> pose, const float ground_grid_size,
+ pcl::PointCloud<pcl::PointXYZRGBNormal> &point_cloud_in_pose_frame)
{
- pcl::PointCloud<pcl::PointXYZRGBNormal> point_cloud_in_local_frame;
+ pcl::PointCloud < pcl::PointXYZRGBNormal > point_cloud_in_local_frame;
transformInversePointCloud(pose, point_cloud_in_pose_frame, point_cloud_in_local_frame);
- pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr point_cloud_in_local_frame_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr point_cloud_in_local_frame_ptr(
+ new pcl::PointCloud<pcl::PointXYZRGBNormal>);
*point_cloud_in_local_frame_ptr = point_cloud_in_local_frame;
- pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
- pass.setInputCloud (point_cloud_in_local_frame_ptr);
- pass.setFilterFieldName ("x");
- pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size);
- pass.filter (*point_cloud_in_local_frame_ptr);
+ pcl::PassThrough < pcl::PointXYZRGBNormal > pass;
+ pass.setInputCloud(point_cloud_in_local_frame_ptr);
+ pass.setFilterFieldName("x");
+ pass.setFilterLimits(-1.0 * ground_grid_size, ground_grid_size);
+ pass.filter(*point_cloud_in_local_frame_ptr);
- pass.setInputCloud (point_cloud_in_local_frame_ptr);
- pass.setFilterFieldName ("y");
- pass.setFilterLimits (-1.0*ground_grid_size, ground_grid_size);
- pass.filter (*point_cloud_in_local_frame_ptr);
+ pass.setInputCloud(point_cloud_in_local_frame_ptr);
+ pass.setFilterFieldName("y");
+ pass.setFilterLimits(-1.0 * ground_grid_size, ground_grid_size);
+ pass.filter(*point_cloud_in_local_frame_ptr);
point_cloud_in_local_frame.clear();
point_cloud_in_local_frame = *point_cloud_in_local_frame_ptr;
@@ -533,14 +535,169 @@ void discardPointsOutOfROI(const Eigen::Matrix<float, 6, 1> pose, const float gr
transformPointCloud(pose, point_cloud_in_local_frame, point_cloud_in_pose_frame);
}
+void keepOnlyHigherPoints(pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_points_accumulation_in_odom_frame,
+ const float ground_grid_resolution)
+{
+ 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 = ground_points_accumulation_in_odom_frame.begin();
+ it != ground_points_accumulation_in_odom_frame.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::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_points_accumulation_in_odom_frame_ptr(
+ new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ *ground_points_accumulation_in_odom_frame_ptr = ground_points_accumulation_in_odom_frame;
+
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+ pcl::VoxelGrid < pcl::PointXYZRGBNormal > voxel_grid;
+ voxel_grid.setInputCloud(ground_points_accumulation_in_odom_frame_ptr);
+ voxel_grid.setLeafSize(ground_grid_resolution, ground_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 * ground_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;
+
+ for (std::vector<int>::const_iterator iterator = pointIdxRadiusSearch.begin();
+ iterator != pointIdxRadiusSearch.end(); ++iterator)
+ {
+ z_coordinates.push_back(ground_points_accumulation_in_odom_frame_ptr->points[*iterator].z);
+ x_coordinates.push_back(ground_points_accumulation_in_odom_frame_ptr->points[*iterator].x);
+ y_coordinates.push_back(ground_points_accumulation_in_odom_frame_ptr->points[*iterator].y);
+ }
+
+ 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 highest_point_in_cell = std::max_element(z_coordinates.begin(), z_coordinates.end());
+ int position_in_vector = std::distance(z_coordinates.begin(), highest_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_CAR_PROB] = PROB_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;
+ }
+
+ ground_points_accumulation_in_odom_frame.clear();
+ ground_points_accumulation_in_odom_frame = *elevation_cloud;
+}
+
+void labelLowObstacles(pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_points_accumulation_in_odom_frame,
+ const float number_of_neighbours_to_compare_height, const float slope_threshold)
+{
+ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_points_accumulation_in_odom_frame_ptr(
+ new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+ *ground_points_accumulation_in_odom_frame_ptr = ground_points_accumulation_in_odom_frame;
+
+ pcl::KdTreeFLANN < pcl::PointXYZRGBNormal > kdtree;
+ kdtree.setInputCloud(ground_points_accumulation_in_odom_frame_ptr);
+ int K = number_of_neighbours_to_compare_height;
+ std::vector<int> pointIdxKNNSearch(K);
+ std::vector<float> pointKNNSquaredDistance(K);
+
+ for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator it = ground_points_accumulation_in_odom_frame_ptr->begin();
+ it != ground_points_accumulation_in_odom_frame_ptr->end(); ++it)
+ {
+ kdtree.nearestKSearch(*it, K, pointIdxKNNSearch, pointKNNSquaredDistance);
+ const int NUM_OF_NEIGHBOURS = pointIdxKNNSearch.size();
+
+ bool low_obstacle_found = false;
+
+ if (NUM_OF_NEIGHBOURS > 0)
+ {
+ for (int i = 0; i < NUM_OF_NEIGHBOURS; i++)
+ {
+ pcl::PointXYZRGBNormal neighbour = ground_points_accumulation_in_odom_frame_ptr->points[pointIdxKNNSearch[i]];
+ float delta_x = it->x - neighbour.x;
+ float delta_y = it->y - neighbour.y;
+ float delta_z = it->z - neighbour.z;
+
+ float distance_2d = sqrt(delta_x*delta_x + delta_y*delta_y);
+ float slope = delta_z / distance_2d;
+
+ if (slope > slope_threshold)
+ low_obstacle_found = true;
+ }
+
+ if(low_obstacle_found)
+ {
+ //std::cout << "low obstacle found!!" << std::endl;
+ it->data_c[DATA_C_1_ID_CLASS] = CLASS_OBSTACLE;
+ it->r = R_CLASS_OBSTACLE;
+ it->g = G_CLASS_OBSTACLE;
+ it->b = B_CLASS_OBSTACLE;
+ }
+ else
+ {
+ //std::cout << "just another boring ground point :(" << std::endl;
+ it->data_c[DATA_C_1_ID_CLASS] = CLASS_GROUND;
+ it->r = R_CLASS_GROUND;
+ it->g = G_CLASS_GROUND;
+ it->b = B_CLASS_GROUND;
+ }
+ }
+ }
+ ground_points_accumulation_in_odom_frame.clear();
+ ground_points_accumulation_in_odom_frame = *ground_points_accumulation_in_odom_frame_ptr;
+}
+
void GroundSegmentationAlgNode::detectLowObstacles(pcl::PointCloud<pcl::PointXYZRGBNormal> &lidar_pcl_cloud)
{
- pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points;
+ pcl::PointCloud < pcl::PointXYZRGBNormal > ground_points;
for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud.begin(); i != lidar_pcl_cloud.end(); ++i)
- if((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
+ if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
ground_points.push_back(*i);
- pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_in_odom_frame;
+ pcl::PointCloud < pcl::PointXYZRGBNormal > ground_points_in_odom_frame;
transformPointCloud(X_k_, ground_points, ground_points_in_odom_frame);
@@ -548,18 +705,15 @@ void GroundSegmentationAlgNode::detectLowObstacles(pcl::PointCloud<pcl::PointXYZ
discardPointsOutOfROI(X_k_, ground_grid_size_, ground_points_accumulation_in_odom_frame_);
- pcl::PointCloud<pcl::PointXYZRGBNormal> ground_points_accumulation;
- transformInversePointCloud(X_k_, ground_points_accumulation_in_odom_frame_, ground_points_accumulation);
+ keepOnlyHigherPoints(ground_points_accumulation_in_odom_frame_, ground_grid_resolution_);
- for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = ground_points_accumulation.begin(); i != ground_points_accumulation.end(); ++i)
- {
- i->r = R_CLASS_GROUND;
- i->g = G_CLASS_GROUND;
- i->b = B_CLASS_GROUND;
+ labelLowObstacles(ground_points_accumulation_in_odom_frame_, number_of_neighbours_to_compare_height_,
+ slope_threshold_);
- lidar_pcl_cloud.push_back(*i);
- }
+ pcl::PointCloud < pcl::PointXYZRGBNormal > ground_points_accumulation;
+ transformInversePointCloud(X_k_, ground_points_accumulation_in_odom_frame_, ground_points_accumulation);
+ lidar_pcl_cloud += ground_points_accumulation;
}
void GroundSegmentationAlgNode::mainNodeThread(void)
@@ -691,6 +845,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
// low obstacle detection parameters (requires a good 3d odometry source)
this->detect_low_obstacles_using_odometry_ = config.detect_low_obstacles_using_odometry;
this->ground_grid_resolution_ = config.ground_grid_resolution;
+ this->ground_grid_size_ = config.ground_grid_size;
this->number_of_neighbours_to_compare_height_ = config.number_of_neighbours_to_compare_height;
this->slope_threshold_ = config.slope_threshold;
@@ -740,6 +895,7 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
std::cout << "detect_low_obstacles_using_odometry = " << this->detect_low_obstacles_using_odometry_ << std::endl;
std::cout << "ground_grid_resolution = " << this->ground_grid_resolution_ << std::endl;
+ std::cout << "ground_grid_size = " << this->ground_grid_size_ << std::endl;
std::cout << "number_of_neighbours_to_compare_height = " << this->number_of_neighbours_to_compare_height_
<< std::endl;
std::cout << "slope_threshold = " << this->slope_threshold_ << std::endl;
--
GitLab