orr_octree.h
Go to the documentation of this file.
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   {
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 
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 
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 
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 
00291         void
00292         build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals = NULL, float enlarge_bounds = 0.00001f);
00293 
00296         void
00297         build (const float* bounds, float voxel_size);
00298 
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 
00354         void
00355         getFullLeavesIntersectedBySphere (const float* p, float radius, std::list<ORROctree::Node*>& out) const;
00356 
00359         ORROctree::Node*
00360         getRandomFullLeafOnSphere (const float* p, float radius) const;
00361 
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 
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 
00411         void
00412         deleteBranch (Node* node);
00413 
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_ */

pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:30