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