From 729eec036a11754945dc5752e3c00139ef6c4a8e Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 9 May 2024 20:50:14 +0900 Subject: [PATCH] fix(ground_segmentation): add intensity field (#6791) * fix(grond_segmentation): add intensity Signed-off-by: badai-nguyen * fix(ray_ground_filter): change to pointXYZI Signed-off-by: badai-nguyen * fix(ransac_ground_filter): change to pointXYZI Signed-off-by: badai-nguyen * fix(scan_ground_filter): intensity bug fix Signed-off-by: badai-nguyen * fix(ground_segmentation): add field select option Signed-off-by: badai-nguyen * Revert "fix(ground_segmentation): add field select option" This reverts commit 36fc5541ab80270b9e6d5575a707801ee38e32e1. * fix: copy input field to output Signed-off-by: badai-nguyen * fix(ray_ground_filter): copy input fields Signed-off-by: badai-nguyen * fix(ransac_ground_filter): copy input fields Signed-off-by: badai-nguyen * fix(ray_ground_filter): indices extract bug fix Signed-off-by: badai-nguyen * refactor: ray_ground_filter Signed-off-by: badai-nguyen * fix: indices max Signed-off-by: badai-nguyen * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ray_ground_filter_nodelet.hpp | 9 +-- .../src/ransac_ground_filter_nodelet.cpp | 33 +++++++--- .../src/ray_ground_filter_nodelet.cpp | 64 ++++++++++++++----- .../src/scan_ground_filter_nodelet.cpp | 4 +- 4 files changed, 78 insertions(+), 32 deletions(-) diff --git a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp index 069c6ea27b1fa..6e2638e8566d8 100644 --- a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp @@ -186,15 +186,16 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter * @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed */ void ExtractPointsIndices( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_only_indices_cloud_ptr, - pcl::PointCloud::Ptr out_removed_indices_cloud_ptr); + const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices, + PointCloud2::SharedPtr out_only_indices_cloud_ptr, + PointCloud2::SharedPtr out_removed_indices_cloud_ptr); boost::optional calcPointVehicleIntersection(const Point & point); void setVehicleFootprint( const double min_x, const double max_x, const double min_y, const double max_y); - + void initializePointCloud2( + const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ed5fb6abe7fe7..587db9c9e0dac 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -284,19 +284,36 @@ void RANSACGroundFilterComponent::filter( const Eigen::Affine3d plane_affine = getPlaneAffine(*segment_ground_cloud_ptr, plane_normal); pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); + int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; + int y_offset = input->fields[pcl::getFieldIndex(*input, "y")].offset; + int z_offset = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int point_step = input->point_step; + + sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr( + new sensor_msgs::msg::PointCloud2); + no_ground_cloud_msg_ptr->header = input->header; + no_ground_cloud_msg_ptr->fields = input->fields; + no_ground_cloud_msg_ptr->point_step = point_step; + size_t output_size = 0; // use not downsampled pointcloud for extract pointcloud that higher than height threshold - for (const auto & p : current_sensor_cloud_ptr->points) { - const Eigen::Vector3d transformed_point = - plane_affine.inverse() * Eigen::Vector3d(p.x, p.y, p.z); + for (size_t global_size = 0; global_size < input->data.size(); global_size += point_step) { + float x = *reinterpret_cast(input->data[global_size + x_offset]); + float y = *reinterpret_cast(input->data[global_size + y_offset]); + float z = *reinterpret_cast(input->data[global_size + z_offset]); + const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z); if (std::abs(transformed_point.z()) > height_threshold_) { - no_ground_cloud_ptr->points.push_back(p); + std::memcpy( + &no_ground_cloud_msg_ptr->data[output_size], &input->data[global_size], point_step); + output_size += point_step; } } + no_ground_cloud_msg_ptr->data.resize(output_size); + no_ground_cloud_msg_ptr->height = input->height; + no_ground_cloud_msg_ptr->width = output_size / point_step / input->height; + no_ground_cloud_msg_ptr->row_step = output_size / input->height; + no_ground_cloud_msg_ptr->is_dense = input->is_dense; + no_ground_cloud_msg_ptr->is_bigendian = input->is_bigendian; - sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr( - new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - no_ground_cloud_msg_ptr->header = input->header; sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_transformed_msg_ptr( new sensor_msgs::msg::PointCloud2); if (!transformPointCloud( diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 7bcae47fd9f1f..c58f1c0e507e5 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -257,20 +257,53 @@ void RayGroundFilterComponent::ClassifyPointCloud( // return (true); // } -void RayGroundFilterComponent::ExtractPointsIndices( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_only_indices_cloud_ptr, - pcl::PointCloud::Ptr out_removed_indices_cloud_ptr) +void RayGroundFilterComponent::initializePointCloud2( + const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr) { - pcl::ExtractIndices extract_ground; - extract_ground.setInputCloud(in_cloud_ptr); - extract_ground.setIndices(pcl::make_shared(in_indices)); - - extract_ground.setNegative(false); // true removes the indices, false leaves only the indices - extract_ground.filter(*out_only_indices_cloud_ptr); + out_cloud_msg_ptr->header = in_cloud_ptr->header; + out_cloud_msg_ptr->height = in_cloud_ptr->height; + out_cloud_msg_ptr->fields = in_cloud_ptr->fields; + out_cloud_msg_ptr->is_bigendian = in_cloud_ptr->is_bigendian; + out_cloud_msg_ptr->point_step = in_cloud_ptr->point_step; + out_cloud_msg_ptr->is_dense = in_cloud_ptr->is_dense; + out_cloud_msg_ptr->data.resize(in_cloud_ptr->data.size()); +} - extract_ground.setNegative(true); // true removes the indices, false leaves only the indices - extract_ground.filter(*out_removed_indices_cloud_ptr); +void RayGroundFilterComponent::ExtractPointsIndices( + const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices, + PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr) +{ + initializePointCloud2(in_cloud_ptr, ground_cloud_msg_ptr); + initializePointCloud2(in_cloud_ptr, no_ground_cloud_msg_ptr); + int point_step = in_cloud_ptr->point_step; + size_t ground_count = 0; + size_t no_ground_count = 0; + std::vector is_ground_idx(in_cloud_ptr->width, false); + for (const auto & idx : in_indices.indices) { + if (std::size_t(idx) >= is_ground_idx.size()) { + continue; + } + is_ground_idx[idx] = true; + } + for (size_t i = 0; i < is_ground_idx.size(); ++i) { + if (is_ground_idx[i]) { + std::memcpy( + &ground_cloud_msg_ptr->data[ground_count * point_step], &in_cloud_ptr->data[i * point_step], + point_step); + ground_count++; + } else { + std::memcpy( + &no_ground_cloud_msg_ptr->data[no_ground_count * point_step], + &in_cloud_ptr->data[i * point_step], point_step); + no_ground_count++; + } + } + ground_cloud_msg_ptr->data.resize(ground_count * point_step); + no_ground_cloud_msg_ptr->data.resize(no_ground_count * point_step); + ground_cloud_msg_ptr->width = ground_count; + no_ground_cloud_msg_ptr->width = no_ground_count; + ground_cloud_msg_ptr->row_step = ground_count * point_step; + no_ground_cloud_msg_ptr->row_step = no_ground_count * point_step; } void RayGroundFilterComponent::filter( @@ -299,14 +332,11 @@ void RayGroundFilterComponent::filter( pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); pcl::PointCloud::Ptr radials_cloud_ptr(new pcl::PointCloud); - ExtractPointsIndices( - current_sensor_cloud_ptr, ground_indices, ground_cloud_ptr, no_ground_cloud_ptr); - sensor_msgs::msg::PointCloud2::SharedPtr no_ground_cloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - no_ground_cloud_msg_ptr->header = input->header; + sensor_msgs::msg::PointCloud2::SharedPtr ground_cloud_msg_ptr(new sensor_msgs::msg::PointCloud2); + ExtractPointsIndices(input, ground_indices, ground_cloud_msg_ptr, no_ground_cloud_msg_ptr); output = *no_ground_cloud_msg_ptr; } diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 7d7e11ee03bdb..6399726740041 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -575,8 +575,6 @@ void ScanGroundFilterComponent::extractObjectPoints( std::memcpy( &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], in_cloud_ptr->point_step * sizeof(uint8_t)); - *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = - 1; // set intensity to 1 output_data_size += in_cloud_ptr->point_step; } } @@ -606,7 +604,7 @@ void ScanGroundFilterComponent::faster_filter( output.row_step = no_ground_indices.indices.size() * input->point_step; output.data.resize(output.row_step); output.width = no_ground_indices.indices.size(); - output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.fields = input->fields; output.is_dense = true; output.height = input->height; output.is_bigendian = input->is_bigendian;