Point Cloud Library (PCL)  1.11.0
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 
40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
42 
43 
44 namespace pcl
45 {
46 
47 namespace registration
48 {
49 
50 template <typename SourceT, typename TargetT> void
52  const pcl::Correspondences& original_correspondences,
53  pcl::Correspondences& remaining_correspondences)
54 {
55  // This is reset after all the checks below
56  remaining_correspondences = original_correspondences;
57 
58  // Check source/target
59  if (!input_)
60  {
61  PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No source was input! Returning all input correspondences.\n",
62  getClassName ().c_str ());
63  return;
64  }
65 
66  if (!target_)
67  {
68  PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No target was input! Returning all input correspondences.\n",
69  getClassName ().c_str ());
70  return;
71  }
72 
73  // Check cardinality
74  if (cardinality_ < 2)
75  {
76  PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Polygon cardinality too low!. Returning all input correspondences.\n",
77  getClassName ().c_str() );
78  return;
79  }
80 
81  // Number of input correspondences
82  const int nr_correspondences = static_cast<int> (original_correspondences.size ());
83 
84  // Not enough correspondences for polygonal rejections
85  if (cardinality_ >= nr_correspondences)
86  {
87  PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Number of correspondences smaller than polygon cardinality! Returning all input correspondences.\n",
88  getClassName ().c_str() );
89  return;
90  }
91 
92  // Check similarity
93  if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f)
94  {
95  PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Invalid edge length similarity - must be in [0,1]!. Returning all input correspondences.\n",
96  getClassName ().c_str() );
97  return;
98  }
99 
100  // Similarity, squared
101  similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_;
102 
103  // Initialization of result
104  remaining_correspondences.clear ();
105  remaining_correspondences.reserve (nr_correspondences);
106 
107  // Number of times a correspondence is sampled and number of times it was accepted
108  std::vector<int> num_samples (nr_correspondences, 0);
109  std::vector<int> num_accepted (nr_correspondences, 0);
110 
111  // Main loop
112  for (int i = 0; i < iterations_; ++i)
113  {
114  // Sample cardinality_ correspondences without replacement
115  const std::vector<int> idx = getUniqueRandomIndices (nr_correspondences, cardinality_);
116 
117  // Verify the polygon similarity
118  if (thresholdPolygon (original_correspondences, idx))
119  {
120  // Increment sample counter and accept counter
121  for (int j = 0; j < cardinality_; ++j)
122  {
123  ++num_samples[ idx[j] ];
124  ++num_accepted[ idx[j] ];
125  }
126  }
127  else
128  {
129  // Not accepted, only increment sample counter
130  for (int j = 0; j < cardinality_; ++j)
131  ++num_samples[ idx[j] ];
132  }
133  }
134 
135  // Now calculate the acceptance rate of each correspondence
136  std::vector<float> accept_rate (nr_correspondences, 0.0f);
137  for (int i = 0; i < nr_correspondences; ++i)
138  {
139  const int numsi = num_samples[i];
140  if (numsi == 0)
141  accept_rate[i] = 0.0f;
142  else
143  accept_rate[i] = static_cast<float> (num_accepted[i]) / static_cast<float> (numsi);
144  }
145 
146  // Compute a histogram in range [0,1] for acceptance rates
147  const int hist_size = nr_correspondences / 2; // TODO: Optimize this
148  const std::vector<int> histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size);
149 
150  // Find the cut point between outliers and inliers using Otsu's thresholding method
151  const int cut_idx = findThresholdOtsu (histogram);
152  const float cut = static_cast<float> (cut_idx) / static_cast<float> (hist_size);
153 
154  // Threshold
155  for (int i = 0; i < nr_correspondences; ++i)
156  if (accept_rate[i] > cut)
157  remaining_correspondences.push_back (original_correspondences[i]);
158 }
159 
160 
161 template <typename SourceT, typename TargetT> std::vector<int>
163  float lower, float upper, int bins)
164 {
165  // Result
166  std::vector<int> result (bins, 0);
167 
168  // Last index into result and increment factor from data value --> index
169  const int last_idx = bins - 1;
170  const float idx_per_val = static_cast<float> (bins) / (upper - lower);
171 
172  // Accumulate
173  for (const float &value : data)
174  ++result[ std::min (last_idx, int (value*idx_per_val)) ];
175 
176  return (result);
177 }
178 
179 
180 template <typename SourceT, typename TargetT> int
182 {
183  // Precision
184  const double eps = std::numeric_limits<double>::epsilon();
185 
186  // Histogram dimension
187  const int nbins = static_cast<int> (histogram.size ());
188 
189  // Mean and inverse of the number of data points
190  double mean = 0.0;
191  double sum_inv = 0.0;
192  for (int i = 0; i < nbins; ++i)
193  {
194  mean += static_cast<double> (i * histogram[i]);
195  sum_inv += static_cast<double> (histogram[i]);
196  }
197  sum_inv = 1.0/sum_inv;
198  mean *= sum_inv;
199 
200  // Probability and mean of class 1 (data to the left of threshold)
201  double class_mean1 = 0.0;
202  double class_prob1 = 0.0;
203  double class_prob2 = 1.0;
204 
205  // Maximized between class variance and associated bin value
206  double between_class_variance_max = 0.0;
207  int result = 0;
208 
209  // Loop over all bin values
210  for (int i = 0; i < nbins; ++i)
211  {
212  class_mean1 *= class_prob1;
213 
214  // Probability of bin i
215  const double prob_i = static_cast<double> (histogram[i]) * sum_inv;
216 
217  // Class probability 1: sum of probabilities from 0 to i
218  class_prob1 += prob_i;
219 
220  // Class probability 2: sum of probabilities from i+1 to nbins-1
221  class_prob2 -= prob_i;
222 
223  // Avoid division by zero below
224  if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps)
225  continue;
226 
227  // Class mean 1: sum of probabilities from 0 to i, weighted by bin value
228  class_mean1 = (class_mean1 + static_cast<double> (i) * prob_i) / class_prob1;
229 
230  // Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value
231  const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2;
232 
233  // Between class variance
234  const double between_class_variance = class_prob1 * class_prob2
235  * (class_mean1 - class_mean2)
236  * (class_mean1 - class_mean2);
237 
238  // If between class variance is maximized, update result
239  if (between_class_variance > between_class_variance_max)
240  {
241  between_class_variance_max = between_class_variance;
242  result = i;
243  }
244  }
245 
246  return (result);
247 }
248 
249 } // namespace registration
250 } // namespace pcl
251 
252 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_
253 
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