From ca47dd2dbd790cb5daac5bdad0cf413c4210fc2e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 4 Dec 2024 17:56:11 -0800 Subject: [PATCH] support melodic --- .../organized_statistical_outlier_removal.h | 19 +- ...ed_statistical_outlier_removal_nodelet.cpp | 170 +++++++++--------- 2 files changed, 94 insertions(+), 95 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..400ebdcd03 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 @@ -42,6 +42,13 @@ #include #include #include +#include +#include +#include + +#include "sensor_msgs/PointCloud2.h" +#include "jsk_recognition_msgs/ClusterPointIndices.h" + #include "jsk_pcl_ros/OrganizedStatisticalOutlierRemovalConfig.h" namespace jsk_pcl_ros @@ -63,13 +70,13 @@ 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); + void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud, + pcl::PointIndices::Ptr pcl_indices_filtered); + std::vector getFilteredIndices(const pcl::PointCloud::Ptr cloud); + void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& 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 2cccba749f..03407d632c 100644 --- a/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp +++ b/jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp @@ -63,7 +63,7 @@ namespace jsk_pcl_ros void OrganizedStatisticalOutlierRemoval::subscribe() { - sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filter, this); + sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filterCloud, this); } void OrganizedStatisticalOutlierRemoval::unsubscribe() @@ -80,107 +80,102 @@ namespace jsk_pcl_ros std_mul_ = config.stddev; } - void OrganizedStatisticalOutlierRemoval::filterCloud( + void OrganizedStatisticalOutlierRemoval::filter( const pcl::PCLPointCloud2::Ptr pcl_cloud, - pcl::PointIndices::Ptr pcl_indices_filtered, - bool keep_organized) + pcl::PointIndices::Ptr pcl_indices_filtered) { -#if PCL_VERSION_COMPARE (<, 1, 9, 0) - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + 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 ()); - tree->setInputCloud (cloud); - - // 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 ()); - int valid_distances = 0; - - for (size_t i = 0; i < indices.size (); ++i) indices[i] = i; - - // Go over all the points and calculate the mean or smallest distance - for (size_t cp = 0; cp < indices.size (); ++cp) + pcl_indices_filtered->indices = OrganizedStatisticalOutlierRemoval::getFilteredIndices(cloud); + } + + std::vector OrganizedStatisticalOutlierRemoval::getFilteredIndices( + const pcl::PointCloud::Ptr cloud) + { + std::vector indices_filtered; + indices_filtered.resize(cloud->size()); +#if PCL_VERSION_COMPARE (<, 1, 9, 0) + // 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 nn_indices (mean_k_); + std::vector nn_dists (mean_k_); + std::vector distances (indices.size ()); + int valid_distances = 0; + + for (size_t i = 0; i < indices.size (); ++i) indices[i] = i; + + // Go over all the points and calculate the mean or smallest distance + for (size_t cp = 0; cp < indices.size (); ++cp) + { + if (!pcl_isfinite (cloud->points[indices[cp]].x) || + !pcl_isfinite (cloud->points[indices[cp]].y) || + !pcl_isfinite (cloud->points[indices[cp]].z)) { - if (!pcl_isfinite (cloud->points[indices[cp]].x) || - !pcl_isfinite (cloud->points[indices[cp]].y) || - !pcl_isfinite (cloud->points[indices[cp]].z)) - { - distances[cp] = 0; - continue; - } - if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0) - { - distances[cp] = 0; - continue; - } - // Minimum distance (if mean_k_ == 2) or mean distance - double dist_sum = 0; - for (int j = 1; j < mean_k_; ++j) - dist_sum += sqrt (nn_dists[j]); - distances[cp] = static_cast (dist_sum / (mean_k_ - 1)); - valid_distances++; + distances[cp] = 0; + continue; } - - // Estimate the mean and the standard deviation of the distance vector - double sum = 0, sq_sum = 0; - for (size_t i = 0; i < distances.size (); ++i) + if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0) { - sum += distances[i]; - sq_sum += distances[i] * distances[i]; + distances[cp] = 0; + continue; } - double mean = sum / static_cast(valid_distances); - double variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); - double stddev = sqrt (variance); - //getMeanStd (distances, mean, stddev); + // Minimum distance (if mean_k_ == 2) or mean distance + double dist_sum = 0; + for (int j = 1; j < mean_k_; ++j) + dist_sum += sqrt (nn_dists[j]); + distances[cp] = static_cast (dist_sum / (mean_k_ - 1)); + valid_distances++; + } - double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier + // Estimate the mean and the standard deviation of the distance vector + double sum = 0, sq_sum = 0; + for (size_t i = 0; i < distances.size (); ++i) + { + sum += distances[i]; + sq_sum += distances[i] * distances[i]; + } + double mean = sum / static_cast(valid_distances); + double variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); + double stddev = sqrt (variance); + //getMeanStd (distances, mean, stddev); + + double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier - // Build a new cloud by neglecting outliers - for (int cp = 0; cp < static_cast (indices.size ()); ++cp) + // Build a new cloud by neglecting outliers + for (int cp = 0; cp < static_cast (indices.size ()); ++cp) + { + bool remove_point = false; + if (negative_) { - bool remove_point = false; - if (negative_) - { - remove_point = (distances[cp] <= distance_threshold); - } - else - { - remove_point = (distances[cp] > distance_threshold); - } - if (!remove_point) - { - indices_filtered.push_back(indices[cp]); - } + remove_point = (distances[cp] <= distance_threshold); + } + else + { + remove_point = (distances[cp] > distance_threshold); + } + if (!remove_point) + { + indices_filtered.push_back(indices[cp]); } - pcl_indices_filtered->indices = indices_filtered; - } - else - { - 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(pcl_cloud); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud); sor.setMeanK(mean_k_); sor.setStddevMulThresh(std_mul_); sor.setNegative(negative_); - sor.setKeepOrganized (keep_organized); - sor.filter(pcl_indices_filtered->indices); + sor.setKeepOrganized (true); + sor.filter(indices_filtered); #endif + return indices_filtered; } - void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg) + void OrganizedStatisticalOutlierRemoval::filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg) { boost::mutex::scoped_lock lock(mutex_); vital_checker_->poke(); @@ -195,10 +190,7 @@ namespace jsk_pcl_ros // 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::filter(pcl_cloud, pcl_indices_filtered); pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2); pcl::ExtractIndices ex; ex.setInputCloud(pcl_cloud); @@ -213,13 +205,13 @@ namespace jsk_pcl_ros output.is_bigendian = msg->is_bigendian; output.fields = msg->fields; output.point_step = msg->point_step; - output.data.resize (msg->data.size ()); + 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.row_step = output.point_step * output.width; output.is_dense = !keep_organized; pub_.publish(output); diagnostic_updater_->update();