Point Cloud Library (PCL)  1.9.1-dev
obj_rec_ransac.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  *
37  */
38 
39 #pragma once
40 
41 #include <pcl/recognition/ransac_based/hypothesis.h>
42 #include <pcl/recognition/ransac_based/model_library.h>
43 #include <pcl/recognition/ransac_based/rigid_transform_space.h>
44 #include <pcl/recognition/ransac_based/orr_octree.h>
45 #include <pcl/recognition/ransac_based/orr_octree_zprojection.h>
46 #include <pcl/recognition/ransac_based/simple_octree.h>
47 #include <pcl/recognition/ransac_based/trimmed_icp.h>
48 #include <pcl/recognition/ransac_based/orr_graph.h>
49 #include <pcl/recognition/ransac_based/auxiliary.h>
50 #include <pcl/recognition/ransac_based/bvh.h>
51 #include <pcl/registration/transformation_estimation_svd.h>
52 #include <pcl/correspondence.h>
53 #include <pcl/pcl_exports.h>
54 #include <pcl/point_cloud.h>
55 #include <cmath>
56 #include <string>
57 #include <vector>
58 #include <list>
59 
60 #define OBJ_REC_RANSAC_VERBOSE
61 
62 namespace pcl
63 {
64  namespace recognition
65  {
66  /** \brief This is a RANSAC-based 3D object recognition method. Do the following to use it: (i) call addModel() k times with k different models
67  * representing the objects to be recognized and (ii) call recognize() with the 3D scene in which the objects should be recognized. Recognition means both
68  * object identification and pose (position + orientation) estimation. Check the method descriptions for more details.
69  *
70  * \note If you use this code in any academic work, please cite:
71  *
72  * - Chavdar Papazov, Sami Haddadin, Sven Parusel, Kai Krieger and Darius Burschka.
73  * Rigid 3D geometry matching for grasping of known objects in cluttered scenes.
74  * The International Journal of Robotics Research 2012. DOI: 10.1177/0278364911436019
75  *
76  * - Chavdar Papazov and Darius Burschka.
77  * An Efficient RANSAC for 3D Object Recognition in Noisy and Occluded Scenes.
78  * In Proceedings of the 10th Asian Conference on Computer Vision (ACCV'10),
79  * November 2010.
80  *
81  *
82  * \author Chavdar Papazov
83  * \ingroup recognition
84  */
86  {
87  public:
90 
92 
93  /** \brief This is an output item of the ObjRecRANSAC::recognize() method. It contains the recognized model, its name (the ones passed to
94  * ObjRecRANSAC::addModel()), the rigid transform which aligns the model with the input scene and the match confidence which is a number
95  * in the interval (0, 1] which gives the fraction of the model surface area matched to the scene. E.g., a match confidence of 0.3 means
96  * that 30% of the object surface area was matched to the scene points. If the scene is represented by a single range image, the match
97  * confidence can not be greater than 0.5 since the range scanner sees only one side of each object.
98  */
99  class Output
100  {
101  public:
102  Output (const std::string& object_name, const float rigid_transform[12], float match_confidence, void* user_data) :
103  object_name_ (object_name),
104  match_confidence_ (match_confidence),
105  user_data_ (user_data)
106  {
107  memcpy(this->rigid_transform_, rigid_transform, 12*sizeof (float));
108  }
109  virtual ~Output (){}
110 
111  public:
112  std::string object_name_;
113  float rigid_transform_[12];
115  void* user_data_;
116  };
117 
119  {
120  public:
121  OrientedPointPair (const float *p1, const float *n1, const float *p2, const float *n2)
122  : p1_ (p1), n1_ (n1), p2_ (p2), n2_ (n2)
123  {
124  }
125 
126  virtual ~OrientedPointPair (){}
127 
128  public:
129  const float *p1_, *n1_, *p2_, *n2_;
130  };
131 
133  {
134  public:
136  virtual ~HypothesisCreator (){}
137 
139  };
140 
142 
143  public:
144  /** \brief Constructor with some important parameters which can not be changed once an instance of that class is created.
145  *
146  * \param[in] pair_width should be roughly half the extent of the visible object part. This means, for each object point p there should be (at least)
147  * one point q (from the same object) such that ||p - q|| <= pair_width. Tradeoff: smaller values allow for detection in more occluded scenes but lead
148  * to more imprecise alignment. Bigger values lead to better alignment but require large visible object parts (i.e., less occlusion).
149  *
150  * \param[in] voxel_size is the size of the leafs of the octree, i.e., the "size" of the discretization. Tradeoff: High values lead to less
151  * computation time but ignore object details. Small values allow to better distinguish between objects, but will introduce more holes in the resulting
152  * "voxel-surface" (especially for a sparsely sampled scene). */
153  ObjRecRANSAC (float pair_width, float voxel_size);
154  virtual ~ObjRecRANSAC ()
155  {
156  this->clear ();
157  this->clearTestData ();
158  }
159 
160  /** \brief Removes all models from the model library and releases some memory dynamically allocated by this instance. */
161  void
162  inline clear()
163  {
164  model_library_.removeAllModels ();
165  scene_octree_.clear ();
166  scene_octree_proj_.clear ();
167  sampled_oriented_point_pairs_.clear ();
168  transform_space_.clear ();
169  scene_octree_points_.reset ();
170  }
171 
172  /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will
173  * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if
174  * "ignore co-planar points" is on. Call this method before calling addModel. This method calls the corresponding
175  * method of the model library. */
176  inline void
177  setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees)
178  {
179  max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS;
180  model_library_.setMaxCoplanarityAngleDegrees (max_coplanarity_angle_degrees);
181  }
182 
183  inline void
185  {
186  scene_bounds_enlargement_factor_ = value;
187  }
188 
189  /** \brief Default is on. This method calls the corresponding method of the model library. */
190  inline void
192  {
193  ignore_coplanar_opps_ = true;
194  model_library_.ignoreCoplanarPointPairsOn ();
195  }
196 
197  /** \brief Default is on. This method calls the corresponding method of the model library. */
198  inline void
200  {
201  ignore_coplanar_opps_ = false;
202  model_library_.ignoreCoplanarPointPairsOff ();
203  }
204 
205  inline void
207  {
208  do_icp_hypotheses_refinement_ = true;
209  }
210 
211  inline void
213  {
214  do_icp_hypotheses_refinement_ = false;
215  }
216 
217  /** \brief Add an object model to be recognized.
218  *
219  * \param[in] points are the object points.
220  * \param[in] normals at each point.
221  * \param[in] object_name is an identifier for the object. If that object is detected in the scene 'object_name'
222  * is returned by the recognition method and you know which object has been detected. Note that 'object_name' has
223  * to be unique!
224  * \param[in] user_data is a pointer to some data (can be NULL)
225  *
226  * The method returns true if the model was successfully added to the model library and false otherwise (e.g., if 'object_name' is already in use).
227  */
228  inline bool
229  addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, void* user_data = nullptr)
230  {
231  return (model_library_.addModel (points, normals, object_name, frac_of_points_for_icp_refinement_, user_data));
232  }
233 
234  /** \brief This method performs the recognition of the models loaded to the model library with the method addModel().
235  *
236  * \param[in] scene is the 3d scene in which the object should be recognized.
237  * \param[in] normals are the scene normals.
238  * \param[out] recognized_objects is the list of output items each one containing the recognized model instance, its name, the aligning rigid transform
239  * and the match confidence (see ObjRecRANSAC::Output for further explanations).
240  * \param[in] success_probability is the user-defined probability of detecting all objects in the scene.
241  */
242  void
243  recognize (const PointCloudIn& scene, const PointCloudN& normals, std::list<ObjRecRANSAC::Output>& recognized_objects, double success_probability = 0.99);
244 
245  inline void
247  {
248  rec_mode_ = ObjRecRANSAC::SAMPLE_OPP;
249  }
250 
251  inline void
253  {
254  rec_mode_ = ObjRecRANSAC::TEST_HYPOTHESES;
255  }
256 
257  inline void
259  {
260  rec_mode_ = ObjRecRANSAC::FULL_RECOGNITION;
261  }
262 
263  /** \brief This function is useful for testing purposes. It returns the oriented point pairs which were sampled from the
264  * scene during the recognition process. Makes sense only if some of the testing modes are active. */
265  inline const std::list<ObjRecRANSAC::OrientedPointPair>&
267  {
268  return (sampled_oriented_point_pairs_);
269  }
270 
271  /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the
272  * recognition process. Makes sense only if some of the testing modes are active. */
273  inline const std::vector<Hypothesis>&
275  {
276  return (accepted_hypotheses_);
277  }
278 
279  /** \brief This function is useful for testing purposes. It returns the accepted hypotheses generated during the
280  * recognition process. Makes sense only if some of the testing modes are active. */
281  inline void
282  getAcceptedHypotheses (std::vector<Hypothesis>& out) const
283  {
284  out = accepted_hypotheses_;
285  }
286 
287  /** \brief Returns the hash table in the model library. */
289  getHashTable () const
290  {
291  return (model_library_.getHashTable ());
292  }
293 
294  inline const ModelLibrary&
296  {
297  return (model_library_);
298  }
299 
300  inline const ModelLibrary::Model*
301  getModel (const std::string& name) const
302  {
303  return (model_library_.getModel (name));
304  }
305 
306  inline const ORROctree&
307  getSceneOctree () const
308  {
309  return (scene_octree_);
310  }
311 
312  inline RigidTransformSpace&
314  {
315  return (transform_space_);
316  }
317 
318  inline float
319  getPairWidth () const
320  {
321  return pair_width_;
322  }
323 
324  protected:
325  enum Recognition_Mode {SAMPLE_OPP, TEST_HYPOTHESES, /*BUILD_CONFLICT_GRAPH,*/ FULL_RECOGNITION};
326 
327  friend class ModelLibrary;
328 
329  inline int
330  computeNumberOfIterations (double success_probability) const
331  {
332  // 'p_obj' is the probability that given that the first sample point belongs to an object,
333  // the second sample point will belong to the same object
334  const double p_obj = 0.25f;
335  // old version: p = p_obj*relative_obj_size_*fraction_of_pairs_in_hash_table_;
336  const double p = p_obj*relative_obj_size_;
337 
338  if ( 1.0 - p <= 0.0 )
339  return 1;
340 
341  return static_cast<int> (std::log (1.0-success_probability)/std::log (1.0-p) + 1.0);
342  }
343 
344  inline void
346  {
347  sampled_oriented_point_pairs_.clear ();
348  accepted_hypotheses_.clear ();
349  transform_space_.clear ();
350  }
351 
352  void
353  sampleOrientedPointPairs (int num_iterations, const std::vector<ORROctree::Node*>& full_scene_leaves, std::list<OrientedPointPair>& output) const;
354 
355  int
356  generateHypotheses (const std::list<OrientedPointPair>& pairs, std::list<HypothesisBase>& out) const;
357 
358  /** \brief Groups close hypotheses in 'hypotheses'. Saves a representative for each group in 'out'. Returns the
359  * number of hypotheses after grouping. */
360  int
361  groupHypotheses(std::list<HypothesisBase>& hypotheses, int num_hypotheses, RigidTransformSpace& transform_space,
362  HypothesisOctree& grouped_hypotheses) const;
363 
364  inline void
365  testHypothesis (Hypothesis* hypothesis, int& match, int& penalty) const;
366 
367  inline void
368  testHypothesisNormalBased (Hypothesis* hypothesis, float& match) const;
369 
370  void
371  buildGraphOfCloseHypotheses (HypothesisOctree& hypotheses, ORRGraph<Hypothesis>& graph) const;
372 
373  void
374  filterGraphOfCloseHypotheses (ORRGraph<Hypothesis>& graph, std::vector<Hypothesis>& out) const;
375 
376  void
377  buildGraphOfConflictingHypotheses (const BVHH& bvh, ORRGraph<Hypothesis*>& graph) const;
378 
379  void
380  filterGraphOfConflictingHypotheses (ORRGraph<Hypothesis*>& graph, std::list<ObjRecRANSAC::Output>& recognized_objects) const;
381 
382  /** \brief Computes the rigid transform that maps the line (a1, b1) to (a2, b2).
383  * The computation is based on the corresponding points 'a1' <-> 'a2' and 'b1' <-> 'b2'
384  * and the normals 'a1_n', 'b1_n', 'a2_n', and 'b2_n'. The result is saved in
385  * 'rigid_transform' which is an array of length 12. The first 9 elements are the
386  * rotational part (row major order) and the last 3 are the translation. */
387  inline void
389  const float *a1, const float *a1_n, const float *b1, const float* b1_n,
390  const float *a2, const float *a2_n, const float *b2, const float* b2_n,
391  float* rigid_transform) const
392  {
393  // Some local variables
394  float o1[3], o2[3], x1[3], x2[3], y1[3], y2[3], z1[3], z2[3], tmp1[3], tmp2[3], Ro1[3], invFrame1[3][3];
395 
396  // Compute the origins
397  o1[0] = 0.5f*(a1[0] + b1[0]);
398  o1[1] = 0.5f*(a1[1] + b1[1]);
399  o1[2] = 0.5f*(a1[2] + b1[2]);
400 
401  o2[0] = 0.5f*(a2[0] + b2[0]);
402  o2[1] = 0.5f*(a2[1] + b2[1]);
403  o2[2] = 0.5f*(a2[2] + b2[2]);
404 
405  // Compute the x-axes
406  aux::diff3 (b1, a1, x1); aux::normalize3 (x1);
407  aux::diff3 (b2, a2, x2); aux::normalize3 (x2);
408  // Compute the y-axes. First y-axis
409  aux::projectOnPlane3 (a1_n, x1, tmp1); aux::normalize3 (tmp1);
410  aux::projectOnPlane3 (b1_n, x1, tmp2); aux::normalize3 (tmp2);
411  aux::sum3 (tmp1, tmp2, y1); aux::normalize3 (y1);
412  // Second y-axis
413  aux::projectOnPlane3 (a2_n, x2, tmp1); aux::normalize3 (tmp1);
414  aux::projectOnPlane3 (b2_n, x2, tmp2); aux::normalize3 (tmp2);
415  aux::sum3 (tmp1, tmp2, y2); aux::normalize3 (y2);
416  // Compute the z-axes
417  aux::cross3 (x1, y1, z1);
418  aux::cross3 (x2, y2, z2);
419 
420  // 1. Invert the matrix [x1|y1|z1] (note that x1, y1, and z1 are treated as columns!)
421  invFrame1[0][0] = x1[0]; invFrame1[0][1] = x1[1]; invFrame1[0][2] = x1[2];
422  invFrame1[1][0] = y1[0]; invFrame1[1][1] = y1[1]; invFrame1[1][2] = y1[2];
423  invFrame1[2][0] = z1[0]; invFrame1[2][1] = z1[1]; invFrame1[2][2] = z1[2];
424  // 2. Compute the desired rotation as rigid_transform = [x2|y2|z2]*invFrame1
425  aux::mult3x3 (x2, y2, z2, invFrame1, rigid_transform);
426 
427  // Construct the translation which is the difference between the rotated o1 and o2
428  aux::mult3x3 (rigid_transform, o1, Ro1);
429  rigid_transform[9] = o2[0] - Ro1[0];
430  rigid_transform[10] = o2[1] - Ro1[1];
431  rigid_transform[11] = o2[2] - Ro1[2];
432  }
433 
434  /** \brief Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles between
435  * \param p1
436  * \param n1
437  * \param p2
438  * \param n2
439  * \param[out] signature is an array of three doubles saving the three angles in the order shown above. */
440  static inline void
441  compute_oriented_point_pair_signature (const float *p1, const float *n1, const float *p2, const float *n2, float signature[3])
442  {
443  // Get the line from p1 to p2
444  float cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
445  aux::normalize3 (cl);
446 
447  signature[0] = std::acos (aux::clamp (aux::dot3 (n1,cl), -1.0f, 1.0f)); cl[0] = -cl[0]; cl[1] = -cl[1]; cl[2] = -cl[2];
448  signature[1] = std::acos (aux::clamp (aux::dot3 (n2,cl), -1.0f, 1.0f));
449  signature[2] = std::acos (aux::clamp (aux::dot3 (n1,n2), -1.0f, 1.0f));
450  }
451 
452  protected:
453  // Parameters
454  float pair_width_;
455  float voxel_size_;
460  float visibility_;
468 
475 
476  std::list<OrientedPointPair> sampled_oriented_point_pairs_;
477  std::vector<Hypothesis> accepted_hypotheses_;
479  };
480  } // namespace recognition
481 } // namespace pcl
ORROctreeZProjection scene_octree_proj_
const ModelLibrary & getModelLibrary() const
Hypothesis * create(const SimpleOctree< Hypothesis, HypothesisCreator, float >::Node *) const
TrimmedICP< pcl::PointXYZ, float > trimmed_icp_
const ORROctree & getSceneOctree() const
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void mult3x3(const T m[9], const T v[3], T out[3])
out = mat*v.
Definition: auxiliary.h:275
Stores some information about the model.
Definition: model_library.h:65
PointCloudIn::Ptr scene_octree_points_
void setSceneBoundsEnlargementFactor(float value)
This is a RANSAC-based 3D object recognition method.
void getAcceptedHypotheses(std::vector< Hypothesis > &out) const
This function is useful for testing purposes.
void projectOnPlane3(const T x[3], const T planeNormal[3], T out[3])
Projects &#39;x&#39; on the plane through 0 and with normal &#39;planeNormal&#39; and saves the result in &#39;out&#39;...
Definition: auxiliary.h:257
RigidTransformSpace & getRigidTransformSpace()
const std::list< ObjRecRANSAC::OrientedPointPair > & getSampledOrientedPointPairs() const
This function is useful for testing purposes.
void cross3(const T v1[3], const T v2[3], T out[3])
Definition: auxiliary.h:177
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
Definition: auxiliary.h:208
std::vector< Hypothesis > accepted_hypotheses_
void computeRigidTransform(const float *a1, const float *a1_n, const float *b1, const float *b1_n, const float *a2, const float *a2_n, const float *b2, const float *b2_n, float *rigid_transform) const
Computes the rigid transform that maps the line (a1, b1) to (a2, b2).
T clamp(T value, T min, T max)
Definition: auxiliary.h:71
void ignoreCoplanarPointPairsOn()
Default is on.
boost::shared_ptr< PointCloud< pcl::PointXYZ > > Ptr
Definition: point_cloud.h:441
Output(const std::string &object_name, const float rigid_transform[12], float match_confidence, void *user_data)
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
Definition: auxiliary.h:160
This is an output item of the ObjRecRANSAC::recognize() method.
pcl::PointCloud< pcl::PointXYZ > PointCloudIn
Definition: model_library.h:61
static void compute_oriented_point_pair_signature(const float *p1, const float *n1, const float *p2, const float *n2, float signature[3])
Computes the signature of the oriented point pair ((p1, n1), (p2, n2)) consisting of the angles betwe...
std::list< OrientedPointPair > sampled_oriented_point_pairs_
This class is an implementation of bounding volume hierarchies.
Definition: bvh.h:65
RigidTransformSpace transform_space_
void normalize3(T v[3])
Normalize v.
Definition: auxiliary.h:240
pcl::PointCloud< pcl::Normal > PointCloudN
Definition: model_library.h:62
void diff3(const T a[3], const T b[3], T c[3])
c = a - b
Definition: auxiliary.h:169
void setMaxCoplanarityAngleDegrees(float max_coplanarity_angle_degrees)
This is a threshold.
void clear()
Removes all models from the model library and releases some memory dynamically allocated by this inst...
const pcl::recognition::ModelLibrary::HashTable & getHashTable() const
Returns the hash table in the model library.
const ModelLibrary::Model * getModel(const std::string &name) const
const std::vector< Hypothesis > & getAcceptedHypotheses() const
This function is useful for testing purposes.
int computeNumberOfIterations(double success_probability) const
#define PCL_EXPORTS
Definition: pcl_macros.h:226
OrientedPointPair(const float *p1, const float *n1, const float *p2, const float *n2)
void ignoreCoplanarPointPairsOff()
Default is on.
That&#39;s a very specialized and simple octree class.
Definition: orr_octree.h:70
bool addModel(const PointCloudIn &points, const PointCloudN &normals, const std::string &object_name, void *user_data=nullptr)
Add an object model to be recognized.