Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * 00037 */ 00038 00039 #ifndef PCL_RECOGNITION_MODEL_LIBRARY_H_ 00040 #define PCL_RECOGNITION_MODEL_LIBRARY_H_ 00041 00042 #include "auxiliary.h" 00043 #include <pcl/recognition/ransac_based/voxel_structure.h> 00044 #include <pcl/recognition/ransac_based/orr_octree.h> 00045 #include <pcl/common/random.h> 00046 #include <pcl/pcl_exports.h> 00047 #include <pcl/point_cloud.h> 00048 #include <pcl/point_types.h> 00049 #include <ctime> 00050 #include <string> 00051 #include <list> 00052 #include <set> 00053 #include <map> 00054 00055 namespace pcl 00056 { 00057 namespace recognition 00058 { 00059 class PCL_EXPORTS ModelLibrary 00060 { 00061 public: 00062 typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn; 00063 typedef pcl::PointCloud<pcl::Normal> PointCloudN; 00064 00065 /** \brief Stores some information about the model. */ 00066 class Model 00067 { 00068 public: 00069 Model (const PointCloudIn& points, const PointCloudN& normals, float voxel_size, const std::string& object_name, 00070 float frac_of_points_for_registration, void* user_data = NULL) 00071 : obj_name_(object_name), 00072 user_data_ (user_data) 00073 { 00074 octree_.build (points, voxel_size, &normals); 00075 00076 const std::vector<ORROctree::Node*>& full_leaves = octree_.getFullLeaves (); 00077 if ( full_leaves.empty () ) 00078 return; 00079 00080 // Initialize 00081 std::vector<ORROctree::Node*>::const_iterator it = full_leaves.begin (); 00082 const float *p = (*it)->getData ()->getPoint (); 00083 aux::copy3 (p, octree_center_of_mass_); 00084 bounds_of_octree_points_[0] = bounds_of_octree_points_[1] = p[0]; 00085 bounds_of_octree_points_[2] = bounds_of_octree_points_[3] = p[1]; 00086 bounds_of_octree_points_[4] = bounds_of_octree_points_[5] = p[2]; 00087 00088 // Compute both the bounds and the center of mass of the octree points 00089 for ( ++it ; it != full_leaves.end () ; ++it ) 00090 { 00091 aux::add3 (octree_center_of_mass_, (*it)->getData ()->getPoint ()); 00092 aux::expandBoundingBoxToContainPoint (bounds_of_octree_points_, (*it)->getData ()->getPoint ()); 00093 } 00094 00095 int num_octree_points = static_cast<int> (full_leaves.size ()); 00096 // Finalize the center of mass computation 00097 aux::mult3 (octree_center_of_mass_, 1.0f/static_cast<float> (num_octree_points)); 00098 00099 int num_points_for_registration = static_cast<int> (static_cast<float> (num_octree_points)*frac_of_points_for_registration); 00100 points_for_registration_.resize (static_cast<size_t> (num_points_for_registration)); 00101 00102 // Prepare for random point sampling 00103 std::vector<int> ids (num_octree_points); 00104 for ( int i = 0 ; i < num_octree_points ; ++i ) 00105 ids[i] = i; 00106 00107 // The random generator 00108 pcl::common::UniformGenerator<int> randgen (0, num_octree_points - 1, static_cast<uint32_t> (time (NULL))); 00109 00110 // Randomly sample some points from the octree 00111 for ( int i = 0 ; i < num_points_for_registration ; ++i ) 00112 { 00113 // Choose a random position within the array of ids 00114 randgen.setParameters (0, static_cast<int> (ids.size ()) - 1); 00115 int rand_pos = randgen.run (); 00116 00117 // Copy the randomly selected octree point 00118 aux::copy3 (octree_.getFullLeaves ()[ids[rand_pos]]->getData ()->getPoint (), points_for_registration_[i]); 00119 00120 // Delete the selected id 00121 ids.erase (ids.begin() + rand_pos); 00122 } 00123 } 00124 00125 virtual ~Model () 00126 { 00127 } 00128 00129 inline const std::string& 00130 getObjectName () const 00131 { 00132 return (obj_name_); 00133 } 00134 00135 inline const ORROctree& 00136 getOctree () const 00137 { 00138 return (octree_); 00139 } 00140 00141 inline void* 00142 getUserData () const 00143 { 00144 return (user_data_); 00145 } 00146 00147 inline const float* 00148 getOctreeCenterOfMass () const 00149 { 00150 return (octree_center_of_mass_); 00151 } 00152 00153 inline const float* 00154 getBoundsOfOctreePoints () const 00155 { 00156 return (bounds_of_octree_points_); 00157 } 00158 00159 inline const PointCloudIn& 00160 getPointsForRegistration () const 00161 { 00162 return (points_for_registration_); 00163 } 00164 00165 protected: 00166 const std::string obj_name_; 00167 ORROctree octree_; 00168 float octree_center_of_mass_[3]; 00169 float bounds_of_octree_points_[6]; 00170 PointCloudIn points_for_registration_; 00171 void* user_data_; 00172 }; 00173 00174 typedef std::list<std::pair<const ORROctree::Node::Data*, const ORROctree::Node::Data*> > node_data_pair_list; 00175 typedef std::map<const Model*, node_data_pair_list> HashTableCell; 00176 typedef VoxelStructure<HashTableCell, float> HashTable; 00177 00178 public: 00179 /** \brief This class is used by 'ObjRecRANSAC' to maintain the object models to be recognized. Normally, you do not need to use 00180 * this class directly. */ 00181 ModelLibrary (float pair_width, float voxel_size, float max_coplanarity_angle = 3.0f*AUX_DEG_TO_RADIANS/*3 degrees*/); 00182 virtual ~ModelLibrary () 00183 { 00184 this->clear(); 00185 } 00186 00187 /** \brief Removes all models from the library and clears the hash table. */ 00188 void 00189 removeAllModels (); 00190 00191 /** \brief This is a threshold. The larger the value the more point pairs will be considered as co-planar and will 00192 * be ignored in the off-line model pre-processing and in the online recognition phases. This makes sense only if 00193 * "ignore co-planar points" is on. Call this method before calling addModel. */ 00194 inline void 00195 setMaxCoplanarityAngleDegrees (float max_coplanarity_angle_degrees) 00196 { 00197 max_coplanarity_angle_ = max_coplanarity_angle_degrees*AUX_DEG_TO_RADIANS; 00198 } 00199 00200 /** \biref Call this method in order NOT to add co-planar point pairs to the hash table. The default behavior 00201 * is ignoring co-planar points on. */ 00202 inline void 00203 ignoreCoplanarPointPairsOn () 00204 { 00205 ignore_coplanar_opps_ = true; 00206 } 00207 00208 /** \biref Call this method in order to add all point pairs (co-planar as well) to the hash table. The default 00209 * behavior is ignoring co-planar points on. */ 00210 inline void 00211 ignoreCoplanarPointPairsOff () 00212 { 00213 ignore_coplanar_opps_ = false; 00214 } 00215 00216 /** \brief Adds a model to the hash table. 00217 * 00218 * \param[in] points represents the model to be added. 00219 * \param[in] normals are the normals at the model points. 00220 * \param[in] object_name is the unique name of the object to be added. 00221 * \param[in] num_points_for_registration is the number of points used for fast ICP registration prior to hypothesis testing 00222 * \param[in] user_data is a pointer to some data (can be NULL) 00223 * 00224 * Returns true if model successfully added and false otherwise (e.g., if object_name is not unique). */ 00225 bool 00226 addModel (const PointCloudIn& points, const PointCloudN& normals, const std::string& object_name, 00227 float frac_of_points_for_registration, void* user_data = NULL); 00228 00229 /** \brief Returns the hash table built by this instance. */ 00230 inline const HashTable& 00231 getHashTable () const 00232 { 00233 return (hash_table_); 00234 } 00235 00236 inline const Model* 00237 getModel (const std::string& name) const 00238 { 00239 std::map<std::string,Model*>::const_iterator it = models_.find (name); 00240 if ( it != models_.end () ) 00241 return (it->second); 00242 00243 return (NULL); 00244 } 00245 00246 inline const std::map<std::string,Model*>& 00247 getModels () const 00248 { 00249 return (models_); 00250 } 00251 00252 protected: 00253 /** \brief Removes all models from the library and destroys the hash table. This method should be called upon destroying this object. */ 00254 void 00255 clear (); 00256 00257 /** \brief Returns true if the oriented point pair was added to the hash table and false otherwise. */ 00258 bool 00259 addToHashTable (Model* model, const ORROctree::Node::Data* data1, const ORROctree::Node::Data* data2); 00260 00261 protected: 00262 float pair_width_; 00263 float voxel_size_; 00264 float max_coplanarity_angle_; 00265 bool ignore_coplanar_opps_; 00266 00267 std::map<std::string,Model*> models_; 00268 HashTable hash_table_; 00269 int num_of_cells_[3]; 00270 }; 00271 } // namespace recognition 00272 } // namespace pcl 00273 00274 #endif // PCL_RECOGNITION_MODEL_LIBRARY_H_