Point Cloud Library (PCL)  1.7.0
hough_3d.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id:$
37  *
38  */
39 
40 #ifndef PCL_RECOGNITION_HOUGH_3D_H_
41 #define PCL_RECOGNITION_HOUGH_3D_H_
42 
43 #include <pcl/recognition/cg/correspondence_grouping.h>
44 #include <pcl/recognition/boost.h>
45 
46 namespace pcl
47 {
48  namespace recognition
49  {
50  /** \brief HoughSpace3D is a 3D voting space. Cast votes can be interpolated in order to better deal with approximations introduced by bin quantization. A weight can also be associated with each vote.
51  * \author Federico Tombari (original), Tommaso Cavallari (PCL port)
52  * \ingroup recognition
53  */
54  class PCL_EXPORTS HoughSpace3D
55  {
56 
57  public:
58 
59  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 
61  /** \brief Constructor
62  *
63  * \param[in] min_coord minimum (x,y,z) coordinates of the Hough space
64  * \param[in] bin_size size of each bing of the Hough space.
65  * \param[in] max_coord maximum (x,y,z) coordinates of the Hough space.
66  */
67  HoughSpace3D (const Eigen::Vector3d &min_coord, const Eigen::Vector3d &bin_size, const Eigen::Vector3d &max_coord);
68 
69  /** \brief Reset all cast votes. */
70  void
71  reset ();
72 
73  /** \brief Casting a vote for a given position in the Hough space.
74  *
75  * \param[in] single_vote_coord coordinates of the vote being cast (in absolute coordinates)
76  * \param[in] weight weight associated with the vote.
77  * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as part of a maximum of the Hough Space.
78  * \return the index of the bin in which the vote has been cast.
79  */
80  int
81  vote (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);
82 
83  /** \brief Vote for a given position in the 3D space. The weight is interpolated between the bin pointed by single_vote_coord and its neighbors.
84  *
85  * \param[in] single_vote_coord coordinates of the vote being cast.
86  * \param[in] weight weight associated with the vote.
87  * \param[in] voter_id the numeric id of the voter. Useful to trace back the voting correspondence, if the vote is returned by findMaxima as a part of a maximum of the Hough Space.
88  * \return the index of the bin in which the vote has been cast.
89  */
90  int
91  voteInt (const Eigen::Vector3d &single_vote_coord, double weight, int voter_id);
92 
93  /** \brief Find the bins with most votes.
94  *
95  * \param[in] min_threshold the minimum number of votes to be included in a bin in order to have its value returned.
96  * If set to a value between -1 and 0 the Hough space maximum_vote is found and the returned values are all the votes greater than -min_threshold * maximum_vote.
97  * \param[out] maxima_values the list of Hough Space bin values greater than min_threshold.
98  * \param[out] maxima_voter_ids for each value returned, a list of the voter ids who cast a vote in that position.
99  * \return The min_threshold used, either set by the user or found by this method.
100  */
101  double
102  findMaxima (double min_threshold, std::vector<double> & maxima_values, std::vector<std::vector<int> > &maxima_voter_ids);
103 
104  protected:
105 
106  /** \brief Minimum coordinate in the Hough Space. */
107  Eigen::Vector3d min_coord_;
108 
109  /** \brief Size of each bin in the Hough Space. */
110  Eigen::Vector3d bin_size_;
111 
112  /** \brief Number of bins for each dimension. */
113  Eigen::Vector3i bin_count_;
114 
115  /** \brief Used to access hough_space_ as if it was a matrix. */
116  int partial_bin_products_[4];
117 
118  /** \brief Total number of bins in the Hough Space. */
120 
121  /** \brief The Hough Space. */
122  std::vector<double> hough_space_;
123  //boost::unordered_map<int, double> hough_space_;
124 
125  /** \brief List of voters for each bin. */
126  boost::unordered_map<int, std::vector<int> > voter_ids_;
127  };
128  }
129 
130  /** \brief Class implementing a 3D correspondence grouping algorithm that can deal with multiple instances of a model template
131  * found into a given scene. Each correspondence casts a vote for a reference point in a 3D Hough Space.
132  * The remaining 3 DOF are taken into account by associating each correspondence with a local Reference Frame.
133  * The suggested PointModelRfT is pcl::ReferenceFrame
134  *
135  * \note If you use this code in any academic work, please cite the original paper:
136  * - F. Tombari, L. Di Stefano:
137  * Object recognition in 3D scenes with occlusions and clutter by Hough voting.
138  * 2010, Fourth Pacific-Rim Symposium on Image and Video Technology
139  *
140  * \author Federico Tombari (original), Tommaso Cavallari (PCL port)
141  * \ingroup recognition
142  */
143  template<typename PointModelT, typename PointSceneT, typename PointModelRfT = pcl::ReferenceFrame, typename PointSceneRfT = pcl::ReferenceFrame>
144  class Hough3DGrouping : public CorrespondenceGrouping<PointModelT, PointSceneT>
145  {
146  public:
150 
154 
156  typedef typename PointCloud::Ptr PointCloudPtr;
158 
160 
161  /** \brief Constructor */
163  : input_rf_ ()
164  , scene_rf_ ()
165  , needs_training_ (true)
166  , model_votes_ ()
167  , hough_threshold_ (-1)
168  , hough_bin_size_ (1.0)
169  , use_interpolation_ (true)
170  , use_distance_weight_ (false)
172  , local_rf_search_radius_ (0.0f)
173  , hough_space_ ()
175  , hough_space_initialized_ (false)
176  {}
177 
178  /** \brief Provide a pointer to the input dataset.
179  * \param[in] cloud the const boost shared pointer to a PointCloud message.
180  */
181  inline void
183  {
185  needs_training_ = true;
186  hough_space_initialized_ = false;
187  input_rf_.reset();
188  }
189 
190  /** \brief Provide a pointer to the input dataset's reference frames.
191  * Each point in the reference frame cloud should be the reference frame of
192  * the correspondent point in the input dataset.
193  *
194  * \param[in] input_rf the pointer to the input cloud's reference frames.
195  */
196  inline void
198  {
199  input_rf_ = input_rf;
200  needs_training_ = true;
201  hough_space_initialized_ = false;
202  }
203 
204  /** \brief Getter for the input dataset's reference frames.
205  * Each point in the reference frame cloud should be the reference frame of
206  * the correspondent point in the input dataset.
207  *
208  * \return the pointer to the input cloud's reference frames.
209  */
210  inline ModelRfCloudConstPtr
211  getInputRf () const
212  {
213  return (input_rf_);
214  }
215 
216  /** \brief Provide a pointer to the scene dataset (i.e. the cloud in which the algorithm has to search for instances of the input model)
217  *
218  * \param[in] scene the const boost shared pointer to a PointCloud message.
219  */
220  inline void
222  {
223  scene_ = scene;
224  hough_space_initialized_ = false;
225  scene_rf_.reset();
226  }
227 
228  /** \brief Provide a pointer to the scene dataset's reference frames.
229  * Each point in the reference frame cloud should be the reference frame of
230  * the correspondent point in the scene dataset.
231  *
232  * \param[in] scene_rf the pointer to the scene cloud's reference frames.
233  */
234  inline void
236  {
237  scene_rf_ = scene_rf;
238  hough_space_initialized_ = false;
239  }
240 
241  /** \brief Getter for the scene dataset's reference frames.
242  * Each point in the reference frame cloud should be the reference frame of
243  * the correspondent point in the scene dataset.
244  *
245  * \return the pointer to the scene cloud's reference frames.
246  */
247  inline SceneRfCloudConstPtr
248  getSceneRf () const
249  {
250  return (scene_rf_);
251  }
252 
253  /** \brief Provide a pointer to the precomputed correspondences between points in the input dataset and
254  * points in the scene dataset. The correspondences are going to be clustered into different model instances
255  * by the algorithm.
256  *
257  * \param[in] corrs the correspondences between the model and the scene.
258  */
259  inline void
261  {
262  model_scene_corrs_ = corrs;
263  hough_space_initialized_ = false;
264  }
265 
266  /** \brief Sets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud.
267  *
268  * \param[in] threshold the threshold for the Hough space voting, if set between -1 and 0 the maximum vote in the
269  * entire space is automatically calculated and -threshold the maximum value is used as a threshold. This means
270  * that a value between -1 and 0 should be used only if at least one instance of the model is always present in
271  * the scene, or if this false positive can be filtered later.
272  */
273  inline void
274  setHoughThreshold (double threshold)
275  {
276  hough_threshold_ = threshold;
277  }
278 
279  /** \brief Gets the minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud.
280  *
281  * \return the threshold for the Hough space voting.
282  */
283  inline double
285  {
286  return (hough_threshold_);
287  }
288 
289  /** \brief Sets the size of each bin into the Hough space.
290  *
291  * \param[in] bin_size the size of each Hough space's bin.
292  */
293  inline void
294  setHoughBinSize (double bin_size)
295  {
296  hough_bin_size_ = bin_size;
297  hough_space_initialized_ = false;
298  }
299 
300  /** \brief Gets the size of each bin into the Hough space.
301  *
302  * \return the size of each Hough space's bin.
303  */
304  inline double
306  {
307  return (hough_bin_size_);
308  }
309 
310  /** \brief Sets whether the vote casting procedure interpolates
311  * the score between neighboring bins of the Hough space or not.
312  *
313  * \param[in] use_interpolation the algorithm should interpolate the vote score between neighboring bins.
314  */
315  inline void
316  setUseInterpolation (bool use_interpolation)
317  {
318  use_interpolation_ = use_interpolation;
319  hough_space_initialized_ = false;
320  }
321 
322  /** \brief Gets whether the vote casting procedure interpolates
323  * the score between neighboring bins of the Hough space or not.
324  *
325  * \return if the algorithm should interpolate the vote score between neighboring bins.
326  */
327  inline bool
329  {
330  return (use_interpolation_);
331  }
332 
333  /** \brief Sets whether the vote casting procedure uses the correspondence's distance as a score.
334  *
335  * \param[in] use_distance_weight the algorithm should use the weighted distance when calculating the Hough voting score.
336  */
337  inline void
338  setUseDistanceWeight (bool use_distance_weight)
339  {
340  use_distance_weight_ = use_distance_weight;
341  hough_space_initialized_ = false;
342  }
343 
344  /** \brief Gets whether the vote casting procedure uses the correspondence's distance as a score.
345  *
346  * \return if the algorithm should use the weighted distance when calculating the Hough voting score.
347  */
348  inline bool
350  {
351  return (use_distance_weight_);
352  }
353 
354  /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
355  * this algorithm makes the computation itself but needs a suitable search radius to compute the normals
356  * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed).
357  *
358  * \param[in] local_rf_normals_search_radius the normals search radius for the local reference frame calculation.
359  */
360  inline void
361  setLocalRfNormalsSearchRadius (float local_rf_normals_search_radius)
362  {
363  local_rf_normals_search_radius_ = local_rf_normals_search_radius;
364  needs_training_ = true;
365  hough_space_initialized_ = false;
366  }
367 
368  /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
369  * this algorithm makes the computation itself but needs a suitable search radius to compute the normals
370  * in order to subsequently compute the RF (if not set a default 15 nearest neighbors search is performed).
371  *
372  * \return the normals search radius for the local reference frame calculation.
373  */
374  inline float
376  {
378  }
379 
380  /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
381  * this algorithm makes the computation itself but needs a suitable search radius to do so.
382  * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally,
383  * otherwise the recognition results won't be correct.
384  *
385  * \param[in] local_rf_search_radius the search radius for the local reference frame calculation.
386  */
387  inline void
388  setLocalRfSearchRadius (float local_rf_search_radius)
389  {
390  local_rf_search_radius_ = local_rf_search_radius;
391  needs_training_ = true;
392  hough_space_initialized_ = false;
393  }
394 
395  /** \brief If the Local reference frame has not been set for either the model cloud or the scene cloud,
396  * this algorithm makes the computation itself but needs a suitable search radius to do so.
397  * \attention This parameter NEEDS to be set if the reference frames are not precomputed externally,
398  * otherwise the recognition results won't be correct.
399  *
400  * \return the search radius for the local reference frame calculation.
401  */
402  inline float
404  {
405  return (local_rf_search_radius_);
406  }
407 
408  /** \brief Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform an off line training of the algorithm. This might be useful if one wants to perform once and for all a pre-computation of votes that only concern the models, increasing the on-line efficiency of the grouping algorithm.
409  * The algorithm is automatically trained on the first invocation of the recognize method or the cluster method if this training function has not been manually invoked.
410  *
411  * \return true if the training had been successful or false if errors have occurred.
412  */
413  bool
414  train ();
415 
416  /** \brief The main function, recognizes instances of the model into the scene set by the user.
417  *
418  * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
419  *
420  * \return true if the recognition had been successful or false if errors have occurred.
421  */
422  bool
423  recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations);
424 
425  /** \brief The main function, recognizes instances of the model into the scene set by the user.
426  *
427  * \param[out] transformations a vector containing one transformation matrix for each instance of the model recognized into the scene.
428  * \param[out] clustered_corrs a vector containing the correspondences for each instance of the model found within the input data (the same output of clusterCorrespondences).
429  *
430  * \return true if the recognition had been successful or false if errors have occurred.
431  */
432  bool
433  recognize (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs);
434 
435  protected:
439 
440  /** \brief The input Rf cloud. */
442 
443  /** \brief The scene Rf cloud. */
445 
446  /** \brief If the training of the Hough space is needed; set on change of either the input cloud or the input_rf. */
448 
449  /** \brief The result of the training. The vector between each model point and the centroid of the model adjusted by its local reference frame.*/
450  std::vector<Eigen::Vector3f> model_votes_;
451 
452  /** \brief The minimum number of votes in the Hough space needed to infer the presence of a model instance into the scene cloud. */
454 
455  /** \brief The size of each bin of the hough space. */
457 
458  /** \brief Use the interpolation between neighboring Hough bins when casting votes. */
460 
461  /** \brief Use the weighted correspondence distance when casting votes. */
463 
464  /** \brief Normals search radius for the potential Rf calculation. */
466 
467  /** \brief Search radius for the potential Rf calculation. */
469 
470  /** \brief The Hough space. */
471  boost::shared_ptr<pcl::recognition::HoughSpace3D> hough_space_;
472 
473  /** \brief Transformations found by clusterCorrespondences method. */
474  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > found_transformations_;
475 
476  /** \brief Whether the Hough space already contains the correct votes for the current input parameters and so the cluster and recognize calls don't need to recompute each value.
477  * Reset on the change of any parameter except the hough_threshold.
478  */
480 
481  /** \brief Cluster the input correspondences in order to distinguish between different instances of the model into the scene.
482  *
483  * \param[out] model_instances a vector containing the clustered correspondences for each model found on the scene.
484  * \return true if the clustering had been successful or false if errors have occurred.
485  */
486  void
487  clusterCorrespondences (std::vector<Correspondences> &model_instances);
488 
489  ///** \brief Finds the transformation matrix between the input and the scene cloud for a set of correspondences using a RANSAC algorithm.
490  // *
491  // * \param[in] the scene cloud in which the PointSceneT has been converted to PointModelT.
492  // * \param[in] corrs a set of correspondences.
493  // * \param[out] transform the transformation matrix between the input cloud and the scene cloud that aligns the found correspondences.
494  // * \return true if the recognition had been successful or false if errors have occurred.
495  // */
496  //bool
497  //getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform);
498 
499  /** \brief The Hough space voting procedure.
500  *
501  * \return true if the voting had been successful or false if errors have occurred.
502  */
503  bool
504  houghVoting ();
505 
506  /** \brief Computes the reference frame for an input cloud.
507  *
508  * \param[in] input the input cloud.
509  * \param[out] rf the resulting reference frame.
510  */
511  template<typename PointType, typename PointRfType> void
512  computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf);
513  };
514 }
515 
516 #ifdef PCL_NO_PRECOMPILE
517 #include <pcl/recognition/impl/cg/hough_3d.hpp>
518 #endif
519 
520 #endif // PCL_RECOGNITION_HOUGH_3D_H_