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 50 std::vector<int> indices;
53 bool temp = extract_removed_indices_;
54 extract_removed_indices_ =
true;
55 applyFilterIndices (indices);
56 extract_removed_indices_ = temp;
59 for (
int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)
60 output.
points[(*removed_indices_)[rii]].x = output.
points[(*removed_indices_)[rii]].y = output.
points[(*removed_indices_)[rii]].z = user_filter_value_;
61 if (!pcl_isfinite (user_filter_value_))
66 applyFilterIndices (indices);
72 template <
typename Po
intT>
void 78 if (input_->isOrganized ())
83 searcher_->setInputCloud (input_);
86 std::vector<int> nn_indices (mean_k_);
87 std::vector<float> nn_dists (mean_k_);
88 std::vector<float> distances (indices_->size ());
89 indices.resize (indices_->size ());
90 removed_indices_->resize (indices_->size ());
94 int valid_distances = 0;
95 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
97 if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
98 !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
99 !pcl_isfinite (input_->points[(*indices_)[iii]].z))
101 distances[iii] = 0.0;
106 if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
108 distances[iii] = 0.0;
109 PCL_WARN (
"[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
114 double dist_sum = 0.0;
115 for (
int k = 1; k < mean_k_ + 1; ++k)
116 dist_sum += sqrt (nn_dists[k]);
117 distances[iii] = static_cast<float> (dist_sum / mean_k_);
122 double sum = 0, sq_sum = 0;
123 for (
size_t i = 0; i < distances.size (); ++i)
126 sq_sum += distances[i] * distances[i];
128 double mean = sum / static_cast<double>(valid_distances);
129 double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
130 double stddev = sqrt (variance);
133 double distance_threshold = mean + std_mul_ * stddev;
136 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
140 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
142 if (extract_removed_indices_)
143 (*removed_indices_)[rii++] = (*indices_)[iii];
148 indices[oii++] = (*indices_)[iii];
152 indices.resize (oii);
153 removed_indices_->resize (rii);
156 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>; 158 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void applyFilter(PointCloud &output)
Filtered results are stored in a separate point cloud.
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.