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)
103 sq_sum += distance * distance;
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);