40 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
41 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
43 #include <pcl/filters/statistical_outlier_removal.h>
46 template <
typename Po
intT>
void
52 if (input_->isOrganized ())
57 searcher_->setInputCloud (input_);
60 std::vector<int> nn_indices (mean_k_);
61 std::vector<float> nn_dists (mean_k_);
62 std::vector<float> distances (indices_->size ());
63 indices.resize (indices_->size ());
64 removed_indices_->resize (indices_->size ());
68 int valid_distances = 0;
69 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
71 if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
72 !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
73 !std::isfinite ((*input_)[(*indices_)[iii]].z))
80 if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
83 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
88 double dist_sum = 0.0;
89 for (
int k = 1; k < mean_k_ + 1; ++k)
90 dist_sum += sqrt (nn_dists[k]);
91 distances[iii] =
static_cast<float> (dist_sum / mean_k_);
96 double sum = 0, sq_sum = 0;
97 for (
const float &
distance : distances)
102 double mean = sum /
static_cast<double>(valid_distances);
103 double variance = (sq_sum - sum * sum /
static_cast<double>(valid_distances)) / (
static_cast<double>(valid_distances) - 1);
104 double stddev = sqrt (variance);
107 double distance_threshold = mean + std_mul_ * stddev;
110 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
114 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
116 if (extract_removed_indices_)
117 (*removed_indices_)[rii++] = (*indices_)[iii];
122 indices[oii++] = (*indices_)[iii];
126 indices.resize (oii);
127 removed_indices_->resize (rii);
130 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
float distance(const PointT &p1, const PointT &p2)