Skip to content

Commit

Permalink
Organized Statistical Outlier Removal PCLPointCloud2 (#1663)
Browse files Browse the repository at this point in the history 
Inherit from FilterIndices and implement applyFilter, move duplicate code
  • Loading branch information
@stefanbuettner @taketwo
stefanbuettner authored and taketwo committed Sep 2, 2017
1 parent 5cc08f8 commit 512f3a3
Show file tree
Hide file tree
Showing 2 changed files with 152 additions and 84 deletions.
47 changes: 18 additions & 29 deletions filters/include/pcl/filters/statistical_outlier_removal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,13 +198,13 @@ namespace pcl
* \ingroup filters
*/
template<>
class PCL_EXPORTS StatisticalOutlierRemoval<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
class PCL_EXPORTS StatisticalOutlierRemoval<pcl::PCLPointCloud2> : public FilterIndices<pcl::PCLPointCloud2>
{
using Filter<pcl::PCLPointCloud2>::filter_name_;
using Filter<pcl::PCLPointCloud2>::getClassName;
using FilterIndices<pcl::PCLPointCloud2>::filter_name_;
using FilterIndices<pcl::PCLPointCloud2>::getClassName;

using Filter<pcl::PCLPointCloud2>::removed_indices_;
using Filter<pcl::PCLPointCloud2>::extract_removed_indices_;
using FilterIndices<pcl::PCLPointCloud2>::removed_indices_;
using FilterIndices<pcl::PCLPointCloud2>::extract_removed_indices_;

typedef pcl::search::Search<pcl::PointXYZ> KdTree;
typedef pcl::search::Search<pcl::PointXYZ>::Ptr KdTreePtr;
Expand All @@ -216,8 +216,8 @@ namespace pcl
public:
/** \brief Empty constructor. */
StatisticalOutlierRemoval (bool extract_removed_indices = false) :
Filter<pcl::PCLPointCloud2>::Filter (extract_removed_indices), mean_k_ (2),
std_mul_ (0.0), tree_ (), negative_ (false)
FilterIndices<pcl::PCLPointCloud2>::FilterIndices (extract_removed_indices), mean_k_ (2),
std_mul_ (0.0), tree_ ()
{
filter_name_ = "StatisticalOutlierRemoval";
}
Expand Down Expand Up @@ -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_;
Expand All @@ -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<int> &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<float>& distances);
};
}

Expand Down
189 changes: 134 additions & 55 deletions filters/src/statistical_outlier_removal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,11 +41,12 @@
#include <pcl/filters/impl/statistical_outlier_removal.hpp>
#include <pcl/conversions.h>

using namespace std;

///////////////////////////////////////////////////////////////////////////////////////////
void
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::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)
{
Expand All @@ -62,6 +63,133 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
output.data.clear ();
return;
}

double mean;
double variance;
double stddev;
vector<float> 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<int> (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<float*>(&output.data[nr_p * output.point_step])+0) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&output.data[nr_p * output.point_step])+1) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&output.data[nr_p * output.point_step])+2) = std::numeric_limits<float>::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<pcl::PCLPointCloud2>::applyFilter (vector<int>& 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<float> 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<pcl::PCLPointCloud2>::generateStatistics (double& mean,
double& variance,
double& stddev,
std::vector<float>& distances)
{
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*input_, *cloud);
Expand All @@ -81,7 +209,7 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);

std::vector<float> 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)
Expand Down Expand Up @@ -116,62 +244,13 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
sum += distances[i];
sq_sum += distances[i] * distances[i];
}
double mean = sum / static_cast<double>(valid_distances);
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(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<int> (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<double>(valid_distances);
variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
stddev = sqrt (variance);
}


#ifndef PCL_NO_PRECOMPILE
#include <pcl/impl/instantiate.hpp>
#include <pcl/point_types.h>
Expand Down

0 comments on commit 512f3a3

Please sign in to comment.