From 5e6e803db543ec9aa5ee66353a57932626349516 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 | 15 +- ...ed_statistical_outlier_removal_nodelet.cpp | 140 ++++++++---------- 2 files changed, 67 insertions(+), 88 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 368409793a..a9aa0f6667 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 @@ -63,7 +63,6 @@ namespace jsk_pcl_ros typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy; - typedef pcl::PointXYZRGB PointT; OrganizedStatisticalOutlierRemoval(); protected: //////////////////////////////////////////////////////// @@ -77,18 +76,16 @@ namespace jsk_pcl_ros virtual void configCallback (Config &config, uint32_t level); - virtual void filter( - const pcl::PointCloud::Ptr cloud, - pcl::PointCloud::Ptr cloud_filtered, - bool keep_organized); - virtual void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg); - virtual void filterCloudWithClusterPointIndices( + void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered, + bool keep_organized); + void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg); + void filterCloudWithClusterPointIndices( const sensor_msgs::PointCloud2::ConstPtr& msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cpi_msg); - virtual void updateDiagnostic( - diagnostic_updater::DiagnosticStatusWrapper &stat); + void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); //////////////////////////////////////////////////////// // ROS variables 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 67dbd35d12..6a4a54745e 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 @@ -115,19 +116,22 @@ namespace jsk_pcl_ros } void OrganizedStatisticalOutlierRemoval::filter( - pcl::PointCloud::Ptr cloud, - pcl::PointCloud::Ptr cloud_filtered, + const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_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) { // Initialize the spatial locator - pcl::search::Search::Ptr tree; - tree.reset (new pcl::search::OrganizedNeighbor ()); + 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 ()); @@ -172,60 +176,42 @@ namespace jsk_pcl_ros double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier - pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); - pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); - pcl::toPCLPointCloud2(*cloud, *pcl_cloud); - pcl_cloud_filtered->is_bigendian = pcl_cloud->is_bigendian; - pcl_cloud_filtered->fields = pcl_cloud->fields; - pcl_cloud_filtered->point_step = pcl_cloud->point_step; - pcl_cloud_filtered->data.resize (pcl_cloud->data.size ()); - pcl_cloud_filtered->width = pcl_cloud->width; - pcl_cloud_filtered->height = pcl_cloud->height; - pcl_cloud_filtered->row_step = pcl_cloud_filtered->point_step * pcl_cloud_filtered->width; - // Build a new cloud by neglecting outliers - int nr_p = 0; 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(&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(); + remove_point = (distances[cp] > distance_threshold); } - else + if (!remove_point) { - 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); + indices_filtered.push_back(indices[cp]); } - nr_p++; } - pcl::fromPCLPointCloud2(*pcl_cloud_filtered, *cloud_filtered); + pcl_indices_filtered->indices = indices_filtered; } else { - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(cloud); - sor.setMeanK (mean_k_); - sor.setStddevMulThresh (std_mul_); - sor.setNegative (negative_); - sor.filter (*cloud_filtered); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(pcl_cloud); + sor.setMeanK(mean_k_); + sor.setStddevMulThresh(std_mul_); + sor.setNegative(negative_); + sor.filter(pcl_indices_filtered->indices); } #else - 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); + sor.filter(pcl_indices_filtered->indices); #endif } @@ -239,14 +225,21 @@ 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; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*msg, *pcl_cloud); // filter - OrganizedStatisticalOutlierRemoval::filter(cloud, cloud_filtered, keep_organized); - - pcl::toROSMsg(*cloud_filtered, output); + pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices()); + pcl_indices_filtered->indices.resize(pcl_cloud->data.size()); + OrganizedStatisticalOutlierRemoval::filter(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 @@ -277,45 +270,34 @@ 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; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - pcl::fromROSMsg(*msg, *cloud); - cloud_filtered->points.resize(cloud->points.size()); - PointT nan_point; - nan_point.x = nan_point.y = nan_point.z = std::numeric_limits::quiet_NaN(); - std::fill(cloud_filtered->points.begin(), cloud_filtered->points.end(), nan_point); + pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2); + pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*msg, *pcl_cloud); + // fill nan in pcl_cloud_filtered + pcl_cloud_filtered->data.resize(pcl_cloud->data.size()); + std::fill(pcl_cloud_filtered->data.begin(), pcl_cloud_filtered->data.end(), 0); - pcl::ExtractIndices ex; - ex.setInputCloud(cloud); + pcl::ExtractIndices ex; + ex.setInputCloud(pcl_cloud); ex.setKeepOrganized(keep_organized); ex.setNegative(false); for (size_t i = 0; i < cpi_msg->cluster_indices.size(); i++) { - pcl::IndicesPtr indices; - pcl::PointCloud::Ptr cluster_cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr cluster_cloud_filtered(new pcl::PointCloud); - indices.reset (new std::vector (cpi_msg->cluster_indices[i].indices)); - ex.setIndices(indices); - ex.filter(*cluster_cloud); - OrganizedStatisticalOutlierRemoval::filter(cluster_cloud, cluster_cloud_filtered, keep_organized); - for (size_t j = 0; j < indices->size(); j++) - { - int ind = (*indices)[j]; - if (!std::isnan(cluster_cloud_filtered->points[ind].x) && - !std::isnan(cluster_cloud_filtered->points[ind].y) && - !std::isnan(cluster_cloud_filtered->points[ind].z)) - { - cloud_filtered->points[ind].x = cluster_cloud_filtered->points[ind].x; - cloud_filtered->points[ind].y = cluster_cloud_filtered->points[ind].y; - cloud_filtered->points[ind].z = cluster_cloud_filtered->points[ind].z; - cloud_filtered->points[ind].rgb = cluster_cloud_filtered->points[ind].rgb; - } - } - + pcl::PCLPointCloud2::Ptr pcl_cluster_cloud (new pcl::PCLPointCloud2); + pcl::PCLPointCloud2::Ptr pcl_cluster_cloud_filtered (new pcl::PCLPointCloud2); + pcl::PointIndices::Ptr pcl_cluster_indices (new pcl::PointIndices()); + pcl::PointIndices::Ptr pcl_cluster_indices_filtered (new pcl::PointIndices()); + pcl_conversions::toPCL(cpi_msg->cluster_indices[i], *pcl_cluster_indices); + ex.setIndices(pcl_cluster_indices); + ex.filter(*pcl_cluster_cloud); + pcl_cluster_indices_filtered->indices.resize(pcl_cluster_cloud->data.size()); + OrganizedStatisticalOutlierRemoval::filter(pcl_cluster_cloud, pcl_cluster_indices_filtered, keep_organized); + ex.setIndices(pcl_cluster_indices_filtered); + ex.filter(*pcl_cluster_cloud_filtered); + pcl::PCLPointCloud2::concatenate(*pcl_cloud_filtered, *pcl_cluster_cloud_filtered); } - - pcl::toROSMsg(*cloud_filtered, output); + pcl_conversions::fromPCL(*pcl_cloud_filtered, output); #if PCL_VERSION_COMPARE (<, 1, 9, 0) if (keep_organized) { // Copy the common fields