源代码
1 * 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2012, Willow Garage, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of the copyright holder(s) nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 * 36 * $Id$ 37 * 38 */ 39 40 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ 41 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ 42 43 #include <pcl/filters/statistical_outlier_removal.h> 44 #include <pcl/common/io.h> 45 46 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 47 template <typename PointT> void 48 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output) 49 { 50 std::vector<int> indices; 51 if (keep_organized_) 52 { 53 bool temp = extract_removed_indices_; 54 extract_removed_indices_ = true; 55 applyFilterIndices (indices); 56 extract_removed_indices_ = temp; 57 58 output = *input_; 59 for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator 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_)) 62 output.is_dense = false; 63 } 64 else 65 { 66 applyFilterIndices (indices); 67 copyPointCloud (*input_, indices, output); 68 } 69 } 70 71 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 72 template <typename PointT> void 73 pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices) 74 { 75 // Initialize the search class 76 if (!searcher_) 77 { 78 if (input_->isOrganized ()) 79 searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 80 else 81 searcher_.reset (new pcl::search::KdTree<PointT> (false)); 82 } 83 searcher_->setInputCloud (input_); 84 85 // The arrays to be used 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 ()); 91 int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator 92 93 // First pass: Compute the mean distances for all points with respect to their k nearest neighbors 94 int valid_distances = 0; 95 for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator 96 { 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)) 100 { 101 distances[iii] = 0.0; 102 continue; 103 } 104 105 // Perform the nearest k search 106 if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0) 107 { 108 distances[iii] = 0.0; 109 PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed. ", getClassName ().c_str (), mean_k_); 110 continue; 111 } 112 113 // Calculate the mean distance to its neighbors 114 double dist_sum = 0.0; 115 for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point 116 dist_sum += sqrt (nn_dists[k]); 117 distances[iii] = static_cast<float> (dist_sum / mean_k_); 118 valid_distances++; 119 } 120 121 // Estimate the mean and the standard deviation of the distance vector 122 double sum = 0, sq_sum = 0; 123 for (size_t i = 0; i < distances.size (); ++i) 124 { 125 sum += distances[i]; 126 sq_sum += distances[i] * distances[i]; 127 } 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); 131 //getMeanStd (distances, mean, stddev); 132 133 double distance_threshold = mean + std_mul_ * stddev; 134 135 // Second pass: Classify the points on the computed distance threshold 136 for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator 137 { 138 // Points having a too high average distance are outliers and are passed to removed indices 139 // Unless negative was set, then it's the opposite condition 140 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold)) 141 { 142 if (extract_removed_indices_) 143 (*removed_indices_)[rii++] = (*indices_)[iii]; 144 continue; 145 } 146 147 // Otherwise it was a normal point for output (inlier) 148 indices[oii++] = (*indices_)[iii]; 149 } 150 151 // Resize the output arrays 152 indices.resize (oii); 153 removed_indices_->resize (rii); 154 } 155 156 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>; 157 158 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
最终会执行
template <typename PointT> void pcl::StatisticalOutlierRemoval<PointT>::applyFilterIndices (std::vector<int> &indices)
1、进行一些简单Initialize
// Initialize the search class if (!searcher_) { if (input_->isOrganized ()) searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); else searcher_.reset (new pcl::search::KdTree<PointT> (false)); } searcher_->setInputCloud (input_);
2、定义一些变量
// The arrays to be used std::vector<int> nn_indices (mean_k_);//搜索完邻域点对应的索引 std::vector<float> nn_dists (mean_k_);//搜索完的每个邻域点与查询点之间的欧式距离 std::vector<float> distances (indices_->size ()); indices.resize (indices_->size ()); removed_indices_->resize (indices_->size ()); int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
3、求每个点的k邻域的均值
// First pass: Compute the mean distances for all points with respect to their k nearest neighbors int valid_distances = 0; for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || !pcl_isfinite (input_->points[(*indices_)[iii]].y) || !pcl_isfinite (input_->points[(*indices_)[iii]].z)) { distances[iii] = 0.0; continue; } // Perform the nearest k search if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0) { distances[iii] = 0.0; PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed. ", getClassName ().c_str (), mean_k_); continue; } // Calculate the mean distance to its neighbors double dist_sum = 0.0; for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point dist_sum += sqrt (nn_dists[k]); distances[iii] = static_cast<float> (dist_sum / mean_k_);//每个点都对应了一个距离变量 valid_distances++; }
4、估计距离的均值和标准差 不是邻域 ，是根据整个数据中的点均值和标准差
// Estimate the mean and the standard deviation of the distance vector double sum = 0, sq_sum = 0; for (size_t i = 0; i < distances.size (); ++i) { sum += distances[i]; sq_sum += distances[i] * distances[i]; }
double mean = sum / static_cast<double>(valid_distances);
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double> (valid_distances) - 1);
double stddev = sqrt (variance);
//getMeanStd (distances, mean, stddev);
5、根据设定的距离阈值与distances[iii]比较 ，超出设定阈值则该点被标记为离群点，并将其移除。
double distance_threshold = mean + std_mul_ * stddev; // Second pass: Classify the points on the computed distance threshold for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator { // Points having a too high average distance are outliers and are passed to removed indices // Unless negative was set, then it's the opposite condition if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold)) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; continue; } // Otherwise it was a normal point for output (inlier) indices[oii++] = (*indices_)[iii]; }
indices.resize (oii);
removed_indices_->resize (rii);// Resize the output arrays