From 16c4f099780cf1f0324d5511a932f12e0596b859 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BCttner?= Date: Thu, 28 Jul 2016 17:06:57 +0200 Subject: [PATCH 1/3] Inherit from FilterIndices and implement applyFilter(std::vector) --- .../pcl/filters/statistical_outlier_removal.h | 41 ++--- filters/src/statistical_outlier_removal.cpp | 167 +++++++++++++++--- 2 files changed, 154 insertions(+), 54 deletions(-) diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 2aeb4153e30..6cd9c750d73 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -198,13 +198,13 @@ namespace pcl * \ingroup filters */ template<> - class PCL_EXPORTS StatisticalOutlierRemoval : public Filter + class PCL_EXPORTS StatisticalOutlierRemoval : public FilterIndices { - using Filter::filter_name_; - using Filter::getClassName; + using FilterIndices::filter_name_; + using FilterIndices::getClassName; - using Filter::removed_indices_; - using Filter::extract_removed_indices_; + using FilterIndices::removed_indices_; + using FilterIndices::extract_removed_indices_; typedef pcl::search::Search KdTree; typedef pcl::search::Search::Ptr KdTreePtr; @@ -216,8 +216,8 @@ namespace pcl public: /** \brief Empty constructor. */ StatisticalOutlierRemoval (bool extract_removed_indices = false) : - Filter::Filter (extract_removed_indices), mean_k_ (2), - std_mul_ (0.0), tree_ (), negative_ (false) + FilterIndices::FilterIndices (extract_removed_indices), mean_k_ (2), + std_mul_ (0.0), tree_ () { filter_name_ = "StatisticalOutlierRemoval"; } @@ -257,25 +257,6 @@ namespace pcl return (std_mul_); } - /** \brief Set whether the indices should be returned, or all points \e except the indices. - * \param negative true if all points \e except the input indices will be returned, false otherwise - */ - inline void - setNegative (bool negative) - { - negative_ = negative; - } - - /** \brief Get the value of the internal #negative_ parameter. If - * true, all points \e except the input indices will be returned. - * \return The value of the "negative" flag - */ - inline bool - getNegative () - { - return (negative_); - } - protected: /** \brief The number of points to use for mean distance estimation. */ int mean_k_; @@ -288,11 +269,11 @@ namespace pcl /** \brief A pointer to the spatial search object. */ KdTreePtr tree_; - /** \brief If true, the outliers will be returned instead of the inliers (default: false). */ - bool negative_; + virtual void + applyFilter (std::vector &indices) override; - void - applyFilter (PCLPointCloud2 &output); + virtual void + applyFilter (PCLPointCloud2 &output) override; }; } diff --git a/filters/src/statistical_outlier_removal.cpp b/filters/src/statistical_outlier_removal.cpp index adf59d849f4..a8bc79b8c2b 100644 --- a/filters/src/statistical_outlier_removal.cpp +++ b/filters/src/statistical_outlier_removal.cpp @@ -41,11 +41,12 @@ #include #include +using namespace std; + /////////////////////////////////////////////////////////////////////////////////////////// void pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 &output) { - output.is_dense = true; // If fields x/y/z are not present, we cannot filter if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) { @@ -62,6 +63,8 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 output.data.clear (); return; } + output.is_dense = true; + // Send the input dataset to the spatial locator pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::fromPCLPointCloud2 (*input_, *cloud); @@ -126,52 +129,168 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 // Copy the common fields output.is_bigendian = input_->is_bigendian; output.point_step = input_->point_step; - output.height = 1; + if (keep_organized_) + { + output.width = input_->width; + output.height = input_->height; + output.data.resize (input_->data.size ()); + } + else + { + output.height = 1; + output.data.resize (indices_->size () * input_->point_step); // reserve enough space + } - output.data.resize (indices_->size () * input_->point_step); // reserve enough space removed_indices_->resize (input_->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) { if (negative_) + remove_point = (distances[cp] <= distance_threshold); + else + remove_point = (distances[cp] > distance_threshold); + + if (remove_point) { - if (distances[cp] <= distance_threshold) + if (extract_removed_indices_) + (*removed_indices_)[nr_removed_p++] = cp; + + if (keep_organized_) { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - continue; + /* 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++; + output.is_dense = false; } + else + continue; } else { - if (distances[cp] > distance_threshold) - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - continue; - } + memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step], + output.point_step); + nr_p++; } + } - memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step], - output.point_step); - nr_p++; + if (!keep_organized_) + { + output.width = nr_p; + output.data.resize (output.width * output.point_step); } - output.width = nr_p; - output.data.resize (output.width * output.point_step); output.row_step = output.point_step * output.width; removed_indices_->resize (nr_removed_p); } + +/////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::StatisticalOutlierRemoval::applyFilter (vector& indices) +{ + // If fields x/y/z are not present, we cannot filter + if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) + { + PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ()); + indices.clear(); + return; + } + + if (std_mul_ == 0.0) + { + PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!\n", getClassName ().c_str ()); + indices.clear(); + return; + } + // Send the input dataset to the spatial locator + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (*input_, *cloud); + + // Initialize the spatial locator + if (!tree_) + { + if (cloud->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + tree_->setInputCloud (cloud); + + // Allocate enough space to hold the results + std::vector nn_indices (mean_k_); + std::vector nn_dists (mean_k_); + + std::vector distances (indices_->size ()); + int valid_distances = 0; + // 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)) + { + distances[cp] = 0; + continue; + } + + if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0) + { + distances[cp] = 0; + PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_); + 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++; + } + + // 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 + + // Second pass: Classify the points on the computed distance threshold + size_t nr_p = 0, nr_removed_p = 0; + for (size_t cp = 0; cp < indices_->size (); ++cp) + { + // Points having a too high average distance are outliers and are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && distances[cp] > distance_threshold) || (negative_ && distances[cp] <= distance_threshold)) + { + if (extract_removed_indices_) + (*removed_indices_)[nr_removed_p++] = (*indices_)[cp]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[nr_p++] = (*indices_)[cp]; + } + + // Resize the output arrays + indices.resize (nr_p); + removed_indices_->resize (nr_p); +} + #ifndef PCL_NO_PRECOMPILE #include #include From a4e01577a69198754a346d30c0bc48970e30b604 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BCttner?= Date: Thu, 28 Jul 2016 17:26:34 +0200 Subject: [PATCH 2/3] Move duplicate code into seperate method. --- .../pcl/filters/statistical_outlier_removal.h | 8 + filters/src/statistical_outlier_removal.cpp | 138 +++++++----------- 2 files changed, 57 insertions(+), 89 deletions(-) diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 6cd9c750d73..f76196d848b 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -274,6 +274,14 @@ namespace pcl virtual void applyFilter (PCLPointCloud2 &output) override; + + /** + * \brief Compute the statistical values used in both applyFilter methods. + * + * This method tries to avoid duplicate code. + */ + virtual void + generateStatistics (double& mean, double& variance, double& stddev, std::vector& distances); }; } diff --git a/filters/src/statistical_outlier_removal.cpp b/filters/src/statistical_outlier_removal.cpp index a8bc79b8c2b..2f8ab7d9ba0 100644 --- a/filters/src/statistical_outlier_removal.cpp +++ b/filters/src/statistical_outlier_removal.cpp @@ -63,70 +63,16 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 output.data.clear (); return; } - output.is_dense = true; - // Send the input dataset to the spatial locator - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::fromPCLPointCloud2 (*input_, *cloud); - - // Initialize the spatial locator - if (!tree_) - { - if (cloud->isOrganized ()) - tree_.reset (new pcl::search::OrganizedNeighbor ()); - else - tree_.reset (new pcl::search::KdTree (false)); - } - - tree_->setInputCloud (cloud); - - // Allocate enough space to hold the results - std::vector nn_indices (mean_k_); - std::vector nn_dists (mean_k_); - - std::vector distances (indices_->size ()); - int valid_distances = 0; - // 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)) - { - distances[cp] = 0; - continue; - } - - if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0) - { - distances[cp] = 0; - PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_); - 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++; - } - - // 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 + double mean; + double variance; + double stddev; + vector distances; + generateStatistics (mean, variance, stddev, distances); + double const distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier // Copy the common fields + output.is_dense = input_->is_dense; output.is_bigendian = input_->is_bigendian; output.point_step = input_->point_step; if (keep_organized_) @@ -189,7 +135,6 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 removed_indices_->resize (nr_removed_p); } - /////////////////////////////////////////////////////////////////////////////////////////// void pcl::StatisticalOutlierRemoval::applyFilter (vector& indices) @@ -208,6 +153,43 @@ pcl::StatisticalOutlierRemoval::applyFilter (vector& i indices.clear(); return; } + + double mean; + double variance; + double stddev; + vector distances; + generateStatistics(mean, variance, stddev, distances); + double const distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier + + // Second pass: Classify the points on the computed distance threshold + size_t nr_p = 0, nr_removed_p = 0; + for (size_t cp = 0; cp < indices_->size (); ++cp) + { + // Points having a too high average distance are outliers and are passed to removed indices + // Unless negative was set, then it's the opposite condition + if ((!negative_ && distances[cp] > distance_threshold) || (negative_ && distances[cp] <= distance_threshold)) + { + if (extract_removed_indices_) + (*removed_indices_)[nr_removed_p++] = (*indices_)[cp]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[nr_p++] = (*indices_)[cp]; + } + + // Resize the output arrays + indices.resize (nr_p); + removed_indices_->resize (nr_p); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::StatisticalOutlierRemoval::generateStatistics (double& mean, + double& variance, + double& stddev, + std::vector& distances) +{ // Send the input dataset to the spatial locator pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::fromPCLPointCloud2 (*input_, *cloud); @@ -227,7 +209,7 @@ pcl::StatisticalOutlierRemoval::applyFilter (vector& i std::vector nn_indices (mean_k_); std::vector nn_dists (mean_k_); - std::vector distances (indices_->size ()); + distances.resize (indices_->size ()); int valid_distances = 0; // Go over all the points and calculate the mean or smallest distance for (size_t cp = 0; cp < indices_->size (); ++cp) @@ -262,35 +244,13 @@ pcl::StatisticalOutlierRemoval::applyFilter (vector& 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 - - // Second pass: Classify the points on the computed distance threshold - size_t nr_p = 0, nr_removed_p = 0; - for (size_t cp = 0; cp < indices_->size (); ++cp) - { - // Points having a too high average distance are outliers and are passed to removed indices - // Unless negative was set, then it's the opposite condition - if ((!negative_ && distances[cp] > distance_threshold) || (negative_ && distances[cp] <= distance_threshold)) - { - if (extract_removed_indices_) - (*removed_indices_)[nr_removed_p++] = (*indices_)[cp]; - continue; - } - - // Otherwise it was a normal point for output (inlier) - indices[nr_p++] = (*indices_)[cp]; - } - // Resize the output arrays - indices.resize (nr_p); - removed_indices_->resize (nr_p); + mean = sum / static_cast(valid_distances); + variance = (sq_sum - sum * sum / static_cast(valid_distances)) / (static_cast(valid_distances) - 1); + stddev = sqrt (variance); } + #ifndef PCL_NO_PRECOMPILE #include #include From d4959cac613a12fc64ff0eb5cc6c6539b3cd1ba4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BCttner?= Date: Thu, 28 Jul 2016 23:33:00 +0200 Subject: [PATCH 3/3] Remove c++11 stuff. --- filters/include/pcl/filters/statistical_outlier_removal.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index f76196d848b..3dcad2df23f 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -270,10 +270,10 @@ namespace pcl KdTreePtr tree_; virtual void - applyFilter (std::vector &indices) override; + applyFilter (std::vector &indices); virtual void - applyFilter (PCLPointCloud2 &output) override; + applyFilter (PCLPointCloud2 &output); /** * \brief Compute the statistical values used in both applyFilter methods.