From 38446c60e87ab9e3f7c34ea03a5bb9c29addcf35 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 10 May 2024 22:37:20 -0700 Subject: [PATCH] support various point cloud type --- .../organized_statistical_outlier_removal.h | 4 +- ...ed_statistical_outlier_removal_nodelet.cpp | 133 +++++++++--------- 2 files changed, 73 insertions(+), 64 deletions(-) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h index 2891305ad7..a79358ac9e 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h @@ -50,7 +50,6 @@ namespace jsk_pcl_ros { public: typedef jsk_pcl_ros::OrganizedStatisticalOutlierRemovalConfig Config; - typedef pcl::PointXYZRGB PointT; OrganizedStatisticalOutlierRemoval(); protected: //////////////////////////////////////////////////////// @@ -65,6 +64,9 @@ namespace jsk_pcl_ros virtual void configCallback (Config &config, uint32_t level); virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg); + virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered, + bool keep_organized); virtual void updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat); diff --git a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp index 2c9cea7014..2cccba749f 100644 --- a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace jsk_pcl_ros @@ -79,30 +80,23 @@ namespace jsk_pcl_ros std_mul_ = config.stddev; } - void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg) + void OrganizedStatisticalOutlierRemoval::filterCloud( + const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered, + bool keep_organized) { - boost::mutex::scoped_lock lock(mutex_); - vital_checker_->poke(); - sensor_msgs::PointCloud2 output; - - if (keep_organized_&& msg->is_dense) { - NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); - } - bool keep_organized = keep_organized_ && !msg->is_dense; - #if PCL_VERSION_COMPARE (<, 1, 9, 0) + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(*pcl_cloud, *cloud); if (keep_organized) { - // Send the input dataset to the spatial locator - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - // Initialize the spatial locator pcl::search::Search::Ptr tree; tree.reset (new pcl::search::OrganizedNeighbor ()); tree->setInputCloud (cloud); // Allocate enough space to hold the results - std::vector indices (cloud->size ()); + std::vector indices (cloud->size()); + std::vector indices_filtered; std::vector nn_indices (mean_k_); std::vector nn_dists (mean_k_); std::vector distances (indices.size ()); @@ -147,74 +141,87 @@ namespace jsk_pcl_ros double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier - // Copy the common fields - output.is_bigendian = msg->is_bigendian; - output.fields = msg->fields; - output.point_step = msg->point_step; - output.data.resize (msg->data.size ()); - // Build a new cloud by neglecting outliers - int nr_p = 0; - int nr_removed_p = 0; - bool remove_point = false; for (int cp = 0; cp < static_cast (indices.size ()); ++cp) { + bool remove_point = false; if (negative_) + { remove_point = (distances[cp] <= distance_threshold); + } else - remove_point = (distances[cp] > distance_threshold); - if (remove_point) { - /* Set the current point to NaN. */ - *(reinterpret_cast(&output.data[nr_p * output.point_step])+0) = std::numeric_limits::quiet_NaN(); - *(reinterpret_cast(&output.data[nr_p * output.point_step])+1) = std::numeric_limits::quiet_NaN(); - *(reinterpret_cast(&output.data[nr_p * output.point_step])+2) = std::numeric_limits::quiet_NaN(); - nr_p++; + remove_point = (distances[cp] > distance_threshold); } - else + if (!remove_point) { - memcpy (&output.data[nr_p * output.point_step], - &msg->data[indices[cp] * output.point_step], - output.point_step); - nr_p++; + indices_filtered.push_back(indices[cp]); } } - output.width = msg->width; - output.height = msg->height; - output.row_step = output.point_step * output.width; + pcl_indices_filtered->indices = indices_filtered; } else { - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK (mean_k_); - sor.setStddevMulThresh (std_mul_); - sor.setNegative (negative_); - sor.filter (*cloud_filtered); - pcl::toROSMsg(*cloud_filtered, output); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(pcl_cloud); + sor.setMeanK(mean_k_); + sor.setStddevMulThresh(std_mul_); + sor.setNegative(negative_); + sor.filter(pcl_indices_filtered->indices); } - output.header = msg->header; - output.is_dense = !keep_organized; - pub_.publish(output); #else - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK (mean_k_); - sor.setStddevMulThresh (std_mul_); - sor.setNegative (negative_); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(pcl_cloud); + sor.setMeanK(mean_k_); + sor.setStddevMulThresh(std_mul_); + sor.setNegative(negative_); sor.setKeepOrganized (keep_organized); - sor.filter (*cloud_filtered); - pcl::toROSMsg(*cloud_filtered, output); + sor.filter(pcl_indices_filtered->indices); +#endif + } + + void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg) + { + boost::mutex::scoped_lock lock(mutex_); + vital_checker_->poke(); + sensor_msgs::PointCloud2 output; + + if (keep_organized_&& msg->is_dense) { + NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); + } + bool keep_organized = keep_organized_ && !msg->is_dense; + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*msg, *pcl_cloud); + + // filter + pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices()); + pcl_indices_filtered->indices.resize(pcl_cloud->data.size()); + OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud, + pcl_indices_filtered, + keep_organized); + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); + pcl::ExtractIndices ex; + ex.setInputCloud(pcl_cloud); + ex.setKeepOrganized(keep_organized); + ex.setNegative(false); + ex.setIndices(pcl_indices_filtered); + ex.filter(*pcl_cloud_filtered); + pcl_conversions::fromPCL(*pcl_cloud_filtered, output); +#if PCL_VERSION_COMPARE (<, 1, 9, 0) + if (keep_organized) { + // Copy the common fields + output.is_bigendian = msg->is_bigendian; + output.fields = msg->fields; + output.point_step = msg->point_step; + output.data.resize (msg->data.size ()); + output.width = msg->width; + output.height = msg->height; + output.row_step = msg->point_step * msg->width; + } +#endif output.header = msg->header; output.is_dense = !keep_organized; pub_.publish(output); -#endif diagnostic_updater_->update(); }