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/organized.h>
45 #include <pcl/search/kdtree.h>
48 template <
typename Po
intT>
void
54 if (input_->isOrganized ())
59 searcher_->setInputCloud (input_);
63 std::vector<float> nn_dists (mean_k_);
64 std::vector<float> distances (indices_->size ());
65 indices.resize (indices_->size ());
66 removed_indices_->resize (indices_->size ());
70 int valid_distances = 0;
71 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
73 if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
74 !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
75 !std::isfinite ((*input_)[(*indices_)[iii]].z))
82 if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
85 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
90 double dist_sum = 0.0;
91 for (
int k = 1; k < mean_k_ + 1; ++k)
92 dist_sum += sqrt (nn_dists[k]);
93 distances[iii] =
static_cast<float> (dist_sum / mean_k_);
98 double sum = 0, sq_sum = 0;
99 for (
const float &
distance : distances)
104 double mean = sum /
static_cast<double>(valid_distances);
105 double variance = (sq_sum - sum * sum /
static_cast<double>(valid_distances)) / (
static_cast<double>(valid_distances) - 1);
106 double stddev = sqrt (variance);
109 double distance_threshold = mean + std_mul_ * stddev;
112 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
116 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
118 if (extract_removed_indices_)
119 (*removed_indices_)[rii++] = (*indices_)[iii];
124 indices[oii++] = (*indices_)[iii];
128 indices.resize (oii);
129 removed_indices_->resize (rii);
132 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
134 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_