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>
44 #include <pcl/search/auto.h>
47 template <
typename Po
intT>
void
55 else if (!searcher_->setInputCloud (input_))
57 PCL_ERROR (
"[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ());
59 removed_indices_->clear ();
64 const int searcher_k = mean_k_ + 1;
65 Indices nn_indices (searcher_k);
66 std::vector<float> nn_dists (searcher_k);
67 std::vector<float> distances (indices_->size ());
68 indices.resize (indices_->size ());
69 removed_indices_->resize (indices_->size ());
73 int valid_distances = 0;
74 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
76 if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
77 !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
78 !std::isfinite ((*input_)[(*indices_)[iii]].z))
85 if (searcher_->nearestKSearch ((*indices_)[iii], searcher_k, nn_indices, nn_dists) == 0)
88 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
93 double dist_sum = 0.0;
94 for (std::size_t k = 1; k < nn_dists.size(); ++k)
95 dist_sum += sqrt(nn_dists[k]);
96 distances[iii] =
static_cast<float>(dist_sum / (nn_dists.size() - 1));
101 double sum = 0, sq_sum = 0;
102 for (
const float &
distance : distances)
107 double mean = sum /
static_cast<double>(valid_distances);
108 double variance = (sq_sum - sum * sum /
static_cast<double>(valid_distances)) / (
static_cast<double>(valid_distances) - 1);
109 double stddev = sqrt (variance);
112 double distance_threshold = mean + std_mul_ * stddev;
115 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
119 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
121 if (extract_removed_indices_)
122 (*removed_indices_)[rii++] = (*indices_)[iii];
127 indices[oii++] = (*indices_)[iii];
131 indices.resize (oii);
132 removed_indices_->resize (rii);
135 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
float distance(const PointT &p1, const PointT &p2)
@ many_knn_search
The search method will mainly be used for nearestKSearch where k is larger than 1.
IndicesAllocator<> Indices
Type used for indices in PCL.