Point Cloud Library (PCL)  1.9.1
 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 #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
104  setInputSource (const PointCloudSourceConstPtr &cloud)
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
113  setInputCloud (const PointCloudSourceConstPtr &cloud)
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
124  setInputTarget (const PointCloudTargetConstPtr &target)
125  {
126  target_ = target;
127  }
128 
129  /** \brief See if this rejector requires source points */
130  bool
132  { return (true); }
133 
134  /** \brief Blob method for setting the source cloud */
135  void
137  {
138  PointCloudSourcePtr cloud (new PointCloudSource);
139  fromPCLPointCloud2 (*cloud2, *cloud);
140  setInputSource (cloud);
141  }
142 
143  /** \brief See if this rejector requires a target cloud */
144  bool
146  { return (true); }
147 
148  /** \brief Method for setting the target cloud */
149  void
151  {
152  PointCloudTargetPtr cloud (new PointCloudTarget);
153  fromPCLPointCloud2 (*cloud2, *cloud);
154  setInputTarget (cloud);
155  }
156 
157  /** \brief Set the polygon cardinality
158  * \param cardinality polygon cardinality
159  */
160  inline void
161  setCardinality (int cardinality)
162  {
163  cardinality_ = cardinality;
164  }
165 
166  /** \brief Get the polygon cardinality
167  * \return polygon cardinality
168  */
169  inline int
171  {
172  return (cardinality_);
173  }
174 
175  /** \brief Set the similarity threshold in [0,1[ between edge lengths,
176  * where 1 is a perfect match
177  * \param similarity_threshold similarity threshold
178  */
179  inline void
180  setSimilarityThreshold (float similarity_threshold)
181  {
182  similarity_threshold_ = similarity_threshold;
183  similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
184  }
185 
186  /** \brief Get the similarity threshold between edge lengths
187  * \return similarity threshold
188  */
189  inline float
191  {
192  return (similarity_threshold_);
193  }
194 
195  /** \brief Set the number of iterations
196  * \param iterations number of iterations
197  */
198  inline void
199  setIterations (int iterations)
200  {
201  iterations_ = iterations;
202  }
203 
204  /** \brief Get the number of iterations
205  * \return number of iterations
206  */
207  inline int
209  {
210  return (iterations_);
211  }
212 
213  /** \brief Polygonal rejection of a single polygon, indexed by a subset of correspondences
214  * \param corr all correspondences into \ref input_ and \ref target_
215  * \param idx sampled indices into \b correspondences, must have a size equal to \ref cardinality_
216  * \return true if all edge length ratios are larger than or equal to \ref similarity_threshold_
217  */
218  inline bool
219  thresholdPolygon (const pcl::Correspondences& corr, const std::vector<int>& idx)
220  {
221  if (cardinality_ == 2) // Special case: when two points are considered, we only have one edge
222  {
223  return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
224  corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
225  cardinality_));
226  }
227  else
228  { // Otherwise check all edges
229  for (int i = 0; i < cardinality_; ++i)
230  if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
231  corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
232  similarity_threshold_squared_))
233  return (false);
234 
235  return (true);
236  }
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
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 */
363  PointCloudSourceConstPtr input_;
364 
365  /** \brief The input point cloud dataset target */
366  PointCloudTargetConstPtr 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>
384 
385 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_
bool requiresTargetPoints() const
See if this rejector requires a target cloud.
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:169
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Blob method for setting the source cloud.
bool requiresSourcePoints() const
See if this rejector requires source points.
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.
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
const std::string & getClassName() const
Get a string representation of the name of this class.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
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...
boost::shared_ptr< const CorrespondenceRejectorPoly > ConstPtr
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.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Method for setting the target cloud.
std::string rejection_name_
The name of the rejection method.
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...
void setIterations(int iterations)
Set the number of iterations.
boost::shared_ptr< CorrespondenceRejectorPoly > Ptr
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.