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_);
62 const int searcher_k = mean_k_ + 1;
63 Indices nn_indices (searcher_k);
64 std::vector<float> nn_dists (searcher_k);
65 std::vector<float> distances (indices_->size ());
66 indices.resize (indices_->size ());
67 removed_indices_->resize (indices_->size ());
71 int valid_distances = 0;
72 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
74 if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
75 !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
76 !std::isfinite ((*input_)[(*indices_)[iii]].z))
83 if (searcher_->nearestKSearch ((*indices_)[iii], searcher_k, nn_indices, nn_dists) == 0)
86 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
91 double dist_sum = 0.0;
92 for (
int k = 1; k < searcher_k; ++k)
93 dist_sum += sqrt (nn_dists[k]);
94 distances[iii] =
static_cast<float> (dist_sum / mean_k_);
99 double sum = 0, sq_sum = 0;
100 for (
const float &
distance : distances)
105 double mean = sum /
static_cast<double>(valid_distances);
106 double variance = (sq_sum - sum * sum /
static_cast<double>(valid_distances)) / (
static_cast<double>(valid_distances) - 1);
107 double stddev = sqrt (variance);
110 double distance_threshold = mean + std_mul_ * stddev;
113 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
117 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
119 if (extract_removed_indices_)
120 (*removed_indices_)[rii++] = (*indices_)[iii];
125 indices[oii++] = (*indices_)[iii];
129 indices.resize (oii);
130 removed_indices_->resize (rii);
133 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
void applyFilterIndices(Indices &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 neighbor search in organized point clouds.
float distance(const PointT &p1, const PointT &p2)
IndicesAllocator<> Indices
Type used for indices in PCL.