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