Point Cloud Library (PCL)  1.7.0
correspondence_rejection_poly.h
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_CORRESPONDENCE_REJECTION_POLY_H_
39 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_
40 
41 #include <pcl/registration/correspondence_rejection.h>
42 #include <pcl/point_cloud.h>
43 
44 namespace pcl
45 {
46  namespace registration
47  {
48  /** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and
49  * pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable
50  * cardinality on each model using the input correspondences.
51  * These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal),
52  * and rejection is performed by thresholding these edge lengths.
53  *
54  * If you use this in academic work, please cite:
55  *
56  * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
57  * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
58  * International Conference on Robotics and Automation (ICRA), 2013.
59  *
60  * \author Anders Glent Buch
61  * \ingroup registration
62  */
63  template <typename SourceT, typename TargetT>
65  {
69 
70  public:
71  typedef boost::shared_ptr<CorrespondenceRejectorPoly> Ptr;
72  typedef boost::shared_ptr<const CorrespondenceRejectorPoly> ConstPtr;
73 
77 
81 
82  /** \brief Empty constructor */
84  : iterations_ (10000)
85  , cardinality_ (3)
86  , similarity_threshold_ (0.75f)
87  , similarity_threshold_squared_ (0.75f * 0.75f)
88  {
89  rejection_name_ = "CorrespondenceRejectorPoly";
90  }
91 
92  /** \brief Get a list of valid correspondences after rejection from the original set of correspondences.
93  * \param[in] original_correspondences the set of initial correspondences given
94  * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
95  */
96  void
97  getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
98  pcl::Correspondences& remaining_correspondences);
99 
100  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
101  * \param[in] cloud a cloud containing XYZ data
102  */
103  inline void
105  {
106  input_ = cloud;
107  }
108 
109  /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
110  * \param[in] cloud a cloud containing XYZ data
111  */
112  inline void
114  {
115  PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n",
116  getClassName ().c_str ());
117  input_ = cloud;
118  }
119 
120  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
121  * \param[in] target a cloud containing XYZ data
122  */
123  inline void
125  {
126  target_ = target;
127  }
128 
129  /** \brief Set the polygon cardinality
130  * \param cardinality polygon cardinality
131  */
132  inline void
133  setCardinality (int cardinality)
134  {
135  cardinality_ = cardinality;
136  }
137 
138  /** \brief Get the polygon cardinality
139  * \return polygon cardinality
140  */
141  inline int
143  {
144  return (cardinality_);
145  }
146 
147  /** \brief Set the similarity threshold in [0,1[ between edge lengths,
148  * where 1 is a perfect match
149  * \param similarity similarity threshold
150  */
151  inline void
152  setSimilarityThreshold (float similarity_threshold)
153  {
154  similarity_threshold_ = similarity_threshold;
155  similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
156  }
157 
158  /** \brief Get the similarity threshold between edge lengths
159  * \return similarity threshold
160  */
161  inline float
163  {
164  return (similarity_threshold_);
165  }
166 
167  /** \brief Set the number of iterations
168  * \param iterations number of iterations
169  */
170  inline void
171  setIterations (int iterations)
172  {
173  iterations_ = iterations;
174  }
175 
176  /** \brief Get the number of iterations
177  * \return number of iterations
178  */
179  inline int
181  {
182  return (iterations_);
183  }
184 
185  /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences
186  * \param corr all correspondences into \ref input_ and \ref target_
187  * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_
188  * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
189  */
190  inline bool
191  thresholdPolygon (const pcl::Correspondences& corr, const std::vector<int>& idx)
192  {
193  if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge
194  {
195  return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
196  corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
197  cardinality_));
198  }
199  else
200  { // Otherwise check all edges
201  for (int i = 0; i < cardinality_; ++i)
202  if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
203  corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
204  similarity_threshold_squared_))
205  return (false);
206 
207  return (true);
208  }
209  }
210 
211  /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors
212  * \param source_indices indices of polygon points in \ref input_, must have a size equal to \ref cardinality_
213  * \param target_indices corresponding indices of polygon points in \ref target_, must have a size equal to \ref cardinality_
214  * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
215  */
216  inline bool
217  thresholdPolygon (const std::vector<int>& source_indices, const std::vector<int>& target_indices)
218  {
219  // Convert indices to correspondences and an index vector pointing to each element
220  pcl::Correspondences corr (cardinality_);
221  std::vector<int> idx (cardinality_);
222  for (int i = 0; i < cardinality_; ++i)
223  {
224  corr[i].index_query = source_indices[i];
225  corr[i].index_match = target_indices[i];
226  idx[i] = i;
227  }
228 
229  return (thresholdPolygon (corr, idx));
230  }
231 
232  protected:
233  /** \brief Apply the rejection algorithm.
234  * \param[out] correspondences the set of resultant correspondences.
235  */
236  inline void
238  {
239  getRemainingCorrespondences (*input_correspondences_, correspondences);
240  }
241 
242  /** \brief Get k unique random indices in range {0,...,n-1} (sampling without replacement)
243  * \note No check is made to ensure that k <= n.
244  * \param n upper index range, exclusive
245  * \param k number of unique indices to sample
246  * \return k unique random indices in range {0,...,n-1}
247  */
248  inline std::vector<int>
249  getUniqueRandomIndices (int n, int k)
250  {
251  // Marked sampled indices and sample counter
252  std::vector<bool> sampled (n, false);
253  int samples = 0;
254  // Resulting unique indices
255  std::vector<int> result;
256  result.reserve (k);
257  do
258  {
259  // Pick a random index in the range
260  const int idx = (std::rand () % n);
261  // If unique
262  if (!sampled[idx])
263  {
264  // Mark as sampled and increment result counter
265  sampled[idx] = true;
266  ++samples;
267  // Store
268  result.push_back (idx);
269  }
270  }
271  while (samples < k);
272 
273  return (result);
274  }
275 
276  /** \brief Squared Euclidean distance between two points using the members x, y and z
277  * \param p1 first point
278  * \param p2 second point
279  * \return squared Euclidean distance
280  */
281  inline float
282  computeSquaredDistance (const SourceT& p1, const TargetT& p2)
283  {
284  const float dx = p2.x - p1.x;
285  const float dy = p2.y - p1.y;
286  const float dz = p2.z - p1.z;
287 
288  return (dx*dx + dy*dy + dz*dz);
289  }
290 
291  /** \brief Edge length similarity thresholding
292  * \param index_query_1 index of first source vertex
293  * \param index_query_2 index of second source vertex
294  * \param index_match_1 index of first target vertex
295  * \param index_match_2 index of second target vertex
296  * \param simsq squared similarity threshold in [0,1]
297  * \return true if edge length ratio is larger than or equal to threshold
298  */
299  inline bool
300  thresholdEdgeLength (int index_query_1,
301  int index_query_2,
302  int index_match_1,
303  int index_match_2,
304  float simsq)
305  {
306  // Distance between source points
307  const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]);
308  // Distance between target points
309  const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]);
310  // Edge length similarity [0,1] where 1 is a perfect match
311  const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
312 
313  return (edge_sim >= simsq);
314  }
315 
316  /** \brief Compute a linear histogram. This function is equivalent to the MATLAB function \b histc, with the
317  * edges set as follows: <b> lower:(upper-lower)/bins:upper </b>
318  * \param data input samples
319  * \param lower lower bound of input samples
320  * \param upper upper bound of input samples
321  * \param bins number of bins in output
322  * \return linear histogram
323  */
324  std::vector<int>
325  computeHistogram (const std::vector<float>& data, float lower, float upper, int bins);
326 
327  /** \brief Find the optimal value for binary histogram thresholding using Otsu's method
328  * \param histogram input histogram
329  * \return threshold value according to Otsu's criterion
330  */
331  int
332  findThresholdOtsu (const std::vector<int>& histogram);
333 
334  /** \brief The input point cloud dataset */
336 
337  /** \brief The input point cloud dataset target */
339 
340  /** \brief Number of iterations to run */
342 
343  /** \brief The polygon cardinality used during rejection */
345 
346  /** \brief Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match */
348 
349  /** \brief Squared value if \ref similarity_threshold_, only for internal use */
351  };
352  }
353 }
354 
355 #include <pcl/registration/impl/correspondence_rejection_poly.hpp>
356 
357 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_