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 /* 00040 * orr_octree.h 00041 * 00042 * Created on: Oct 23, 2012 00043 * Author: papazov 00044 */ 00045 00046 #ifndef PCL_RECOGNITION_ORR_OCTREE_H_ 00047 #define PCL_RECOGNITION_ORR_OCTREE_H_ 00048 00049 #include "auxiliary.h" 00050 #include <pcl/point_types.h> 00051 #include <pcl/point_cloud.h> 00052 #include <pcl/pcl_exports.h> 00053 #include <cstdlib> 00054 #include <ctime> 00055 #include <vector> 00056 #include <list> 00057 #include <set> 00058 00059 //#define PCL_REC_ORR_OCTREE_VERBOSE 00060 00061 namespace pcl 00062 { 00063 namespace recognition 00064 { 00065 /** \brief That's a very specialized and simple octree class. That's the way it is intended to 00066 * be, that's why no templates and stuff like this. 00067 * 00068 * \author Chavdar Papazov 00069 * \ingroup recognition 00070 */ 00071 class PCL_EXPORTS ORROctree 00072 { 00073 public: 00074 typedef pcl::PointCloud<pcl::PointXYZ> PointCloudIn; 00075 typedef pcl::PointCloud<pcl::PointXYZ> PointCloudOut; 00076 typedef pcl::PointCloud<pcl::Normal> PointCloudN; 00077 00078 class Node 00079 { 00080 public: 00081 class Data 00082 { 00083 public: 00084 Data (int id_x, int id_y, int id_z, int lin_id, void* user_data = NULL) 00085 : id_x_ (id_x), 00086 id_y_ (id_y), 00087 id_z_ (id_z), 00088 lin_id_ (lin_id), 00089 num_points_ (0), 00090 user_data_ (user_data) 00091 { 00092 n_[0] = n_[1] = n_[2] = p_[0] = p_[1] = p_[2] = 0.0f; 00093 } 00094 00095 virtual~ Data (){} 00096 00097 inline void 00098 addToPoint (float x, float y, float z) 00099 { 00100 p_[0] += x; p_[1] += y; p_[2] += z; 00101 ++num_points_; 00102 } 00103 00104 inline void 00105 computeAveragePoint () 00106 { 00107 if ( num_points_ < 2 ) 00108 return; 00109 00110 aux::mult3 (p_, 1.0f/static_cast<float> (num_points_)); 00111 num_points_ = 1; 00112 } 00113 00114 inline void 00115 addToNormal (float x, float y, float z) { n_[0] += x; n_[1] += y; n_[2] += z;} 00116 00117 inline const float* 00118 getPoint () const { return p_;} 00119 00120 inline float* 00121 getPoint (){ return p_;} 00122 00123 inline const float* 00124 getNormal () const { return n_;} 00125 00126 inline float* 00127 getNormal (){ return n_;} 00128 00129 inline void 00130 get3dId (int id[3]) const 00131 { 00132 id[0] = id_x_; 00133 id[1] = id_y_; 00134 id[2] = id_z_; 00135 } 00136 00137 inline int 00138 get3dIdX () const {return id_x_;} 00139 00140 inline int 00141 get3dIdY () const {return id_y_;} 00142 00143 inline int 00144 get3dIdZ () const {return id_z_;} 00145 00146 inline int 00147 getLinearId () const { return lin_id_;} 00148 00149 inline void 00150 setUserData (void* user_data){ user_data_ = user_data;} 00151 00152 inline void* 00153 getUserData () const { return user_data_;} 00154 00155 inline void 00156 insertNeighbor (Node* node){ neighbors_.insert (node);} 00157 00158 inline const std::set<Node*>& 00159 getNeighbors () const { return (neighbors_);} 00160 00161 protected: 00162 float n_[3], p_[3]; 00163 int id_x_, id_y_, id_z_, lin_id_, num_points_; 00164 std::set<Node*> neighbors_; 00165 void *user_data_; 00166 }; 00167 00168 Node () 00169 : data_ (NULL), 00170 parent_ (NULL), 00171 children_(NULL) 00172 {} 00173 00174 virtual~ Node () 00175 { 00176 this->deleteChildren (); 00177 this->deleteData (); 00178 } 00179 00180 inline void 00181 setCenter(const float *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2];} 00182 00183 inline void 00184 setBounds(const float *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; bounds_[2] = b[2]; bounds_[3] = b[3]; bounds_[4] = b[4]; bounds_[5] = b[5];} 00185 00186 inline void 00187 setParent(Node* parent) { parent_ = parent;} 00188 00189 inline void 00190 setData(Node::Data* data) { data_ = data;} 00191 00192 /** \brief Computes the "radius" of the node which is half the diagonal length. */ 00193 inline void 00194 computeRadius() 00195 { 00196 float v[3] = {0.5f*(bounds_[1]-bounds_[0]), 0.5f*(bounds_[3]-bounds_[2]), 0.5f*(bounds_[5]-bounds_[4])}; 00197 radius_ = static_cast<float> (aux::length3 (v)); 00198 } 00199 00200 inline const float* 00201 getCenter() const { return center_;} 00202 00203 inline const float* 00204 getBounds() const { return bounds_;} 00205 00206 inline void 00207 getBounds(float b[6]) const 00208 { 00209 memcpy (b, bounds_, 6*sizeof (float)); 00210 } 00211 00212 inline Node* 00213 getChild (int id) { return &children_[id];} 00214 00215 inline Node* 00216 getChildren () { return children_;} 00217 00218 inline Node::Data* 00219 getData (){ return data_;} 00220 00221 inline const Node::Data* 00222 getData () const { return data_;} 00223 00224 inline void 00225 setUserData (void* user_data){ data_->setUserData (user_data);} 00226 00227 inline Node* 00228 getParent (){ return parent_;} 00229 00230 inline bool 00231 hasData (){ return static_cast<bool> (data_);} 00232 00233 inline bool 00234 hasChildren (){ return static_cast<bool> (children_);} 00235 00236 /** \brief Computes the "radius" of the node which is half the diagonal length. */ 00237 inline float 00238 getRadius (){ return radius_;} 00239 00240 bool 00241 createChildren (); 00242 00243 inline void 00244 deleteChildren () 00245 { 00246 if ( children_ ) 00247 { 00248 delete[] children_; 00249 children_ = NULL; 00250 } 00251 } 00252 00253 inline void 00254 deleteData () 00255 { 00256 if ( data_ ) 00257 { 00258 delete data_; 00259 data_ = NULL; 00260 } 00261 } 00262 00263 /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens 00264 * of either of the nodes has no data. */ 00265 inline void 00266 makeNeighbors (Node* node) 00267 { 00268 if ( !this->getData () || !node->getData () ) 00269 return; 00270 00271 this->getData ()->insertNeighbor (node); 00272 node->getData ()->insertNeighbor (this); 00273 } 00274 00275 protected: 00276 Node::Data *data_; 00277 float center_[3], bounds_[6], radius_; 00278 Node *parent_, *children_; 00279 }; 00280 00281 ORROctree (); 00282 virtual ~ORROctree (){ this->clear ();} 00283 00284 void 00285 clear (); 00286 00287 /** \brief Creates an octree which encloses 'points' and with leaf size equal to 'voxel_size'. 00288 * 'enlarge_bounds' makes sure that no points from the input will lie on the octree boundary 00289 * by enlarging the bounds by that factor. For example, enlarge_bounds = 1 means that the 00290 * bounds will be enlarged by 100%. The default value is fine. */ 00291 void 00292 build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = NULL, float enlarge_bounds = 0.00001f); 00293 00294 /** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf 00295 * size equal to 'voxel_size'. */ 00296 void 00297 build (const float* bounds, float voxel_size); 00298 00299 /** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within 00300 * the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method 00301 * returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and 00302 * method just returns a pointer to the leaf. */ 00303 inline ORROctree::Node* 00304 createLeaf (float x, float y, float z) 00305 { 00306 // Make sure that the input point is within the octree bounds 00307 if ( x < bounds_[0] || x > bounds_[1] || 00308 y < bounds_[2] || y > bounds_[3] || 00309 z < bounds_[4] || z > bounds_[5] ) 00310 { 00311 return (NULL); 00312 } 00313 00314 ORROctree::Node* node = root_; 00315 const float *c; 00316 int id; 00317 00318 // Go down to the right leaf 00319 for ( int l = 0 ; l < tree_levels_ ; ++l ) 00320 { 00321 node->createChildren (); 00322 c = node->getCenter (); 00323 id = 0; 00324 00325 if ( x >= c[0] ) id |= 4; 00326 if ( y >= c[1] ) id |= 2; 00327 if ( z >= c[2] ) id |= 1; 00328 00329 node = node->getChild (id); 00330 } 00331 00332 if ( !node->getData () ) 00333 { 00334 Node::Data* data = new Node::Data ( 00335 static_cast<int> ((node->getCenter ()[0] - bounds_[0])/voxel_size_), 00336 static_cast<int> ((node->getCenter ()[1] - bounds_[2])/voxel_size_), 00337 static_cast<int> ((node->getCenter ()[2] - bounds_[4])/voxel_size_), 00338 static_cast<int> (full_leaves_.size ())); 00339 00340 node->setData (data); 00341 this->insertNeighbors (node); 00342 full_leaves_.push_back (node); 00343 } 00344 00345 return (node); 00346 } 00347 00348 /** \brief This method returns a super set of the full leavess which are intersected by the sphere 00349 * with radius 'radius' and centered at 'p'. Pointers to the intersected full leaves are saved in 00350 * 'out'. The method computes a super set in the sense that in general not all leaves saved in 'out' 00351 * are really intersected by the sphere. The intersection test is based on the leaf radius (since 00352 * its faster than checking all leaf corners and sides), so we report more leaves than we should, 00353 * but still, this is a fair approximation. */ 00354 void 00355 getFullLeavesIntersectedBySphere (const float* p, float radius, std::list<ORROctree::Node*>& out) const; 00356 00357 /** \brief Randomly chooses and returns a full leaf that is intersected by the sphere with center 'p' 00358 * and 'radius'. Returns NULL if no leaf is intersected by that sphere. */ 00359 ORROctree::Node* 00360 getRandomFullLeafOnSphere (const float* p, float radius) const; 00361 00362 /** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the leaf 00363 * with id [i, j, k] or NULL is no such leaf exists. */ 00364 ORROctree::Node* 00365 getLeaf (int i, int j, int k) 00366 { 00367 float offset = 0.5f*voxel_size_; 00368 float p[3] = {bounds_[0] + offset + static_cast<float> (i)*voxel_size_, 00369 bounds_[2] + offset + static_cast<float> (j)*voxel_size_, 00370 bounds_[4] + offset + static_cast<float> (k)*voxel_size_}; 00371 00372 return (this->getLeaf (p[0], p[1], p[2])); 00373 } 00374 00375 /** \brief Returns a pointer to the leaf containing p = (x, y, z) or NULL if no such leaf exists. */ 00376 inline ORROctree::Node* 00377 getLeaf (float x, float y, float z) 00378 { 00379 // Make sure that the input point is within the octree bounds 00380 if ( x < bounds_[0] || x > bounds_[1] || 00381 y < bounds_[2] || y > bounds_[3] || 00382 z < bounds_[4] || z > bounds_[5] ) 00383 { 00384 return (NULL); 00385 } 00386 00387 ORROctree::Node* node = root_; 00388 const float *c; 00389 int id; 00390 00391 // Go down to the right leaf 00392 for ( int l = 0 ; l < tree_levels_ ; ++l ) 00393 { 00394 if ( !node->hasChildren () ) 00395 return (NULL); 00396 00397 c = node->getCenter (); 00398 id = 0; 00399 00400 if ( x >= c[0] ) id |= 4; 00401 if ( y >= c[1] ) id |= 2; 00402 if ( z >= c[2] ) id |= 1; 00403 00404 node = node->getChild (id); 00405 } 00406 00407 return (node); 00408 } 00409 00410 /** \brief Deletes the branch 'node' is part of. */ 00411 void 00412 deleteBranch (Node* node); 00413 00414 /** \brief Returns a vector with all octree leaves which contain at least one point. */ 00415 inline std::vector<ORROctree::Node*>& 00416 getFullLeaves () { return full_leaves_;} 00417 00418 inline const std::vector<ORROctree::Node*>& 00419 getFullLeaves () const { return full_leaves_;} 00420 00421 void 00422 getFullLeavesPoints (PointCloudOut& out) const; 00423 00424 void 00425 getNormalsOfFullLeaves (PointCloudN& out) const; 00426 00427 inline ORROctree::Node* 00428 getRoot (){ return root_;} 00429 00430 inline const float* 00431 getBounds () const 00432 { 00433 return (bounds_); 00434 } 00435 00436 inline void 00437 getBounds (float b[6]) const 00438 { 00439 memcpy (b, bounds_, 6*sizeof (float)); 00440 } 00441 00442 inline float 00443 getVoxelSize () const { return voxel_size_;} 00444 00445 inline void 00446 insertNeighbors (Node* node) 00447 { 00448 const float* c = node->getCenter (); 00449 float s = 0.5f*voxel_size_; 00450 Node *neigh; 00451 00452 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00453 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00454 neigh = this->getLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00455 neigh = this->getLeaf (c[0]+s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00456 neigh = this->getLeaf (c[0]+s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00457 neigh = this->getLeaf (c[0]+s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00458 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00459 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00460 neigh = this->getLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00461 00462 neigh = this->getLeaf (c[0] , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00463 neigh = this->getLeaf (c[0] , c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00464 neigh = this->getLeaf (c[0] , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00465 neigh = this->getLeaf (c[0] , c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00466 //neigh = this->getLeaf (c[0] , c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00467 neigh = this->getLeaf (c[0] , c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00468 neigh = this->getLeaf (c[0] , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00469 neigh = this->getLeaf (c[0] , c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00470 neigh = this->getLeaf (c[0] , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00471 00472 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00473 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00474 neigh = this->getLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00475 neigh = this->getLeaf (c[0]-s, c[1] , c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00476 neigh = this->getLeaf (c[0]-s, c[1] , c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00477 neigh = this->getLeaf (c[0]-s, c[1] , c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00478 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh); 00479 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2] ); if ( neigh ) node->makeNeighbors (neigh); 00480 neigh = this->getLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); 00481 } 00482 00483 protected: 00484 float voxel_size_, bounds_[6]; 00485 int tree_levels_; 00486 Node* root_; 00487 std::vector<Node*> full_leaves_; 00488 }; 00489 } // namespace recognition 00490 } // namespace pcl 00491 00492 #endif /* PCL_RECOGNITION_ORR_OCTREE_H_ */