Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
correspondence_rejection_poly.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 */
38
39#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
40#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
41
42namespace pcl {
43
44namespace registration {
45
46template <typename SourceT, typename TargetT>
47void
49 const pcl::Correspondences& original_correspondences,
50 pcl::Correspondences& remaining_correspondences)
51{
52 // This is reset after all the checks below
53 remaining_correspondences = original_correspondences;
54
55 // Check source/target
56 if (!input_) {
57 PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No source was "
58 "input! Returning all input correspondences.\n",
59 getClassName().c_str());
60 return;
61 }
62
63 if (!target_) {
64 PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] No target was "
65 "input! Returning all input correspondences.\n",
66 getClassName().c_str());
67 return;
68 }
69
70 // Check cardinality
71 if (cardinality_ < 2) {
72 PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Polygon "
73 "cardinality too low!. Returning all input correspondences.\n",
74 getClassName().c_str());
75 return;
76 }
77
78 // Number of input correspondences
79 const int nr_correspondences = static_cast<int>(original_correspondences.size());
80
81 // Not enough correspondences for polygonal rejections
82 if (cardinality_ >= nr_correspondences) {
83 PCL_ERROR("[pcl::registration::%s::getRemainingCorrespondences] Number of "
84 "correspondences smaller than polygon cardinality! Returning all input "
85 "correspondences.\n",
86 getClassName().c_str());
87 return;
88 }
89
90 // Check similarity
91 if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f) {
92 PCL_ERROR(
93 "[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length "
94 "similarity - must be in [0,1]!. Returning all input correspondences.\n",
95 getClassName().c_str());
96 return;
97 }
98
99 // Similarity, squared
100 similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
101
102 // Initialization of result
103 remaining_correspondences.clear();
104 remaining_correspondences.reserve(nr_correspondences);
105
106 // Number of times a correspondence is sampled and number of times it was accepted
107 std::vector<int> num_samples(nr_correspondences, 0);
108 std::vector<int> num_accepted(nr_correspondences, 0);
109
110 // Main loop
111 for (int i = 0; i < iterations_; ++i) {
112 // Sample cardinality_ correspondences without replacement
113 const std::vector<int> idx =
114 getUniqueRandomIndices(nr_correspondences, cardinality_);
115
116 // Verify the polygon similarity
117 if (thresholdPolygon(original_correspondences, idx)) {
118 // Increment sample counter and accept counter
119 for (int j = 0; j < cardinality_; ++j) {
120 ++num_samples[idx[j]];
121 ++num_accepted[idx[j]];
122 }
123 }
124 else {
125 // Not accepted, only increment sample counter
126 for (int j = 0; j < cardinality_; ++j)
127 ++num_samples[idx[j]];
128 }
129 }
130
131 // Now calculate the acceptance rate of each correspondence
132 std::vector<float> accept_rate(nr_correspondences, 0.0f);
133 for (int i = 0; i < nr_correspondences; ++i) {
134 const int numsi = num_samples[i];
135 if (numsi == 0)
136 accept_rate[i] = 0.0f;
137 else
138 accept_rate[i] = static_cast<float>(num_accepted[i]) / static_cast<float>(numsi);
139 }
140
141 // Compute a histogram in range [0,1] for acceptance rates
142 const int hist_size = nr_correspondences / 2; // TODO: Optimize this
143 const std::vector<int> histogram =
144 computeHistogram(accept_rate, 0.0f, 1.0f, hist_size);
145
146 // Find the cut point between outliers and inliers using Otsu's thresholding method
147 const int cut_idx = findThresholdOtsu(histogram);
148 const float cut = static_cast<float>(cut_idx) / static_cast<float>(hist_size);
149
150 // Threshold
151 for (int i = 0; i < nr_correspondences; ++i)
152 if (accept_rate[i] > cut)
153 remaining_correspondences.push_back(original_correspondences[i]);
154}
155
156template <typename SourceT, typename TargetT>
157std::vector<int>
159 const std::vector<float>& data, float lower, float upper, int bins)
160{
161 // Result
162 std::vector<int> result(bins, 0);
163
164 // Last index into result and increment factor from data value --> index
165 const int last_idx = bins - 1;
166 const float idx_per_val = static_cast<float>(bins) / (upper - lower);
167
168 // Accumulate
169 for (const float& value : data)
170 ++result[std::min(last_idx, static_cast<int>(value * idx_per_val))];
171
172 return (result);
173}
174
175template <typename SourceT, typename TargetT>
176int
178 const std::vector<int>& histogram)
179{
180 // Precision
181 constexpr double eps = std::numeric_limits<double>::epsilon();
182
183 // Histogram dimension
184 const int nbins = static_cast<int>(histogram.size());
185
186 // Mean and inverse of the number of data points
187 double mean = 0.0;
188 double sum_inv = 0.0;
189 for (int i = 0; i < nbins; ++i) {
190 mean += static_cast<double>(i * histogram[i]);
191 sum_inv += static_cast<double>(histogram[i]);
192 }
193 sum_inv = 1.0 / sum_inv;
194 mean *= sum_inv;
195
196 // Probability and mean of class 1 (data to the left of threshold)
197 double class_mean1 = 0.0;
198 double class_prob1 = 0.0;
199 double class_prob2 = 1.0;
200
201 // Maximized between class variance and associated bin value
202 double between_class_variance_max = 0.0;
203 int result = 0;
204
205 // Loop over all bin values
206 for (int i = 0; i < nbins; ++i) {
207 class_mean1 *= class_prob1;
208
209 // Probability of bin i
210 const double prob_i = static_cast<double>(histogram[i]) * sum_inv;
211
212 // Class probability 1: sum of probabilities from 0 to i
213 class_prob1 += prob_i;
214
215 // Class probability 2: sum of probabilities from i+1 to nbins-1
216 class_prob2 -= prob_i;
217
218 // Avoid division by zero below
219 if (std::min(class_prob1, class_prob2) < eps ||
220 std::max(class_prob1, class_prob2) > 1.0 - eps)
221 continue;
222
223 // Class mean 1: sum of probabilities from 0 to i, weighted by bin value
224 class_mean1 = (class_mean1 + static_cast<double>(i) * prob_i) / class_prob1;
225
226 // Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value
227 const double class_mean2 = (mean - class_prob1 * class_mean1) / class_prob2;
228
229 // Between class variance
230 const double between_class_variance = class_prob1 * class_prob2 *
231 (class_mean1 - class_mean2) *
232 (class_mean1 - class_mean2);
233
234 // If between class variance is maximized, update result
235 if (between_class_variance > between_class_variance_max) {
236 between_class_variance_max = between_class_variance;
237 result = i;
238 }
239 }
240
241 return (result);
242}
243
244} // namespace registration
245} // namespace pcl
246
247#endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
std::vector< int > computeHistogram(const std::vector< float > &data, float lower, float upper, int bins)
Compute a linear histogram.
int findThresholdOtsu(const std::vector< int > &histogram)
Find the optimal value for binary histogram thresholding using Otsu's method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences