From 5f76fd0d649d21c00ffcadf1b36495336f57b17f Mon Sep 17 00:00:00 2001 From: amslrobots Date: Mon, 19 Aug 2024 23:09:23 +0900 Subject: [PATCH 01/15] Create removal filter for realsense --- CMakeLists.txt | 6 +++ src/removal_filter.cpp | 102 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 108 insertions(+) create mode 100644 src/removal_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ec2311b..65a8825 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -267,6 +267,12 @@ target_link_libraries(pc_extractor add_executable(virtual_road_projector src/virtual_road_projector.cpp) target_link_libraries(virtual_road_projector ${catkin_LIBRARIES}) +add_executable(removal_filter src/removal_filter.cpp) +target_link_libraries(removal_filter + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} +) + if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(localmap_creator_test diff --git a/src/removal_filter.cpp b/src/removal_filter.cpp new file mode 100644 index 0000000..4861759 --- /dev/null +++ b/src/removal_filter.cpp @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class RemovalFilter +{ +public: + RemovalFilter(void); + +protected: + typedef pcl::PointXYZ PointType; + typedef pcl::PointCloud PointCloudType; + typedef pcl::PointCloud::Ptr PointCloudTypePtr; + + void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); + void voxel_grid_filter(PointCloudTypePtr cloud, const float leaf_size); + void random_filter(PointCloudTypePtr cloud, const int size); + void removal_filter(PointCloudTypePtr cloud); + + std::string target_frame_; + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Subscriber point_cloud_sub_; + ros::Publisher point_cloud_pub_; + tf::TransformListener tfListener_; +}; + +RemovalFilter::RemovalFilter(void) : private_nh_("~") +{ + private_nh_.param("target_frame", target_frame_, {"base_link"}); + + point_cloud_sub_ = nh_.subscribe( + "/cloud", 1, &RemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); + point_cloud_pub_ = nh_.advertise("/cloud/filtered", 1); +} + +void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) +{ + PointCloudTypePtr pc_ptr(new PointCloudType); + pcl::fromROSMsg(*msg, *pc_ptr); + + // transform + PointCloudTypePtr pc_transformed_ptr(new PointCloudType); + pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); + random_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); + + // add dummy data + pcl::PointCloud cloud; + cloud.width = 50; + cloud.height = 50; + cloud.resize(cloud.width * cloud.height); + for (auto &point : cloud) + { + point.x = 1024 * rand() / (RAND_MAX + 1.0f); + point.y = 1024 * rand() / (RAND_MAX + 1.0f); + point.z = 0.0; + } + pc_transformed_ptr->insert(pc_transformed_ptr->end(), cloud.begin(), cloud.end()); + + // filter + removal_filter(pc_transformed_ptr); + + // convert pcl::PointCloud to sensor_msgs::PointCloud2 + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(*pc_transformed_ptr, cloud_msg); + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = target_frame_; + point_cloud_pub_.publish(cloud_msg); +} + +void RemovalFilter::random_filter(PointCloudTypePtr cloud, const int size) +{ + pcl::RandomSample sor; + sor.setInputCloud(cloud); + sor.setSample(size); + sor.filter(*cloud); +} + +void RemovalFilter::removal_filter(PointCloudTypePtr cloud) +{ + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud); + sor.setMeanK(50); + sor.setStddevMulThresh(0.8); + sor.filter(*cloud); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "removal_filter"); + RemovalFilter removal_filter; + ros::spin(); + + return 0; +} From b28b566720954c13309e080cce818f6af6edbded Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 14:05:37 +0900 Subject: [PATCH 02/15] Rename file --- src/{removal_filter.cpp => realsense_outlier_removal_filter.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/{removal_filter.cpp => realsense_outlier_removal_filter.cpp} (100%) diff --git a/src/removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp similarity index 100% rename from src/removal_filter.cpp rename to src/realsense_outlier_removal_filter.cpp From 315885cced63cde59f8820fac4724544d68a5c7a Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:08:19 +0900 Subject: [PATCH 03/15] Remove function --- src/realsense_outlier_removal_filter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 4861759..57261ff 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -20,7 +20,6 @@ class RemovalFilter typedef pcl::PointCloud::Ptr PointCloudTypePtr; void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); - void voxel_grid_filter(PointCloudTypePtr cloud, const float leaf_size); void random_filter(PointCloudTypePtr cloud, const int size); void removal_filter(PointCloudTypePtr cloud); From 12246bf8076203a941e3f72fae364ba670272648 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:23:22 +0900 Subject: [PATCH 04/15] Rename file --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 65a8825..16808b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -267,8 +267,8 @@ target_link_libraries(pc_extractor add_executable(virtual_road_projector src/virtual_road_projector.cpp) target_link_libraries(virtual_road_projector ${catkin_LIBRARIES}) -add_executable(removal_filter src/removal_filter.cpp) -target_link_libraries(removal_filter +add_executable(realsense_outlier_removal_filter src/realsense_outlier_removal_filter.cpp) +target_link_libraries(realsense_outlier_removal_filter ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) From 7a8f3132537e999e545a1355c0198332ac85c5fa Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:23:53 +0900 Subject: [PATCH 05/15] Update typedef --- src/realsense_outlier_removal_filter.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 57261ff..8c26098 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -15,13 +15,12 @@ class RemovalFilter RemovalFilter(void); protected: - typedef pcl::PointXYZ PointType; - typedef pcl::PointCloud PointCloudType; - typedef pcl::PointCloud::Ptr PointCloudTypePtr; + typedef pcl::PointXYZ PointT; + typedef pcl::PointCloud PointCloudT; void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); - void random_filter(PointCloudTypePtr cloud, const int size); - void removal_filter(PointCloudTypePtr cloud); + void random_filter(PointCloudT::Ptr cloud, const int size); + void removal_filter(PointCloudT::Ptr cloud); std::string target_frame_; ros::NodeHandle nh_; @@ -42,11 +41,11 @@ RemovalFilter::RemovalFilter(void) : private_nh_("~") void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { - PointCloudTypePtr pc_ptr(new PointCloudType); + PointCloudT::Ptr pc_ptr(new PointCloudT); pcl::fromROSMsg(*msg, *pc_ptr); // transform - PointCloudTypePtr pc_transformed_ptr(new PointCloudType); + PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); random_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); @@ -74,15 +73,15 @@ void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) point_cloud_pub_.publish(cloud_msg); } -void RemovalFilter::random_filter(PointCloudTypePtr cloud, const int size) +void RemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) { - pcl::RandomSample sor; + pcl::RandomSample sor; sor.setInputCloud(cloud); sor.setSample(size); sor.filter(*cloud); } -void RemovalFilter::removal_filter(PointCloudTypePtr cloud) +void RemovalFilter::removal_filter(PointCloudT::Ptr cloud) { pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud); From 8ed0a6118ada9bcb225cde4c262721949f35d018 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:24:55 +0900 Subject: [PATCH 06/15] Fix format --- src/realsense_outlier_removal_filter.cpp | 114 +++++++++++------------ 1 file changed, 57 insertions(+), 57 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 8c26098..ee001c0 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -1,9 +1,9 @@ -#include -#include #include #include #include #include +#include +#include #include #include #include @@ -12,89 +12,89 @@ class RemovalFilter { public: - RemovalFilter(void); + RemovalFilter(void); protected: - typedef pcl::PointXYZ PointT; - typedef pcl::PointCloud PointCloudT; + typedef pcl::PointXYZ PointT; + typedef pcl::PointCloud PointCloudT; - void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); - void random_filter(PointCloudT::Ptr cloud, const int size); - void removal_filter(PointCloudT::Ptr cloud); + void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); + void random_filter(PointCloudT::Ptr cloud, const int size); + void removal_filter(PointCloudT::Ptr cloud); - std::string target_frame_; - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - ros::Subscriber point_cloud_sub_; - ros::Publisher point_cloud_pub_; - tf::TransformListener tfListener_; + std::string target_frame_; + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Subscriber point_cloud_sub_; + ros::Publisher point_cloud_pub_; + tf::TransformListener tfListener_; }; RemovalFilter::RemovalFilter(void) : private_nh_("~") { - private_nh_.param("target_frame", target_frame_, {"base_link"}); + private_nh_.param("target_frame", target_frame_, {"base_link"}); - point_cloud_sub_ = nh_.subscribe( - "/cloud", 1, &RemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); - point_cloud_pub_ = nh_.advertise("/cloud/filtered", 1); + point_cloud_sub_ = + nh_.subscribe("/cloud", 1, &RemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); + point_cloud_pub_ = nh_.advertise("/cloud/filtered", 1); } void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { - PointCloudT::Ptr pc_ptr(new PointCloudT); - pcl::fromROSMsg(*msg, *pc_ptr); + PointCloudT::Ptr pc_ptr(new PointCloudT); + pcl::fromROSMsg(*msg, *pc_ptr); - // transform - PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); - pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); - random_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); + // transform + PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); + pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); + random_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); - // add dummy data - pcl::PointCloud cloud; - cloud.width = 50; - cloud.height = 50; - cloud.resize(cloud.width * cloud.height); - for (auto &point : cloud) - { - point.x = 1024 * rand() / (RAND_MAX + 1.0f); - point.y = 1024 * rand() / (RAND_MAX + 1.0f); - point.z = 0.0; - } - pc_transformed_ptr->insert(pc_transformed_ptr->end(), cloud.begin(), cloud.end()); + // add dummy data + pcl::PointCloud cloud; + cloud.width = 50; + cloud.height = 50; + cloud.resize(cloud.width * cloud.height); + for (auto &point : cloud) + { + point.x = 1024 * rand() / (RAND_MAX + 1.0f); + point.y = 1024 * rand() / (RAND_MAX + 1.0f); + point.z = 0.0; + } + pc_transformed_ptr->insert(pc_transformed_ptr->end(), cloud.begin(), cloud.end()); - // filter - removal_filter(pc_transformed_ptr); + // filter + removal_filter(pc_transformed_ptr); - // convert pcl::PointCloud to sensor_msgs::PointCloud2 - sensor_msgs::PointCloud2 cloud_msg; - pcl::toROSMsg(*pc_transformed_ptr, cloud_msg); - cloud_msg.header.stamp = ros::Time::now(); - cloud_msg.header.frame_id = target_frame_; - point_cloud_pub_.publish(cloud_msg); + // convert pcl::PointCloud to sensor_msgs::PointCloud2 + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(*pc_transformed_ptr, cloud_msg); + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = target_frame_; + point_cloud_pub_.publish(cloud_msg); } void RemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) { - pcl::RandomSample sor; - sor.setInputCloud(cloud); - sor.setSample(size); - sor.filter(*cloud); + pcl::RandomSample sor; + sor.setInputCloud(cloud); + sor.setSample(size); + sor.filter(*cloud); } void RemovalFilter::removal_filter(PointCloudT::Ptr cloud) { - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK(50); - sor.setStddevMulThresh(0.8); - sor.filter(*cloud); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud); + sor.setMeanK(50); + sor.setStddevMulThresh(0.8); + sor.filter(*cloud); } int main(int argc, char **argv) { - ros::init(argc, argv, "removal_filter"); - RemovalFilter removal_filter; - ros::spin(); + ros::init(argc, argv, "removal_filter"); + RemovalFilter removal_filter; + ros::spin(); - return 0; + return 0; } From ec1224621b78c94eb7bcdfd6590801a8eb629bf1 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:28:48 +0900 Subject: [PATCH 07/15] Rename class --- src/realsense_outlier_removal_filter.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index ee001c0..b01959f 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -9,10 +9,10 @@ #include #include -class RemovalFilter +class OutlierRemovalFilter { public: - RemovalFilter(void); + OutlierRemovalFilter(void); protected: typedef pcl::PointXYZ PointT; @@ -20,7 +20,7 @@ class RemovalFilter void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); void random_filter(PointCloudT::Ptr cloud, const int size); - void removal_filter(PointCloudT::Ptr cloud); + void outlier_removal_filter(PointCloudT::Ptr cloud); std::string target_frame_; ros::NodeHandle nh_; @@ -30,16 +30,16 @@ class RemovalFilter tf::TransformListener tfListener_; }; -RemovalFilter::RemovalFilter(void) : private_nh_("~") +OutlierRemovalFilter::OutlierRemovalFilter(void) : private_nh_("~") { private_nh_.param("target_frame", target_frame_, {"base_link"}); - point_cloud_sub_ = - nh_.subscribe("/cloud", 1, &RemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); + point_cloud_sub_ = nh_.subscribe( + "/cloud", 1, &OutlierRemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); point_cloud_pub_ = nh_.advertise("/cloud/filtered", 1); } -void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) +void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { PointCloudT::Ptr pc_ptr(new PointCloudT); pcl::fromROSMsg(*msg, *pc_ptr); @@ -63,7 +63,7 @@ void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) pc_transformed_ptr->insert(pc_transformed_ptr->end(), cloud.begin(), cloud.end()); // filter - removal_filter(pc_transformed_ptr); + outlier_removal_filter(pc_transformed_ptr); // convert pcl::PointCloud to sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud_msg; @@ -73,7 +73,7 @@ void RemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) point_cloud_pub_.publish(cloud_msg); } -void RemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) +void OutlierRemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) { pcl::RandomSample sor; sor.setInputCloud(cloud); @@ -81,7 +81,7 @@ void RemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) sor.filter(*cloud); } -void RemovalFilter::removal_filter(PointCloudT::Ptr cloud) +void OutlierRemovalFilter::outlier_removal_filter(PointCloudT::Ptr cloud) { pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud); @@ -92,8 +92,8 @@ void RemovalFilter::removal_filter(PointCloudT::Ptr cloud) int main(int argc, char **argv) { - ros::init(argc, argv, "removal_filter"); - RemovalFilter removal_filter; + ros::init(argc, argv, "outlier_removal_filter"); + OutlierRemovalFilter outlier_removal_filter; ros::spin(); return 0; From 3ddfefb02871d27c8f11149405f4be93518539f1 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:30:54 +0900 Subject: [PATCH 08/15] Rename function --- src/realsense_outlier_removal_filter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index b01959f..ccd25f0 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -19,7 +19,7 @@ class OutlierRemovalFilter typedef pcl::PointCloud PointCloudT; void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); - void random_filter(PointCloudT::Ptr cloud, const int size); + void random_sample_filter(PointCloudT::Ptr cloud, const int size); void outlier_removal_filter(PointCloudT::Ptr cloud); std::string target_frame_; @@ -47,7 +47,7 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr // transform PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); - random_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); + random_sample_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); // add dummy data pcl::PointCloud cloud; @@ -73,7 +73,7 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr point_cloud_pub_.publish(cloud_msg); } -void OutlierRemovalFilter::random_filter(PointCloudT::Ptr cloud, const int size) +void OutlierRemovalFilter::random_sample_filter(PointCloudT::Ptr cloud, const int size) { pcl::RandomSample sor; sor.setInputCloud(cloud); From 5c5095fc9783444403f62833e11c699ba7ee0dab Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 15:32:14 +0900 Subject: [PATCH 09/15] Remove library --- src/realsense_outlier_removal_filter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index ccd25f0..d5f79c5 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include #include From 6426896f5a4c85ca7f4cee877c170d6b38493f3e Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 16:20:38 +0900 Subject: [PATCH 10/15] Add function for adding dummy data --- src/realsense_outlier_removal_filter.cpp | 28 ++++++++++++++---------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index d5f79c5..7e9b02c 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -19,6 +19,7 @@ class OutlierRemovalFilter void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); void random_sample_filter(PointCloudT::Ptr cloud, const int size); + void add_dummy_ground_points(PointCloudT::Ptr cloud, const int point_num, const float size); void outlier_removal_filter(PointCloudT::Ptr cloud); std::string target_frame_; @@ -49,17 +50,7 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr random_sample_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); // add dummy data - pcl::PointCloud cloud; - cloud.width = 50; - cloud.height = 50; - cloud.resize(cloud.width * cloud.height); - for (auto &point : cloud) - { - point.x = 1024 * rand() / (RAND_MAX + 1.0f); - point.y = 1024 * rand() / (RAND_MAX + 1.0f); - point.z = 0.0; - } - pc_transformed_ptr->insert(pc_transformed_ptr->end(), cloud.begin(), cloud.end()); + add_dummy_ground_points(pc_transformed_ptr, 100, 1); // filter outlier_removal_filter(pc_transformed_ptr); @@ -80,6 +71,19 @@ void OutlierRemovalFilter::random_sample_filter(PointCloudT::Ptr cloud, const in sor.filter(*cloud); } +void OutlierRemovalFilter::add_dummy_ground_points(PointCloudT::Ptr cloud, const int point_num, const float size) +{ + pcl::PointCloud dummy_cloud; + dummy_cloud.points.resize(point_num); + for (auto &point : dummy_cloud) + { + point.x = size * rand() / (RAND_MAX + 1.0f) - size / 2.0; + point.y = size * rand() / (RAND_MAX + 1.0f) - size / 2.0; + point.z = 0.0; + } + cloud->insert(cloud->end(), dummy_cloud.begin(), dummy_cloud.end()); +} + void OutlierRemovalFilter::outlier_removal_filter(PointCloudT::Ptr cloud) { pcl::StatisticalOutlierRemoval sor; @@ -91,7 +95,7 @@ void OutlierRemovalFilter::outlier_removal_filter(PointCloudT::Ptr cloud) int main(int argc, char **argv) { - ros::init(argc, argv, "outlier_removal_filter"); + ros::init(argc, argv, "realsense_outlier_removal_filter"); OutlierRemovalFilter outlier_removal_filter; ros::spin(); From 232d60fcfe88f8cd05f3f95c837cb8aa10b96cb4 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 16:29:29 +0900 Subject: [PATCH 11/15] Update function --- src/realsense_outlier_removal_filter.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 7e9b02c..24ee8c6 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -20,7 +20,7 @@ class OutlierRemovalFilter void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); void random_sample_filter(PointCloudT::Ptr cloud, const int size); void add_dummy_ground_points(PointCloudT::Ptr cloud, const int point_num, const float size); - void outlier_removal_filter(PointCloudT::Ptr cloud); + void outlier_removal_filter(PointCloudT::Ptr cloud, const int num_neighbors, const float std_dev_mul_thresh); std::string target_frame_; ros::NodeHandle nh_; @@ -53,7 +53,7 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr add_dummy_ground_points(pc_transformed_ptr, 100, 1); // filter - outlier_removal_filter(pc_transformed_ptr); + outlier_removal_filter(pc_transformed_ptr, 50, 1.0); // convert pcl::PointCloud to sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud_msg; @@ -84,12 +84,13 @@ void OutlierRemovalFilter::add_dummy_ground_points(PointCloudT::Ptr cloud, const cloud->insert(cloud->end(), dummy_cloud.begin(), dummy_cloud.end()); } -void OutlierRemovalFilter::outlier_removal_filter(PointCloudT::Ptr cloud) +void OutlierRemovalFilter::outlier_removal_filter( + PointCloudT::Ptr cloud, const int num_neighbors, const float std_dev_mul_thresh) { - pcl::StatisticalOutlierRemoval sor; + pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud); - sor.setMeanK(50); - sor.setStddevMulThresh(0.8); + sor.setMeanK(num_neighbors); + sor.setStddevMulThresh(std_dev_mul_thresh); sor.filter(*cloud); } From 09d9ffe6f870be1e33dc882eda730812deb6c9ce Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 16:41:17 +0900 Subject: [PATCH 12/15] Add function for passthrough filter --- src/realsense_outlier_removal_filter.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 24ee8c6..ae44271 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -18,6 +19,7 @@ class OutlierRemovalFilter typedef pcl::PointCloud PointCloudT; void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); + void passthrough_filter(PointCloudT::Ptr cloud, const std::string &axis, const float min, const float max); void random_sample_filter(PointCloudT::Ptr cloud, const int size); void add_dummy_ground_points(PointCloudT::Ptr cloud, const int point_num, const float size); void outlier_removal_filter(PointCloudT::Ptr cloud, const int num_neighbors, const float std_dev_mul_thresh); @@ -47,6 +49,9 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr // transform PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); + + // filter + passthrough_filter(pc_transformed_ptr, "z", -0.05, 3.0); random_sample_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); // add dummy data @@ -63,6 +68,16 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr point_cloud_pub_.publish(cloud_msg); } +void OutlierRemovalFilter::passthrough_filter( + PointCloudT::Ptr cloud, const std::string &axis, const float min, const float max) +{ + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName(axis); + pass.setFilterLimits(min, max); + pass.filter(*cloud); +} + void OutlierRemovalFilter::random_sample_filter(PointCloudT::Ptr cloud, const int size) { pcl::RandomSample sor; From 4160d68be71210329a8979267927fbcd9955bef0 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 17:00:25 +0900 Subject: [PATCH 13/15] Add param setting --- src/realsense_outlier_removal_filter.cpp | 56 +++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index ae44271..291901c 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -9,6 +9,18 @@ #include #include +struct Params +{ + std::string target_frame; + bool remove_underground; + float ramdom_sample_percent; + bool use_dummy_ground_points; + int dummy_ground_points_num; + float dummy_ground_points_size; + int outlier_removal_num_neighbors; + float outlier_removal_std_dev_mul_thresh; +}; + class OutlierRemovalFilter { public: @@ -18,13 +30,15 @@ class OutlierRemovalFilter typedef pcl::PointXYZ PointT; typedef pcl::PointCloud PointCloudT; + void load_params(void); + void print_params(void); void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg); void passthrough_filter(PointCloudT::Ptr cloud, const std::string &axis, const float min, const float max); void random_sample_filter(PointCloudT::Ptr cloud, const int size); void add_dummy_ground_points(PointCloudT::Ptr cloud, const int point_num, const float size); void outlier_removal_filter(PointCloudT::Ptr cloud, const int num_neighbors, const float std_dev_mul_thresh); - std::string target_frame_; + Params params_; ros::NodeHandle nh_; ros::NodeHandle private_nh_; ros::Subscriber point_cloud_sub_; @@ -34,13 +48,39 @@ class OutlierRemovalFilter OutlierRemovalFilter::OutlierRemovalFilter(void) : private_nh_("~") { - private_nh_.param("target_frame", target_frame_, {"base_link"}); + load_params(); + print_params(); point_cloud_sub_ = nh_.subscribe( "/cloud", 1, &OutlierRemovalFilter::cloud_callback, this, ros::TransportHints().reliable().tcpNoDelay()); point_cloud_pub_ = nh_.advertise("/cloud/filtered", 1); } +void OutlierRemovalFilter::load_params(void) +{ + private_nh_.param("target_frame", params_.target_frame, {"base_link"}); + private_nh_.param("remove_underground", params_.remove_underground, {false}); + private_nh_.param("ramdom_sample_percent", params_.ramdom_sample_percent, {0.1}); + private_nh_.param("dummy_ground_points/enable", params_.use_dummy_ground_points, {false}); + private_nh_.param("dummy_ground_points/num", params_.dummy_ground_points_num, {100}); + private_nh_.param("dummy_ground_points/size", params_.dummy_ground_points_size, {2.0}); + private_nh_.param("outlier_removal/num_neighbors", params_.outlier_removal_num_neighbors, {50}); + private_nh_.param("outlier_removal/std_dev_mul_thresh", params_.outlier_removal_std_dev_mul_thresh, {1.0}); +} + +void OutlierRemovalFilter::print_params(void) +{ + ROS_INFO("=== Realsense Outlier Removal Filter ==="); + ROS_INFO_STREAM("target_frame: " << params_.target_frame); + ROS_INFO_STREAM("remove_underground: " << params_.remove_underground); + ROS_INFO_STREAM("ramdom_sample_percent: " << params_.ramdom_sample_percent); + ROS_INFO_STREAM("dummy_ground_points/enable: " << params_.use_dummy_ground_points); + ROS_INFO_STREAM("dummy_ground_points/num: " << params_.dummy_ground_points_num); + ROS_INFO_STREAM("dummy_ground_points/size: " << params_.dummy_ground_points_size); + ROS_INFO_STREAM("outlier_removal/num_neighbors: " << params_.outlier_removal_num_neighbors); + ROS_INFO_STREAM("outlier_removal/std_dev_mul_thresh: " << params_.outlier_removal_std_dev_mul_thresh); +} + void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { PointCloudT::Ptr pc_ptr(new PointCloudT); @@ -51,14 +91,18 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); // filter - passthrough_filter(pc_transformed_ptr, "z", -0.05, 3.0); - random_sample_filter(pc_transformed_ptr, pc_transformed_ptr->size() / 1000); + if (params_.remove_underground) + passthrough_filter(pc_transformed_ptr, "z", -0.05, 3.0); + random_sample_filter( + pc_transformed_ptr, static_cast(pc_transformed_ptr->size() * params_.ramdom_sample_percent / 100)); // add dummy data - add_dummy_ground_points(pc_transformed_ptr, 100, 1); + if (params_.use_dummy_ground_points) + add_dummy_ground_points(pc_transformed_ptr, params_.dummy_ground_points_num, params_.dummy_ground_points_size); // filter - outlier_removal_filter(pc_transformed_ptr, 50, 1.0); + outlier_removal_filter( + pc_transformed_ptr, params_.outlier_removal_num_neighbors, params_.outlier_removal_std_dev_mul_thresh); // convert pcl::PointCloud to sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud_msg; From d0cb508504c6eeb5bcc32b3181bae6f0cbb755d7 Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 17:02:28 +0900 Subject: [PATCH 14/15] Update comments --- src/realsense_outlier_removal_filter.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 291901c..467ddd7 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -83,6 +83,7 @@ void OutlierRemovalFilter::print_params(void) void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { + // convert sensor_msgs::PointCloud2 to pcl::PointCloud PointCloudT::Ptr pc_ptr(new PointCloudT); pcl::fromROSMsg(*msg, *pc_ptr); @@ -90,7 +91,7 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); - // filter + // preprocess if (params_.remove_underground) passthrough_filter(pc_transformed_ptr, "z", -0.05, 3.0); random_sample_filter( @@ -100,13 +101,15 @@ void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr if (params_.use_dummy_ground_points) add_dummy_ground_points(pc_transformed_ptr, params_.dummy_ground_points_num, params_.dummy_ground_points_size); - // filter + // outlier removal outlier_removal_filter( pc_transformed_ptr, params_.outlier_removal_num_neighbors, params_.outlier_removal_std_dev_mul_thresh); // convert pcl::PointCloud to sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(*pc_transformed_ptr, cloud_msg); + + // publish cloud_msg.header.stamp = ros::Time::now(); cloud_msg.header.frame_id = target_frame_; point_cloud_pub_.publish(cloud_msg); From 7918548160a548a5acbb98b9dc6c2f214012b55a Mon Sep 17 00:00:00 2001 From: Toshiki Nakamura Date: Sat, 24 Aug 2024 17:19:22 +0900 Subject: [PATCH 15/15] Remove variable --- src/realsense_outlier_removal_filter.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/realsense_outlier_removal_filter.cpp b/src/realsense_outlier_removal_filter.cpp index 467ddd7..d529f1a 100644 --- a/src/realsense_outlier_removal_filter.cpp +++ b/src/realsense_outlier_removal_filter.cpp @@ -84,34 +84,31 @@ void OutlierRemovalFilter::print_params(void) void OutlierRemovalFilter::cloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) { // convert sensor_msgs::PointCloud2 to pcl::PointCloud - PointCloudT::Ptr pc_ptr(new PointCloudT); - pcl::fromROSMsg(*msg, *pc_ptr); + PointCloudT::Ptr cloud(new PointCloudT); + pcl::fromROSMsg(*msg, *cloud); // transform - PointCloudT::Ptr pc_transformed_ptr(new PointCloudT); - pcl_ros::transformPointCloud(target_frame_, *pc_ptr, *pc_transformed_ptr, tfListener_); + pcl_ros::transformPointCloud(params_.target_frame, *cloud, *cloud, tfListener_); // preprocess if (params_.remove_underground) - passthrough_filter(pc_transformed_ptr, "z", -0.05, 3.0); - random_sample_filter( - pc_transformed_ptr, static_cast(pc_transformed_ptr->size() * params_.ramdom_sample_percent / 100)); + passthrough_filter(cloud, "z", -0.05, 3.0); + random_sample_filter(cloud, static_cast(cloud->size() * params_.ramdom_sample_percent / 100)); // add dummy data if (params_.use_dummy_ground_points) - add_dummy_ground_points(pc_transformed_ptr, params_.dummy_ground_points_num, params_.dummy_ground_points_size); + add_dummy_ground_points(cloud, params_.dummy_ground_points_num, params_.dummy_ground_points_size); // outlier removal - outlier_removal_filter( - pc_transformed_ptr, params_.outlier_removal_num_neighbors, params_.outlier_removal_std_dev_mul_thresh); + outlier_removal_filter(cloud, params_.outlier_removal_num_neighbors, params_.outlier_removal_std_dev_mul_thresh); // convert pcl::PointCloud to sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud_msg; - pcl::toROSMsg(*pc_transformed_ptr, cloud_msg); + pcl::toROSMsg(*cloud, cloud_msg); // publish cloud_msg.header.stamp = ros::Time::now(); - cloud_msg.header.frame_id = target_frame_; + cloud_msg.header.frame_id = params_.target_frame; point_cloud_pub_.publish(cloud_msg); }