Point Cloud Library (PCL)  1.13.0-dev
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 namespace registration {
46 /** \brief CorrespondenceRejectorPoly implements a correspondence rejection method that
47  * exploits low-level and pose-invariant geometric constraints between two point sets by
48  * forming virtual polygons of a user-specifiable cardinality on each model using the
49  * input correspondences. These polygons are then checked in a pose-invariant manner
50  * (i.e. the side lengths must be approximately equal), and rejection is performed by
51  * thresholding these edge lengths.
52  *
53  * If you use this in academic work, please cite:
54  *
55  * A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger.
56  * Pose Estimation using Local Structure-Specific Shape and Appearance Context.
57  * International Conference on Robotics and Automation (ICRA), 2013.
58  *
59  * \author Anders Glent Buch
60  * \ingroup registration
61  */
62 template <typename SourceT, typename TargetT>
67 
68 public:
69  using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT>>;
70  using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT>>;
71 
75 
79 
80  /** \brief Empty constructor */
82  : iterations_(10000)
83  , cardinality_(3)
84  , similarity_threshold_(0.75f)
85  , similarity_threshold_squared_(0.75f * 0.75f)
86  {
87  rejection_name_ = "CorrespondenceRejectorPoly";
88  }
89 
90  /** \brief Get a list of valid correspondences after rejection from the original set
91  * of correspondences.
92  * \param[in] original_correspondences the set of initial correspondences given
93  * \param[out] remaining_correspondences the resultant filtered set of remaining
94  * 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
101  * compute the correspondence distance.
102  * \param[in] cloud a cloud containing XYZ data
103  */
104  inline void
106  {
107  input_ = cloud;
108  }
109 
110  /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to
111  * compute the correspondence distance.
112  * \param[in] target a cloud containing XYZ data
113  */
114  inline void
116  {
117  target_ = target;
118  }
119 
120  /** \brief See if this rejector requires source points */
121  bool
122  requiresSourcePoints() const override
123  {
124  return (true);
125  }
126 
127  /** \brief Blob method for setting the source cloud */
128  void
130  {
132  fromPCLPointCloud2(*cloud2, *cloud);
133  setInputSource(cloud);
134  }
135 
136  /** \brief See if this rejector requires a target cloud */
137  bool
138  requiresTargetPoints() const override
139  {
140  return (true);
141  }
142 
143  /** \brief Method for setting the target cloud */
144  void
146  {
148  fromPCLPointCloud2(*cloud2, *cloud);
149  setInputTarget(cloud);
150  }
151 
152  /** \brief Set the polygon cardinality
153  * \param cardinality polygon cardinality
154  */
155  inline void
156  setCardinality(int cardinality)
157  {
158  cardinality_ = cardinality;
159  }
160 
161  /** \brief Get the polygon cardinality
162  * \return polygon cardinality
163  */
164  inline int
166  {
167  return (cardinality_);
168  }
169 
170  /** \brief Set the similarity threshold in [0,1[ between edge lengths,
171  * where 1 is a perfect match
172  * \param similarity_threshold similarity threshold
173  */
174  inline void
175  setSimilarityThreshold(float similarity_threshold)
176  {
177  similarity_threshold_ = similarity_threshold;
178  similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
179  }
180 
181  /** \brief Get the similarity threshold between edge lengths
182  * \return similarity threshold
183  */
184  inline float
186  {
187  return (similarity_threshold_);
188  }
189 
190  /** \brief Set the number of iterations
191  * \param iterations number of iterations
192  */
193  inline void
194  setIterations(int iterations)
195  {
196  iterations_ = iterations;
197  }
198 
199  /** \brief Get the number of iterations
200  * \return number of iterations
201  */
202  inline int
204  {
205  return (iterations_);
206  }
207 
208  /** \brief Polygonal rejection of a single polygon, indexed by a subset of
209  * correspondences \param corr all correspondences into \ref input_ and \ref target_
210  * \param idx sampled indices into \b correspondences, must have a size equal to \ref
211  * cardinality_ \return true if all edge length ratios are larger than or equal to
212  * \ref similarity_threshold_
213  */
214  inline bool
215  thresholdPolygon(const pcl::Correspondences& corr, const std::vector<int>& idx)
216  {
217  if (cardinality_ ==
218  2) // Special case: when two points are considered, we only have one edge
219  {
220  return (thresholdEdgeLength(corr[idx[0]].index_query,
221  corr[idx[1]].index_query,
222  corr[idx[0]].index_match,
223  corr[idx[1]].index_match,
224  similarity_threshold_squared_));
225  }
226  // Otherwise check all edges
227  for (int i = 0; i < cardinality_; ++i) {
228  if (!thresholdEdgeLength(corr[idx[i]].index_query,
229  corr[idx[(i + 1) % cardinality_]].index_query,
230  corr[idx[i]].index_match,
231  corr[idx[(i + 1) % cardinality_]].index_match,
232  similarity_threshold_squared_)) {
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
241  * equal to \ref cardinality_
242  * \param target_indices corresponding indices of polygon points in \ref target_, must
243  * have a size equal to \ref cardinality_
244  * \return true if all edge length ratios are larger than or equal to
245  * \ref similarity_threshold_
246  */
247  inline bool
248  thresholdPolygon(const pcl::Indices& source_indices,
249  const pcl::Indices& target_indices)
250  {
251  // Convert indices to correspondences and an index vector pointing to each element
252  pcl::Correspondences corr(cardinality_);
253  std::vector<int> idx(cardinality_);
254  for (int i = 0; i < cardinality_; ++i) {
255  corr[i].index_query = source_indices[i];
256  corr[i].index_match = target_indices[i];
257  idx[i] = i;
258  }
259 
260  return (thresholdPolygon(corr, idx));
261  }
262 
263 protected:
264  /** \brief Apply the rejection algorithm.
265  * \param[out] correspondences the set of resultant correspondences.
266  */
267  inline void
268  applyRejection(pcl::Correspondences& correspondences) override
269  {
270  getRemainingCorrespondences(*input_correspondences_, correspondences);
271  }
272 
273  /** \brief Get k unique random indices in range {0,...,n-1} (sampling without
274  * replacement) \note No check is made to ensure that k <= n.
275  * \param n upper index range, exclusive
276  * \param k number of unique indices to sample
277  * \return k unique random indices in range {0,...,n-1}
278  */
279  inline std::vector<int>
281  {
282  // Marked sampled indices and sample counter
283  std::vector<bool> sampled(n, false);
284  int samples = 0;
285  // Resulting unique indices
286  std::vector<int> result;
287  result.reserve(k);
288  do {
289  // Pick a random index in the range
290  const int idx = (std::rand() % n);
291  // If unique
292  if (!sampled[idx]) {
293  // Mark as sampled and increment result counter
294  sampled[idx] = true;
295  ++samples;
296  // Store
297  result.push_back(idx);
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 =
336  computeSquaredDistance((*input_)[index_query_1], (*input_)[index_query_2]);
337  // Distance between target points
338  const float dist_tgt =
339  computeSquaredDistance((*target_)[index_match_1], (*target_)[index_match_2]);
340  // Edge length similarity [0,1] where 1 is a perfect match
341  const float edge_sim =
342  (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
343 
344  return (edge_sim >= simsq);
345  }
346 
347  /** \brief Compute a linear histogram. This function is equivalent to the MATLAB
348  * function \b histc, with the edges set as follows: <b>
349  * lower:(upper-lower)/bins:upper </b>
350  * \param data input samples
351  * \param lower lower bound of input samples
352  * \param upper upper bound of input samples
353  * \param bins number of bins in output
354  * \return linear histogram
355  */
356  std::vector<int>
357  computeHistogram(const std::vector<float>& data, float lower, float upper, int bins);
358 
359  /** \brief Find the optimal value for binary histogram thresholding using Otsu's
360  * method
361  * \param histogram input histogram \return threshold value according to Otsu's
362  * criterion
363  */
364  int
365  findThresholdOtsu(const std::vector<int>& histogram);
366 
367  /** \brief The input point cloud dataset */
369 
370  /** \brief The input point cloud dataset target */
372 
373  /** \brief Number of iterations to run */
375 
376  /** \brief The polygon cardinality used during rejection */
378 
379  /** \brief Lower edge length threshold in [0,1] used for verifying polygon
380  * similarities, where 1 is a perfect match */
382 
383  /** \brief Squared value if \ref similarity_threshold_, only for internal use */
385 };
386 } // namespace registration
387 } // namespace pcl
388 
389 #include <pcl/registration/impl/correspondence_rejection_poly.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
CorrespondenceRejector represents the base class for correspondence rejection methods
shared_ptr< const CorrespondenceRejector > ConstPtr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< CorrespondenceRejector > Ptr
std::string rejection_name_
The name of the rejection method.
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.
bool thresholdPolygon(const pcl::Correspondences &corr, const std::vector< int > &idx)
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
float computeSquaredDistance(const SourceT &p1, const TargetT &p2)
Squared Euclidean distance between two points using the members x, y and z.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
PointCloudSourceConstPtr input_
The input point cloud dataset.
float getSimilarityThreshold()
Get the similarity threshold between edge lengths.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
bool thresholdPolygon(const pcl::Indices &source_indices, const pcl::Indices &target_indices)
Polygonal rejection of a single polygon, indexed by two point index vectors.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setCardinality(int cardinality)
Set the polygon cardinality.
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
void setIterations(int iterations)
Set the number of iterations.
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source 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.
std::vector< int > getUniqueRandomIndices(int n, int k)
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
bool requiresSourcePoints() const override
See if this rejector requires source points.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
int cardinality_
The polygon cardinality used during rejection.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:163
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_EXPORTS
Definition: pcl_macros.h:323
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr