Point Cloud Library (PCL)  1.11.1
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 
39 #pragma once
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  using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT> >;
72  using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT> >;
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) override;
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  PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorPoly::setInputCloud is deprecated. Please use setInputSource instead")
113  inline void
114  setInputCloud (const PointCloudSourceConstPtr &cloud)
115  {
116  input_ = cloud;
117  }
118 
119  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
120  * \param[in] target a cloud containing XYZ data
121  */
122  inline void
124  {
125  target_ = target;
126  }
127 
128  /** \brief See if this rejector requires source points */
129  bool
130  requiresSourcePoints () const override
131  { return (true); }
132 
133  /** \brief Blob method for setting the source cloud */
134  void
136  {
138  fromPCLPointCloud2 (*cloud2, *cloud);
139  setInputSource (cloud);
140  }
141 
142  /** \brief See if this rejector requires a target cloud */
143  bool
144  requiresTargetPoints () const override
145  { return (true); }
146 
147  /** \brief Method for setting the target cloud */
148  void
150  {
152  fromPCLPointCloud2 (*cloud2, *cloud);
153  setInputTarget (cloud);
154  }
155 
156  /** \brief Set the polygon cardinality
157  * \param cardinality polygon cardinality
158  */
159  inline void
160  setCardinality (int cardinality)
161  {
162  cardinality_ = cardinality;
163  }
164 
165  /** \brief Get the polygon cardinality
166  * \return polygon cardinality
167  */
168  inline int
170  {
171  return (cardinality_);
172  }
173 
174  /** \brief Set the similarity threshold in [0,1[ between edge lengths,
175  * where 1 is a perfect match
176  * \param similarity_threshold similarity threshold
177  */
178  inline void
179  setSimilarityThreshold (float similarity_threshold)
180  {
181  similarity_threshold_ = similarity_threshold;
182  similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
183  }
184 
185  /** \brief Get the similarity threshold between edge lengths
186  * \return similarity threshold
187  */
188  inline float
190  {
191  return (similarity_threshold_);
192  }
193 
194  /** \brief Set the number of iterations
195  * \param iterations number of iterations
196  */
197  inline void
198  setIterations (int iterations)
199  {
200  iterations_ = iterations;
201  }
202 
203  /** \brief Get the number of iterations
204  * \return number of iterations
205  */
206  inline int
208  {
209  return (iterations_);
210  }
211 
212  /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences
213  * \param corr all correspondences into \ref input_ and \ref target_
214  * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_
215  * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
216  */
217  inline bool
218  thresholdPolygon (const pcl::Correspondences& corr, const std::vector<int>& idx)
219  {
220  if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge
221  {
222  return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
223  corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
224  cardinality_));
225  }
226  // Otherwise check all edges
227  for (int i = 0; i < cardinality_; ++i)
228  {
229  if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
230  corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
231  similarity_threshold_squared_))
232  {
233  return (false);
234  }
235  }
236  return (true);
237  }
238 
239  /** \brief Polygonal rejection of a single polygon, indexed by two point index vectors
240  * \param source_indices indices of polygon points in \ref input_, must have a size equal to \ref cardinality_
241  * \param target_indices corresponding indices of polygon points in \ref target_, must have a size equal to \ref cardinality_
242  * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
243  */
244  inline bool
245  thresholdPolygon (const std::vector<int>& source_indices, const std::vector<int>& target_indices)
246  {
247  // Convert indices to correspondences and an index vector pointing to each element
248  pcl::Correspondences corr (cardinality_);
249  std::vector<int> idx (cardinality_);
250  for (int i = 0; i < cardinality_; ++i)
251  {
252  corr[i].index_query = source_indices[i];
253  corr[i].index_match = target_indices[i];
254  idx[i] = i;
255  }
256 
257  return (thresholdPolygon (corr, idx));
258  }
259 
260  protected:
261  /** \brief Apply the rejection algorithm.
262  * \param[out] correspondences the set of resultant correspondences.
263  */
264  inline void
265  applyRejection (pcl::Correspondences &correspondences) override
266  {
267  getRemainingCorrespondences (*input_correspondences_, correspondences);
268  }
269 
270  /** \brief Get k unique random indices in range {0,...,n-1} (sampling without replacement)
271  * \note No check is made to ensure that k <= n.
272  * \param n upper index range, exclusive
273  * \param k number of unique indices to sample
274  * \return k unique random indices in range {0,...,n-1}
275  */
276  inline std::vector<int>
277  getUniqueRandomIndices (int n, int k)
278  {
279  // Marked sampled indices and sample counter
280  std::vector<bool> sampled (n, false);
281  int samples = 0;
282  // Resulting unique indices
283  std::vector<int> result;
284  result.reserve (k);
285  do
286  {
287  // Pick a random index in the range
288  const int idx = (std::rand () % n);
289  // If unique
290  if (!sampled[idx])
291  {
292  // Mark as sampled and increment result counter
293  sampled[idx] = true;
294  ++samples;
295  // Store
296  result.push_back (idx);
297  }
298  }
299  while (samples < k);
300 
301  return (result);
302  }
303 
304  /** \brief Squared Euclidean distance between two points using the members x, y and z
305  * \param p1 first point
306  * \param p2 second point
307  * \return squared Euclidean distance
308  */
309  inline float
310  computeSquaredDistance (const SourceT& p1, const TargetT& p2)
311  {
312  const float dx = p2.x - p1.x;
313  const float dy = p2.y - p1.y;
314  const float dz = p2.z - p1.z;
315 
316  return (dx*dx + dy*dy + dz*dz);
317  }
318 
319  /** \brief Edge length similarity thresholding
320  * \param index_query_1 index of first source vertex
321  * \param index_query_2 index of second source vertex
322  * \param index_match_1 index of first target vertex
323  * \param index_match_2 index of second target vertex
324  * \param simsq squared similarity threshold in [0,1]
325  * \return true if edge length ratio is larger than or equal to threshold
326  */
327  inline bool
328  thresholdEdgeLength (int index_query_1,
329  int index_query_2,
330  int index_match_1,
331  int index_match_2,
332  float simsq)
333  {
334  // Distance between source points
335  const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]);
336  // Distance between target points
337  const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]);
338  // Edge length similarity [0,1] where 1 is a perfect match
339  const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
340 
341  return (edge_sim >= simsq);
342  }
343 
344  /** \brief Compute a linear histogram. This function is equivalent to the MATLAB function \b histc, with the
345  * edges set as follows: <b> lower:(upper-lower)/bins:upper </b>
346  * \param data input samples
347  * \param lower lower bound of input samples
348  * \param upper upper bound of input samples
349  * \param bins number of bins in output
350  * \return linear histogram
351  */
352  std::vector<int>
353  computeHistogram (const std::vector<float>& data, float lower, float upper, int bins);
354 
355  /** \brief Find the optimal value for binary histogram thresholding using Otsu's method
356  * \param histogram input histogram
357  * \return threshold value according to Otsu's criterion
358  */
359  int
360  findThresholdOtsu (const std::vector<int>& histogram);
361 
362  /** \brief The input point cloud dataset */
364 
365  /** \brief The input point cloud dataset target */
367 
368  /** \brief Number of iterations to run */
370 
371  /** \brief The polygon cardinality used during rejection */
373 
374  /** \brief Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match */
376 
377  /** \brief Squared value if \ref similarity_threshold_, only for internal use */
379  };
380  }
381 }
382 
383 #include <pcl/registration/impl/correspondence_rejection_poly.hpp>
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:61
pcl::registration::CorrespondenceRejectorPoly::setTargetPoints
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
Definition: correspondence_rejection_poly.h:149
pcl
Definition: convolution.h:46
pcl::registration::CorrespondenceRejectorPoly::CorrespondenceRejectorPoly
CorrespondenceRejectorPoly()
Empty constructor.
Definition: correspondence_rejection_poly.h:83
pcl::registration::CorrespondenceRejectorPoly::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: correspondence_rejection_poly.h:80
pcl::registration::CorrespondenceRejectorPoly::setSimilarityThreshold
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
Definition: correspondence_rejection_poly.h:179
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition: correspondence_rejection.h:62
pcl::registration::CorrespondenceRejectorPoly
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
Definition: correspondence_rejection_poly.h:65
pcl::registration::CorrespondenceRejectorPoly::setIterations
void setIterations(int iterations)
Set the number of iterations.
Definition: correspondence_rejection_poly.h:198
pcl::registration::CorrespondenceRejectorPoly::similarity_threshold_squared_
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.
Definition: correspondence_rejection_poly.h:378
pcl::registration::CorrespondenceRejectorPoly::getCardinality
int getCardinality()
Get the polygon cardinality.
Definition: correspondence_rejection_poly.h:169
pcl::registration::CorrespondenceRejectorPoly::input_
PointCloudSourceConstPtr input_
The input point cloud dataset.
Definition: correspondence_rejection_poly.h:363
pcl::registration::CorrespondenceRejectorPoly::getUniqueRandomIndices
std::vector< int > getUniqueRandomIndices(int n, int k)
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
Definition: correspondence_rejection_poly.h:277
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: correspondence_rejection.h:132
pcl::registration::CorrespondenceRejectorPoly::thresholdPolygon
bool thresholdPolygon(const std::vector< int > &source_indices, const std::vector< int > &target_indices)
Polygonal rejection of a single polygon, indexed by two point index vectors.
Definition: correspondence_rejection_poly.h:245
pcl::registration::CorrespondenceRejectorPoly::thresholdPolygon
bool thresholdPolygon(const pcl::Correspondences &corr, const std::vector< int > &idx)
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
Definition: correspondence_rejection_poly.h:218
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::registration::CorrespondenceRejectorPoly::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: correspondence_rejection_poly.h:75
pcl::registration::CorrespondenceRejectorPoly::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: correspondence_rejection_poly.h:76
PCL_DEPRECATED
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition: pcl_macros.h:147
pcl::registration::CorrespondenceRejectorPoly::getSimilarityThreshold
float getSimilarityThreshold()
Get the similarity threshold between edge lengths.
Definition: correspondence_rejection_poly.h:189
pcl::PCLPointCloud2::ConstPtr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
Definition: PCLPointCloud2.h:35
pcl::registration::CorrespondenceRejectorPoly::setInputSource
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
Definition: correspondence_rejection_poly.h:104
pcl::registration::CorrespondenceRejectorPoly::thresholdEdgeLength
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
Definition: correspondence_rejection_poly.h:328
pcl::registration::CorrespondenceRejectorPoly::computeSquaredDistance
float computeSquaredDistance(const SourceT &p1, const TargetT &p2)
Squared Euclidean distance between two points using the members x, y and z.
Definition: correspondence_rejection_poly.h:310
pcl::registration::CorrespondenceRejectorPoly::similarity_threshold_
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
Definition: correspondence_rejection_poly.h:375
pcl::registration::CorrespondenceRejectorPoly::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
Definition: correspondence_rejection_poly.h:123
pcl::registration::CorrespondenceRejectorPoly::target_
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition: correspondence_rejection_poly.h:366
pcl::registration::CorrespondenceRejectorPoly::getIterations
int getIterations()
Get the number of iterations.
Definition: correspondence_rejection_poly.h:207
pcl::registration::CorrespondenceRejectorPoly::iterations_
int iterations_
Number of iterations to run.
Definition: correspondence_rejection_poly.h:369
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
pcl::registration::CorrespondenceRejectorPoly::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: correspondence_rejection_poly.h:79
pcl::registration::CorrespondenceRejectorPoly::cardinality_
int cardinality_
The polygon cardinality used during rejection.
Definition: correspondence_rejection_poly.h:372
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:90
pcl::registration::CorrespondenceRejectorPoly::setCardinality
void setCardinality(int cardinality)
Set the polygon cardinality.
Definition: correspondence_rejection_poly.h:160
pcl::registration::CorrespondenceRejectorPoly::setSourcePoints
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
Definition: correspondence_rejection_poly.h:135
pcl::registration::CorrespondenceRejectorPoly::applyRejection
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
Definition: correspondence_rejection_poly.h:265
pcl::registration::CorrespondenceRejectorPoly::requiresTargetPoints
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
Definition: correspondence_rejection_poly.h:144
pcl::registration::CorrespondenceRejector::input_correspondences_
CorrespondencesConstPtr input_correspondences_
The input correspondences.
Definition: correspondence_rejection.h:188
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition: correspondence_rejection.h:185
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:328
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition: correspondence_rejection.h:59
pcl::fromPCLPointCloud2
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:168
pcl::registration::CorrespondenceRejectorPoly::requiresSourcePoints
bool requiresSourcePoints() const override
See if this rejector requires source points.
Definition: correspondence_rejection_poly.h:130