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