From 762da269a2302f74c54d6807460ffbcf7e969cc9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BCttner?= Date: Sat, 2 Sep 2017 17:40:57 +0200 Subject: [PATCH] Organized Statistical Outlier Removal PCLPointCloud2 (#1663) Inherit from FilterIndices and implement applyFilter, move duplicate code --- .../pcl/filters/statistical_outlier_removal.h | 47 ++--- filters/src/statistical_outlier_removal.cpp | 189 +++++++++++++----- 2 files changed, 152 insertions(+), 84 deletions(-) diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 2aeb4153e30..3dcad2df23f 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,19 @@ 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); - void + virtual void applyFilter (PCLPointCloud2 &output); + + /** + * \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 adf59d849f4..2f8ab7d9ba0 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,133 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 output.data.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 + + // 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_) + { + 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 + } + + 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 (extract_removed_indices_) + (*removed_indices_)[nr_removed_p++] = cp; + + if (keep_organized_) + { + /* 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 + { + 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.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; + } + + 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); @@ -81,7 +209,7 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 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) @@ -116,62 +244,13 @@ pcl::StatisticalOutlierRemoval::applyFilter (PCLPointCloud2 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 - - // Copy the common fields - output.is_bigendian = input_->is_bigendian; - output.point_step = input_->point_step; - output.height = 1; - - 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; - for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) - { - if (negative_) - { - if (distances[cp] <= distance_threshold) - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - 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++; - } - 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); + 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