template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
{
// Initialize the search class
if (!searcher_)
{
if (input_->isOrganized ())
searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
searcher_.reset (new pcl::search::KdTree<PointT> (false));
}
searcher_->setInputCloud (input_);
// The arrays to be used
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices_->size ());
indices.resize (indices_->size ());
removed_indices_->resize (indices_->size ());
int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
// First pass: Compute the mean distances for all points with respect to their k nearest neighbors
for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
{
if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
!pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
!pcl_isfinite (input_->points[(*indices_)[iii]].z))
{
distances[iii] = 0.0;
continue;
}
// Perform the nearest k search
if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
{
distances[iii] = 0.0;
PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
continue;
}
// Calculate the mean distance to its neighbors
double dist_sum = 0.0;
for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point
dist_sum += sqrt (nn_dists[k]);
distances[iii] = static_cast<float> (dist_sum / mean_k_);
}
// Estimate the mean and the standard deviation of the distance vector
double mean, stddev;
getMeanStd (distances, mean, stddev);
double distance_threshold = mean + std_mul_ * stddev;
// Second pass: Classify the points on the computed distance threshold
for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
{
// 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[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
{
if (extract_removed_indices_)
(*removed_indices_)[rii++] = (*indices_)[iii];
continue;
}
// Otherwise it was a normal point for output (inlier)
indices[oii++] = (*indices_)[iii];
}
// Resize the output arrays
indices.resize (oii);
removed_indices_->resize (rii);
}
void
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &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)
{
ROS_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
if (std_mul_ == 0.0)
{
ROS_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// Initialize the spatial locator
//initTree (spatial_locator_type_, tree_, k_);
// TODO: fix this
tree_.reset (new pcl::KdTreeFLANN<pcl::PointXYZ>);
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input_, *cloud);
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 ());
// 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;
ROS_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.", 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] = dist_sum / (mean_k_-1);
}
// Estimate the mean and the standard deviation of the distance vector
double mean, stddev;
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
// Build a new cloud by neglecting outliers
int nr_p = 0;
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
if (negative_)
{
if (distances[cp] <= distance_threshold)
continue;
}
else
{
if (distances[cp] > distance_threshold)
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;
}
template <typename PointT> void
pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
{
if (std_mul_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multiplier not set!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (input_->points.empty ())
{
output.width = output.height = 0;
output.points.clear ();
return;
}
// Initialize the spatial locator
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
else
tree_.reset (new pcl::search::KdTree<PointT> (false));
}
// Send the input dataset to the spatial locator
tree_->setInputCloud (input_);
// 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 ());
// Go over all the points and calculate the mean or smallest distance
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
!pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
!pcl_isfinite (input_->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));
}
// Estimate the mean and the standard deviation of the distance vector
double mean, stddev;
getMeanStd (distances, mean, stddev);
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
output.points.resize (input_->points.size ()); // reserve enough space
removed_indices_->resize (input_->points.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;
}
}
output.points[nr_p++] = input_->points[(*indices_)[cp]];
}
output.points.resize (nr_p);
output.width = nr_p;
output.height = 1;
output.is_dense = true; // nearestKSearch filters invalid points
removed_indices_->resize (nr_removed_p);
}
