Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Inherit StatisticalOutlierRemoval<PCLPointCloud2> from FilterIndices #1663

Merged
merged 3 commits into from
Sep 2, 2017
Merged

Inherit StatisticalOutlierRemoval<PCLPointCloud2> from FilterIndices #1663

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
16c4f09
Inherit from FilterIndices and implement applyFilter(std::vector<int>)
Jul 28, 2016
a4e0157
Move duplicate code into seperate method.
Jul 28, 2016
d4959ca
Remove c++11 stuff.
stefanbuettner Jul 28, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 18 additions & 29 deletions filters/include/pcl/filters/statistical_outlier_removal.h
View file
Open in desktop
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
Open in desktop
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