From a381af87ad8528ffce0aa441413de00a64736c5b Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 11 Apr 2024 20:45:56 +0900
Subject: [PATCH 01/13] fix(grond_segmentation): add intensity
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
index 34573b564fa36..e03e9dcefb323 100644
--- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -612,7 +612,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.assign(input->fields.begin(), input->fields.begin() + 4);
output.is_dense = true;
output.height = input->height;
output.is_bigendian = input->is_bigendian;
From 9f35fce9782c461bb6fb280fc7c35f5895afeb97 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Sat, 13 Apr 2024 01:37:08 +0900
Subject: [PATCH 02/13] fix(ray_ground_filter): change to pointXYZI
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../include/ground_segmentation/ray_ground_filter_nodelet.hpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
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..d77c56d5f376a 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
@@ -84,7 +84,7 @@ namespace ground_segmentation
{
class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
{
- typedef pcl::PointXYZ PointType_;
+ typedef pcl::PointXYZI PointType_;
struct PointXYZRTColor
{
From 34da1b3706b51b539d3cda5cd1207ac18fd2eeac Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Sat, 13 Apr 2024 01:41:59 +0900
Subject: [PATCH 03/13] fix(ransac_ground_filter): change to pointXYZI
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/ransac_ground_filter_nodelet.hpp | 2 +-
.../ground_segmentation/src/ransac_ground_filter_nodelet.cpp | 4 ++--
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
index f2fc47a50ff62..c2d3646dc2e8d 100644
--- a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
+++ b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
@@ -56,7 +56,7 @@ struct RGB
class RANSACGroundFilterComponent : public pointcloud_preprocessor::Filter
{
- using PointType = pcl::PointXYZ;
+ using PointType = pcl::PointXYZI;
protected:
void filter(
diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
index ed5fb6abe7fe7..6ad606ee5efbe 100644
--- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
@@ -198,11 +198,11 @@ void RANSACGroundFilterComponent::extractPointsIndices(
Eigen::Affine3d RANSACGroundFilterComponent::getPlaneAffine(
const pcl::PointCloud<PointType> segment_ground_cloud, const Eigen::Vector3d & plane_normal)
{
- pcl::CentroidPoint<pcl::PointXYZ> centroid;
+ pcl::CentroidPoint<PointType> centroid;
for (const auto p : segment_ground_cloud.points) {
centroid.add(p);
}
- pcl::PointXYZ centroid_point;
+ PointType centroid_point;
centroid.get(centroid_point);
Eigen::Translation<double, 3> trans(centroid_point.x, centroid_point.y, centroid_point.z);
const ground_segmentation::PlaneBasis basis = getPlaneBasis(plane_normal);
From ebc1283957d036ad9b31440d7ee5ebf4a9c3c777 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Mon, 15 Apr 2024 00:16:26 +0900
Subject: [PATCH 04/13] fix(scan_ground_filter): intensity bug fix
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 2 --
1 file changed, 2 deletions(-)
diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
index e03e9dcefb323..8505a4c145976 100644
--- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -581,8 +581,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<float *>(&out_object_cloud.data[output_data_size + intensity_offset_]) =
- 1; // set intensity to 1
output_data_size += in_cloud_ptr->point_step;
}
}
From 36fc5541ab80270b9e6d5575a707801ee38e32e1 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Tue, 16 Apr 2024 22:14:14 +0900
Subject: [PATCH 05/13] fix(ground_segmentation): add field select option
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/ground_segmentation.launch.py | 7 ++++++-
.../ground_segmentation/scan_ground_filter_nodelet.hpp | 1 +
.../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 7 ++++++-
3 files changed, 13 insertions(+), 2 deletions(-)
diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index f502aef017979..1b0e38c46d71c 100644
--- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -117,7 +117,10 @@ def create_additional_pipeline(self, lidar_name):
("output", f"{lidar_name}/pointcloud"),
],
parameters=[
- self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"]
+ self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"],
+ {"input_frame": "base_link"},
+ {"output_frame": "base_link"},
+ {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -267,6 +270,7 @@ def create_common_pipeline(self, input_topic, output_topic):
self.vehicle_info,
{"input_frame": "base_link"},
{"output_frame": "base_link"},
+ {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -545,6 +549,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("use_intra_process", "True")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")
+ add_launch_arg("output_intensity_field", "False")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
index d97bbaa2118e5..034f9a226d5b6 100644
--- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
+++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
@@ -196,6 +196,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster,
// otherwise select middle point
+ int output_field_num_{3};
size_t radial_dividers_num_;
VehicleInfo vehicle_info_;
diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
index 8505a4c145976..9972b5a068e7e 100644
--- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -58,6 +58,11 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
+ if (declare_parameter<bool>("output_intensity_field")) {
+ output_field_num_ = 4;
+ } else {
+ output_field_num_ = 3;
+ }
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
@@ -610,7 +615,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() + 4);
+ output.fields.assign(input->fields.begin(), input->fields.begin() + output_field_num_);
output.is_dense = true;
output.height = input->height;
output.is_bigendian = input->is_bigendian;
From 13c76f1f07bb0d071c4a67caba3b04a55ec1c42f Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 18 Apr 2024 10:25:16 +0900
Subject: [PATCH 06/13] Revert "fix(ground_segmentation): add field select
option"
This reverts commit 36fc5541ab80270b9e6d5575a707801ee38e32e1.
---
.../ground_segmentation/ground_segmentation.launch.py | 7 +------
.../ground_segmentation/scan_ground_filter_nodelet.hpp | 1 -
.../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 7 +------
3 files changed, 2 insertions(+), 13 deletions(-)
diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index 1b0e38c46d71c..f502aef017979 100644
--- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -117,10 +117,7 @@ def create_additional_pipeline(self, lidar_name):
("output", f"{lidar_name}/pointcloud"),
],
parameters=[
- self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"],
- {"input_frame": "base_link"},
- {"output_frame": "base_link"},
- {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
+ self.ground_segmentation_param[f"{lidar_name}_ground_filter"]["parameters"]
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -270,7 +267,6 @@ def create_common_pipeline(self, input_topic, output_topic):
self.vehicle_info,
{"input_frame": "base_link"},
{"output_frame": "base_link"},
- {"output_intensity_field": LaunchConfiguration("output_intensity_field")},
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
@@ -549,7 +545,6 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("use_intra_process", "True")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")
- add_launch_arg("output_intensity_field", "False")
set_container_executable = SetLaunchConfiguration(
"container_executable",
diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
index 034f9a226d5b6..d97bbaa2118e5 100644
--- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
+++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp
@@ -196,7 +196,6 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster,
// otherwise select middle point
- int output_field_num_{3};
size_t radial_dividers_num_;
VehicleInfo vehicle_info_;
diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
index 9972b5a068e7e..8505a4c145976 100644
--- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -58,11 +58,6 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
use_lowest_point_ = declare_parameter<bool>("use_lowest_point");
- if (declare_parameter<bool>("output_intensity_field")) {
- output_field_num_ = 4;
- } else {
- output_field_num_ = 3;
- }
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
@@ -615,7 +610,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() + output_field_num_);
+ output.fields.assign(input->fields.begin(), input->fields.begin() + 4);
output.is_dense = true;
output.height = input->height;
output.is_bigendian = input->is_bigendian;
From 0ee96121570bb002b19b1a6dfda2bb4e3b1ff81d Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 18 Apr 2024 11:08:28 +0900
Subject: [PATCH 07/13] fix: copy input field to output
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/src/scan_ground_filter_nodelet.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
index 8505a4c145976..2bc62f406ceef 100644
--- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
@@ -610,7 +610,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() + 4);
+ output.fields = input->fields;
output.is_dense = true;
output.height = input->height;
output.is_bigendian = input->is_bigendian;
From 0fd66e6d5320edd8133bfb01bb3a55a5a5cc61c6 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 26 Apr 2024 10:55:55 +0900
Subject: [PATCH 08/13] fix(ray_ground_filter): copy input fields
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ray_ground_filter_nodelet.hpp | 11 ++--
.../src/ray_ground_filter_nodelet.cpp | 56 ++++++++++++++-----
2 files changed, 47 insertions(+), 20 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 d77c56d5f376a..74877255f1077 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
@@ -84,7 +84,7 @@ namespace ground_segmentation
{
class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
{
- typedef pcl::PointXYZI PointType_;
+ typedef pcl::PointXYZ PointType_;
struct PointXYZRTColor
{
@@ -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<PointType_>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
- pcl::PointCloud<PointType_>::Ptr out_only_indices_cloud_ptr,
- pcl::PointCloud<PointType_>::Ptr out_removed_indices_cloud_ptr);
+ const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices,
+ PointCloud2::SharedPtr out_only_indices_cloud_ptr,
+ PointCloud2::SharedPtr out_removed_indices_cloud_ptr);
boost::optional<float> 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/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
index 7bcae47fd9f1f..9221e8737422a 100644
--- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
@@ -257,20 +257,45 @@ void RayGroundFilterComponent::ClassifyPointCloud(
// return (true);
// }
-void RayGroundFilterComponent::ExtractPointsIndices(
- const pcl::PointCloud<PointType_>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
- pcl::PointCloud<PointType_>::Ptr out_only_indices_cloud_ptr,
- pcl::PointCloud<PointType_>::Ptr out_removed_indices_cloud_ptr)
+void RayGroundFilterComponent::initializePointCloud2(
+ const PointCloud2::ConstSharedPtr & in_cloud_ptr, PointCloud2::SharedPtr & out_cloud_msg_ptr)
{
- pcl::ExtractIndices<PointType_> extract_ground;
- extract_ground.setInputCloud(in_cloud_ptr);
- extract_ground.setIndices(pcl::make_shared<pcl::PointIndices>(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, const 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);
+ size_t ground_count = 0;
+ int point_step = in_cloud_ptr->point_step;
+ size_t prev_ground_count = 0;
+ size_t no_ground_count = 0;
+ for (const auto & idx : in_indices.indices) {
+ std::memcpy(
+ &ground_cloud_msg_ptr->data[ground_count * point_step], &in_cloud_ptr->data[idx * point_step],
+ point_step);
+ std::memcpy(
+ &no_ground_cloud_msg_ptr->data[prev_ground_count * point_step],
+ &in_cloud_ptr->data[idx * point_step], point_step * (idx - prev_ground_count));
+ prev_ground_count = idx - prev_ground_count;
+ ground_count++;
+ prev_ground_count = idx;
+ }
+ 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,11 +324,12 @@ void RayGroundFilterComponent::filter(
pcl::PointCloud<PointType_>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<PointType_>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr radials_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
- 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);
+ 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);
+
pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);
no_ground_cloud_msg_ptr->header = input->header;
From 3414c05a0687b7f54f4722d2b72284bfa6f9d2b5 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Fri, 26 Apr 2024 10:56:10 +0900
Subject: [PATCH 09/13] fix(ransac_ground_filter): copy input fields
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ransac_ground_filter_nodelet.hpp | 2 +-
.../src/ransac_ground_filter_nodelet.cpp | 37 ++++++++++++++-----
2 files changed, 28 insertions(+), 11 deletions(-)
diff --git a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
index c2d3646dc2e8d..f2fc47a50ff62 100644
--- a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
+++ b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp
@@ -56,7 +56,7 @@ struct RGB
class RANSACGroundFilterComponent : public pointcloud_preprocessor::Filter
{
- using PointType = pcl::PointXYZI;
+ using PointType = pcl::PointXYZ;
protected:
void filter(
diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
index 6ad606ee5efbe..587db9c9e0dac 100644
--- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
@@ -198,11 +198,11 @@ void RANSACGroundFilterComponent::extractPointsIndices(
Eigen::Affine3d RANSACGroundFilterComponent::getPlaneAffine(
const pcl::PointCloud<PointType> segment_ground_cloud, const Eigen::Vector3d & plane_normal)
{
- pcl::CentroidPoint<PointType> centroid;
+ pcl::CentroidPoint<pcl::PointXYZ> centroid;
for (const auto p : segment_ground_cloud.points) {
centroid.add(p);
}
- PointType centroid_point;
+ pcl::PointXYZ centroid_point;
centroid.get(centroid_point);
Eigen::Translation<double, 3> trans(centroid_point.x, centroid_point.y, centroid_point.z);
const ground_segmentation::PlaneBasis basis = getPlaneBasis(plane_normal);
@@ -284,19 +284,36 @@ void RANSACGroundFilterComponent::filter(
const Eigen::Affine3d plane_affine = getPlaneAffine(*segment_ground_cloud_ptr, plane_normal);
pcl::PointCloud<PointType>::Ptr no_ground_cloud_ptr(new pcl::PointCloud<PointType>);
+ 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<float *>(input->data[global_size + x_offset]);
+ float y = *reinterpret_cast<float *>(input->data[global_size + y_offset]);
+ float z = *reinterpret_cast<float *>(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(
From e77389ba83f23b1bccb9ce305482ee9721375fcd Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 1 May 2024 00:50:50 +0900
Subject: [PATCH 10/13] fix(ray_ground_filter): indices extract bug fix
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ray_ground_filter_nodelet.hpp | 2 +-
.../src/ray_ground_filter_nodelet.cpp | 32 ++++++++++++-------
2 files changed, 22 insertions(+), 12 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 74877255f1077..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,7 +186,7 @@ class RayGroundFilterComponent : public pointcloud_preprocessor::Filter
* @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
*/
void ExtractPointsIndices(
- const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices,
+ const PointCloud2::ConstSharedPtr in_cloud_ptr, pcl::PointIndices & in_indices,
PointCloud2::SharedPtr out_only_indices_cloud_ptr,
PointCloud2::SharedPtr out_removed_indices_cloud_ptr);
diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
index 9221e8737422a..108d82d8f2556 100644
--- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
@@ -270,25 +270,39 @@ void RayGroundFilterComponent::initializePointCloud2(
}
void RayGroundFilterComponent::ExtractPointsIndices(
- const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices,
+ 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);
size_t ground_count = 0;
int point_step = in_cloud_ptr->point_step;
- size_t prev_ground_count = 0;
+ size_t prev_ground_idx = 0;
size_t no_ground_count = 0;
+ // sort indices
+ sort(in_indices.indices.begin(), in_indices.indices.end());
for (const auto & idx : in_indices.indices) {
std::memcpy(
&ground_cloud_msg_ptr->data[ground_count * point_step], &in_cloud_ptr->data[idx * point_step],
point_step);
- std::memcpy(
- &no_ground_cloud_msg_ptr->data[prev_ground_count * point_step],
- &in_cloud_ptr->data[idx * point_step], point_step * (idx - prev_ground_count));
- prev_ground_count = idx - prev_ground_count;
ground_count++;
- prev_ground_count = idx;
+ if (idx - prev_ground_idx > 1) {
+ std::memcpy(
+ &no_ground_cloud_msg_ptr->data[no_ground_count * point_step],
+ &in_cloud_ptr->data[(prev_ground_idx + 1) * point_step],
+ point_step * (idx - prev_ground_idx - 1));
+ no_ground_count += idx - prev_ground_idx - 1;
+ }
+ prev_ground_idx = idx;
+ }
+
+ // Check no_ground_points after last idx
+ if (prev_ground_idx < in_cloud_ptr->width - 1) {
+ std::memcpy(
+ &no_ground_cloud_msg_ptr->data[no_ground_count * point_step],
+ &in_cloud_ptr->data[(prev_ground_idx + 1) * point_step],
+ point_step * (in_cloud_ptr->width - prev_ground_idx - 1));
+ no_ground_count += in_cloud_ptr->width - prev_ground_idx - 1;
}
ground_cloud_msg_ptr->data.resize(ground_count * point_step);
no_ground_cloud_msg_ptr->data.resize(no_ground_count * point_step);
@@ -329,10 +343,6 @@ void RayGroundFilterComponent::filter(
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);
-
- pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr);
- no_ground_cloud_msg_ptr->header = input->header;
-
output = *no_ground_cloud_msg_ptr;
}
From 5cf469e770a838d3cae325ecd82e879961cdef70 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 2 May 2024 21:31:46 +0900
Subject: [PATCH 11/13] refactor: ray_ground_filter
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../src/ray_ground_filter_nodelet.cpp | 35 +++++++------------
1 file changed, 13 insertions(+), 22 deletions(-)
diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
index 108d82d8f2556..ae2ad224cf46c 100644
--- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
@@ -275,34 +275,25 @@ void RayGroundFilterComponent::ExtractPointsIndices(
{
initializePointCloud2(in_cloud_ptr, ground_cloud_msg_ptr);
initializePointCloud2(in_cloud_ptr, no_ground_cloud_msg_ptr);
- size_t ground_count = 0;
int point_step = in_cloud_ptr->point_step;
- size_t prev_ground_idx = 0;
+ size_t ground_count = 0;
size_t no_ground_count = 0;
- // sort indices
- sort(in_indices.indices.begin(), in_indices.indices.end());
+ std::vector<bool> is_ground_idx(in_cloud_ptr->width, false);
for (const auto & idx : in_indices.indices) {
- std::memcpy(
- &ground_cloud_msg_ptr->data[ground_count * point_step], &in_cloud_ptr->data[idx * point_step],
- point_step);
- ground_count++;
- if (idx - prev_ground_idx > 1) {
+ 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[(prev_ground_idx + 1) * point_step],
- point_step * (idx - prev_ground_idx - 1));
- no_ground_count += idx - prev_ground_idx - 1;
+ &in_cloud_ptr->data[i * point_step], point_step);
+ no_ground_count++;
}
- prev_ground_idx = idx;
- }
-
- // Check no_ground_points after last idx
- if (prev_ground_idx < in_cloud_ptr->width - 1) {
- std::memcpy(
- &no_ground_cloud_msg_ptr->data[no_ground_count * point_step],
- &in_cloud_ptr->data[(prev_ground_idx + 1) * point_step],
- point_step * (in_cloud_ptr->width - prev_ground_idx - 1));
- no_ground_count += in_cloud_ptr->width - prev_ground_idx - 1;
}
ground_cloud_msg_ptr->data.resize(ground_count * point_step);
no_ground_cloud_msg_ptr->data.resize(no_ground_count * point_step);
From 8b5419e527980e4b6a2a2a6e2b57e106994511d9 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 9 May 2024 20:24:55 +0900
Subject: [PATCH 12/13] fix: indices max
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
.../ground_segmentation/src/ray_ground_filter_nodelet.cpp | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
index ae2ad224cf46c..3add6f2203c5b 100644
--- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
@@ -280,6 +280,10 @@ void RayGroundFilterComponent::ExtractPointsIndices(
size_t no_ground_count = 0;
std::vector<bool> 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) {
From 743ef48ba8d69f12491549aec4baef17c196e2b2 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 9 May 2024 11:27:01 +0000
Subject: [PATCH 13/13] style(pre-commit): autofix
---
.../ground_segmentation/src/ray_ground_filter_nodelet.cpp | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
index 3add6f2203c5b..c58f1c0e507e5 100644
--- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
+++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
@@ -280,8 +280,7 @@ void RayGroundFilterComponent::ExtractPointsIndices(
size_t no_ground_count = 0;
std::vector<bool> is_ground_idx(in_cloud_ptr->width, false);
for (const auto & idx : in_indices.indices) {
- if(std::size_t(idx) >= is_ground_idx.size())
- {
+ if (std::size_t(idx) >= is_ground_idx.size()) {
continue;
}
is_ground_idx[idx] = true;