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/common/io.h>
47 template <
typename Po
intT>
void
53 if (input_->isOrganized ())
58 searcher_->setInputCloud (input_);
61 std::vector<int> nn_indices (mean_k_);
62 std::vector<float> nn_dists (mean_k_);
63 std::vector<float> distances (indices_->size ());
64 indices.resize (indices_->size ());
65 removed_indices_->resize (indices_->size ());
69 int valid_distances = 0;
70 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
72 if (!std::isfinite (input_->points[(*indices_)[iii]].x) ||
73 !std::isfinite (input_->points[(*indices_)[iii]].y) ||
74 !std::isfinite (input_->points[(*indices_)[iii]].z))
81 if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
84 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
89 double dist_sum = 0.0;
90 for (
int k = 1; k < mean_k_ + 1; ++k)
91 dist_sum += sqrt (nn_dists[k]);
92 distances[iii] =
static_cast<float> (dist_sum / mean_k_);
97 double sum = 0, sq_sum = 0;
98 for (
const float &distance : distances)
101 sq_sum += distance * distance;
103 double mean = sum /
static_cast<double>(valid_distances);
104 double variance = (sq_sum - sum * sum /
static_cast<double>(valid_distances)) / (
static_cast<double>(valid_distances) - 1);
105 double stddev = sqrt (variance);
108 double distance_threshold = mean + std_mul_ * stddev;
111 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
115 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
117 if (extract_removed_indices_)
118 (*removed_indices_)[rii++] = (*indices_)[iii];
123 indices[oii++] = (*indices_)[iii];
127 indices.resize (oii);
128 removed_indices_->resize (rii);
131 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
133 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...