Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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
135  setSourcePoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
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
149  setTargetPoints (pcl::PCLPointCloud2::ConstPtr cloud2) override
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>
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
Definition: conversions.h:168
shared_ptr< const CorrespondenceRejector > ConstPtr
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
shared_ptr< CorrespondenceRejector > Ptr
bool thresholdPolygon(const pcl::Correspondences &corr, const std::vector< int > &idx)
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
std::vector< int > getUniqueRandomIndices(int n, int k)
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
CorrespondenceRejector represents the base class for correspondence rejection methods ...
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.
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
PointCloudSourceConstPtr input_
The input point cloud dataset.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
int cardinality_
The polygon cardinality used during rejection.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float getSimilarityThreshold()
Get the similarity threshold between edge lengths.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
float computeSquaredDistance(const SourceT &p1, const TargetT &p2)
Squared Euclidean distance between two points using the members x, y and z.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
bool requiresSourcePoints() const override
See if this rejector requires source points.
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void setCardinality(int cardinality)
Set the polygon cardinality.
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
void setIterations(int iterations)
Set the number of iterations.
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.