Point Cloud Library (PCL)  1.14.1-dev
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 
42 namespace pcl {
43 
44 namespace registration {
45 
46 template <typename SourceT, typename TargetT>
47 void
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 
156 template <typename SourceT, typename TargetT>
157 std::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 
175 template <typename SourceT, typename TargetT>
176 int
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