Skip to content

Commit

Permalink
Move duplicate code into seperate method.
Browse files Browse the repository at this point in the history
  • Loading branch information
Stefan Büttner authored and Stefan Büttner committed Jul 28, 2016
1 parent bfc0223 commit a6c59cf
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 89 deletions.
8 changes: 8 additions & 0 deletions filters/include/pcl/filters/statistical_outlier_removal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>& distances);
};
}

Expand Down
138 changes: 49 additions & 89 deletions filters/src/statistical_outlier_removal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,70 +63,16 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
output.data.clear ();
return;
}
output.is_dense = true;

// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*input_, *cloud);

// Initialize the spatial locator
if (!tree_)
{
if (cloud->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
else
tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
}

tree_->setInputCloud (cloud);

// Allocate enough space to hold the results
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);

std::vector<float> 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<float> (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<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
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_)
Expand Down Expand Up @@ -189,7 +135,6 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2
removed_indices_->resize (nr_removed_p);
}


///////////////////////////////////////////////////////////////////////////////////////////
void
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (vector<int>& indices)
Expand All @@ -208,6 +153,43 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (vector<int>& i
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 @@ -227,7 +209,7 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (vector<int>& i
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 @@ -262,35 +244,13 @@ pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (vector<int>& i
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

// 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<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 a6c59cf

Please sign in to comment.