源代码
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.\n", 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.\n", 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
时间： 2024-10-09 23:47:05