From f088f318a8665ff0128adb50613c9373fe14dbd4 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 2 Dec 2024 19:10:57 -0800 Subject: [PATCH] support melodic --- .../organized_statistical_outlier_removal.h | 5 +- ...ed_statistical_outlier_removal_nodelet.cpp | 75 ++++++++++++------- 2 files changed, 53 insertions(+), 27 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 a79358ac9e..4cf5acdddb 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 @@ -65,8 +65,11 @@ namespace jsk_pcl_ros virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg); virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud, - pcl::PointIndices::Ptr pcl_indices_filtered, + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered, bool keep_organized); + virtual void filterIndices(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 2cccba749f..22239c15cc 100644 --- a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp @@ -82,13 +82,12 @@ namespace jsk_pcl_ros void OrganizedStatisticalOutlierRemoval::filterCloud( const pcl::PCLPointCloud2::Ptr pcl_cloud, - pcl::PointIndices::Ptr pcl_indices_filtered, + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered, bool keep_organized) { -#if PCL_VERSION_COMPARE (<, 1, 9, 0) - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::fromPCLPointCloud2(*pcl_cloud, *cloud); if (keep_organized) { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(*pcl_cloud, *cloud); // Initialize the spatial locator pcl::search::Search::Ptr tree; tree.reset (new pcl::search::OrganizedNeighbor ()); @@ -96,7 +95,6 @@ namespace jsk_pcl_ros // Allocate enough space to hold the results 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 ()); @@ -142,6 +140,9 @@ namespace jsk_pcl_ros double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier // 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; @@ -153,12 +154,22 @@ namespace jsk_pcl_ros { remove_point = (distances[cp] > distance_threshold); } - if (!remove_point) + + if (remove_point) { - indices_filtered.push_back(indices[cp]); + /* Set the current point to NaN. */ + *(reinterpret_cast(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+0) = std::numeric_limits::quiet_NaN(); + *(reinterpret_cast(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+1) = std::numeric_limits::quiet_NaN(); + *(reinterpret_cast(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+2) = std::numeric_limits::quiet_NaN(); } + else + { + memcpy (&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step], + &pcl_cloud->data[indices[cp] * pcl_cloud_filtered->point_step], + pcl_cloud_filtered->point_step); + } + nr_p++; } - pcl_indices_filtered->indices = indices_filtered; } else { @@ -167,9 +178,16 @@ namespace jsk_pcl_ros sor.setMeanK(mean_k_); sor.setStddevMulThresh(std_mul_); sor.setNegative(negative_); - sor.filter(pcl_indices_filtered->indices); + sor.filter(*pcl_cloud_filtered); } -#else + } + + void OrganizedStatisticalOutlierRemoval::filterIndices( + const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered, + bool keep_organized) + { +#if PCL_VERSION_COMPARE (>=, 1, 9, 0) pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(pcl_cloud); sor.setMeanK(mean_k_); @@ -190,15 +208,15 @@ namespace jsk_pcl_ros NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized."); } bool keep_organized = keep_organized_ && !msg->is_dense; + // filter +#if PCL_VERSION_COMPARE (>=, 1, 9, 0) 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); + OrganizedStatisticalOutlierRemoval::filterIndices(pcl_cloud, + pcl_indices_filtered, + keep_organized); pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); pcl::ExtractIndices ex; ex.setInputCloud(pcl_cloud); @@ -207,17 +225,22 @@ namespace jsk_pcl_ros 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; - } +#else + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*msg, *pcl_cloud); + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); + // Copy the common fields + pcl_cloud_filtered->is_bigendian = msg->is_bigendian; + pcl_cloud_filtered->fields = msg->fields; + pcl_cloud_filtered->point_step = msg->point_step; + pcl_cloud_filtered->data.resize (msg->data.size ()); + pcl_cloud_filtered->width = msg->width; + pcl_cloud_filtered->height = msg->height; + pcl_cloud_filtered->row_step = msg->point_step * msg->width; + OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud, + pcl_cloud_filtered, + keep_organized); + pcl_conversions::fromPCL(*pcl_cloud_filtered, output); #endif output.header = msg->header; output.is_dense = !keep_organized;