Point Cloud Library (PCL)  1.7.1
model_library.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 #ifndef PCL_RECOGNITION_MODEL_LIBRARY_H_
40 #define PCL_RECOGNITION_MODEL_LIBRARY_H_
41 
42 #include "auxiliary.h"
43 #include <pcl/recognition/ransac_based/voxel_structure.h>
44 #include <pcl/recognition/ransac_based/orr_octree.h>
45 #include <pcl/common/random.h>
46 #include <pcl/pcl_exports.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/point_types.h>
49 #include <ctime>
50 #include <string>
51 #include <list>
52 #include <set>
53 #include <map>
54 
55 namespace pcl
56 {
57  namespace recognition
58  {
59  class PCL_EXPORTS ModelLibrary
60  {
61  public:
64 
65  /** \brief Stores some information about the model. */
66  class Model
67  {
68  public:
69  Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name,
70  float frac_of_points_for_registration, void* user_data = NULL)
71  : obj_name_(object_name),
72  user_data_ (user_data)
73  {
74  octree_.build (points, voxel_size, &normals);
75 
76  const std::vector<ORROctree::Node*>& full_leaves = octree_.getFullLeaves ();
77  if ( full_leaves.empty () )
78  return;
79 
80  // Initialize
81  std::vector<ORROctree::Node*>::const_iterator it = full_leaves.begin ();
82  const float *p = (*it)->getData ()->getPoint ();
83  aux::copy3 (p, octree_center_of_mass_);
84  bounds_of_octree_points_[0] = bounds_of_octree_points_[1] = p[0];
85  bounds_of_octree_points_[2] = bounds_of_octree_points_[3] = p[1];
86  bounds_of_octree_points_[4] = bounds_of_octree_points_[5] = p[2];
87 
88  // Compute both the bounds and the center of mass of the octree points
89  for ( ++it ; it != full_leaves.end () ; ++it )
90  {
91  aux::add3 (octree_center_of_mass_, (*it)->getData ()->getPoint ());
92  aux::expandBoundingBoxToContainPoint (bounds_of_octree_points_, (*it)->getData ()->getPoint ());
93  }
94 
95  int num_octree_points = static_cast<int> (full_leaves.size ());
96  // Finalize the center of mass computation
97  aux::mult3 (octree_center_of_mass_, 1.0f/static_cast<float> (num_octree_points));
98 
99  int num_points_for_registration = static_cast<int> (static_cast<float> (num_octree_points)*frac_of_points_for_registration);
100  points_for_registration_.resize (static_cast<size_t> (num_points_for_registration));
101 
102  // Prepare for random point sampling
103  std::vector<int> ids (num_octree_points);
104  for ( int i = 0 ; i < num_octree_points ; ++i )
105  ids[i] = i;
106 
107  // The random generator
108  pcl::common::UniformGenerator<int> randgen (0, num_octree_points - 1, static_cast<uint32_t> (time (NULL)));
109 
110  // Randomly sample some points from the octree
111  for ( int i = 0 ; i < num_points_for_registration ; ++i )
112  {
113  // Choose a random position within the array of ids
114  randgen.setParameters (0, static_cast<int> (ids.size ()) - 1);
115  int rand_pos = randgen.run ();
116 
117  // Copy the randomly selected octree point
118  aux::copy3 (octree_.getFullLeaves ()[ids[rand_pos]]->getData ()->getPoint (), points_for_registration_[i]);
119 
120  // Delete the selected id
121  ids.erase (ids.begin() + rand_pos);
122  }
123  }
124 
125  virtual ~Model ()
126  {
127  }
128 
129  inline const std::string&
130  getObjectName () const
131  {
132  return (obj_name_);
133  }
134 
135  inline const ORROctree&
136  getOctree () const
137  {
138  return (octree_);
139  }
140 
141  inline void*
142  getUserData () const
143  {
144  return (user_data_);
145  }
146 
147  inline const float*
149  {
150  return (octree_center_of_mass_);
151  }
152 
153  inline const float*
155  {
156  return (bounds_of_octree_points_);
157  }
158 
159  inline const PointCloudIn&
161  {
162  return (points_for_registration_);
163  }
164 
165  protected:
166  const std::string obj_name_;
168  float octree_center_of_mass_[3];
169  float bounds_of_octree_points_[6];
171  void* user_data_;
172  };
173 
174  typedef std::list<std::pair<const ORROctree::Node::Data*, const ORROctree::Node::Data*> > node_data_pair_list;
175  typedef std::map<const Model*, node_data_pair_list> HashTableCell;
177 
178  public:
179  /** \brief This class is used by 'ObjRecRANSAC' to maintain the object models to be recognized. Normally, you do not need to use
180  * this class directly. */
181  ModelLibrary (float pair_width, float voxel_size, float max_coplanarity_angle = 3.0f*AUX_DEG_TO_RADIANS/*3 degrees*/);
182  virtual ~ModelLibrary ()
183  {
184  this->clear();
185  }
186 
187  /** \brief Removes all models from the library and clears the hash table. */
188  void
189  removeAllModels ();
190 
191  /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will
192  * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if
193  * "ignore co-planar points" is on. Call this method before calling addModel. */
194  inline void
195  setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees)
196  {
197  max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS;
198  }
199 
200  /** \biref Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior
201  * is ignoring co-planar points on. */
202  inline void
204  {
205  ignore_coplanar_opps_ = true;
206  }
207 
208  /** \biref Call this method in order to add all point pairs (co-planar as well) to the hash table. The default
209  * behavior is ignoring co-planar points on. */
210  inline void
212  {
213  ignore_coplanar_opps_ = false;
214  }
215 
216  /** \brief Adds a model to the hash table.
217  *
218  * \param[in] points represents the model to be added.
219  * \param[in] normals are the normals at the model points.
220  * \param[in] object_name is the unique name of the object to be added.
221  * \param[in] num_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing
222  * \param[in] user_data is a pointer to some data (can be NULL)
223  *
224  * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */
225  bool
226  addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name,
227  float frac_of_points_for_registration, void* user_data = NULL);
228 
229  /** \brief Returns the hash table built by this instance. */
230  inline const HashTable&
231  getHashTable () const
232  {
233  return (hash_table_);
234  }
235 
236  inline const Model*
237  getModel (const std::string& name) const
238  {
239  std::map<std::string,Model*>::const_iterator it = models_.find (name);
240  if ( it != models_.end () )
241  return (it->second);
242 
243  return (NULL);
244  }
245 
246  inline const std::map<std::string,Model*>&
247  getModels () const
248  {
249  return (models_);
250  }
251 
252  protected:
253  /** \brief Removes all models from the library and destroys the hash table. This method should be called upon destroying this object. */
254  void
255  clear ();
256 
257  /** \brief Returns true if the oriented point pair was added to the hash table and false otherwise. */
258  bool
259  addToHashTable (Model* model, const ORROctree::Node::Data* data1, const ORROctree::Node::Data* data2);
260 
261  protected:
262  float pair_width_;
263  float voxel_size_;
266 
267  std::map<std::string,Model*> models_;
269  int num_of_cells_[3];
270  };
271  } // namespace recognition
272 } // namespace pcl
273 
274 #endif // PCL_RECOGNITION_MODEL_LIBRARY_H_