pcl: simple_octree.hpp Source File

Go to the documentation of this file.
00001 /*
00002  * simple_octree.hpp
00003  *
00004  *  Created on: Mar 12, 2013
00005  *      Author: papazov
00006  */
00007 
00008 #ifndef SIMPLE_OCTREE_HPP_
00009 #define SIMPLE_OCTREE_HPP_
00010 
00011 //===============================================================================================================================
00012 
00013 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00014 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::Node ()
00015 : data_ (0),
00016   parent_ (0),
00017   children_(0)
00018 {}
00019 
00020 //===============================================================================================================================
00021 
00022 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00023 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::~Node ()
00024 {
00025   this->deleteChildren ();
00026   this->deleteData ();
00027 }
00028 
00029 //===============================================================================================================================
00030 
00031 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00032 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setCenter (const Scalar *c)
00033 {
00034   center_[0] = c[0];
00035   center_[1] = c[1];
00036   center_[2] = c[2];
00037 }
00038 
00039 //===============================================================================================================================
00040 
00041 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00042 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::setBounds (const Scalar *b)
00043 {
00044   bounds_[0] = b[0];
00045   bounds_[1] = b[1];
00046   bounds_[2] = b[2];
00047   bounds_[3] = b[3];
00048   bounds_[4] = b[4];
00049   bounds_[5] = b[5];
00050 }
00051 
00052 //===============================================================================================================================
00053 
00054 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00055 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::computeRadius ()
00056 {
00057   Scalar v[3] = {static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]),
00058                  static_cast<Scalar> (0.5)*(bounds_[3]-bounds_[2]),
00059                  static_cast<Scalar> (0.5)*(bounds_[5]-bounds_[4])};
00060 
00061   radius_ = static_cast<Scalar> (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
00062 }
00063 
00064 //===============================================================================================================================
00065 
00066 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline bool
00067 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::createChildren ()
00068 {
00069   if ( children_ )
00070     return (false);
00071 
00072   Scalar bounds[6], center[3], childside = static_cast<Scalar> (0.5)*(bounds_[1]-bounds_[0]);
00073   children_ = new Node[8];
00074 
00075   // Compute bounds and center for child 0, i.e., for (0,0,0)
00076   bounds[0] = bounds_[0]; bounds[1] = center_[0];
00077   bounds[2] = bounds_[2]; bounds[3] = center_[1];
00078   bounds[4] = bounds_[4]; bounds[5] = center_[2];
00079   // Compute the center of the new child
00080   center[0] = 0.5f*(bounds[0] + bounds[1]);
00081   center[1] = 0.5f*(bounds[2] + bounds[3]);
00082   center[2] = 0.5f*(bounds[4] + bounds[5]);
00083   // Save the results
00084   children_[0].setBounds(bounds);
00085   children_[0].setCenter(center);
00086 
00087   // Compute bounds and center for child 1, i.e., for (0,0,1)
00088   bounds[4] = center_[2]; bounds[5] = bounds_[5];
00089   // Update the center
00090   center[2] += childside;
00091   // Save the results
00092   children_[1].setBounds(bounds);
00093   children_[1].setCenter(center);
00094 
00095   // Compute bounds and center for child 3, i.e., for (0,1,1)
00096   bounds[2] = center_[1]; bounds[3] = bounds_[3];
00097   // Update the center
00098   center[1] += childside;
00099   // Save the results
00100   children_[3].setBounds(bounds);
00101   children_[3].setCenter(center);
00102 
00103   // Compute bounds and center for child 2, i.e., for (0,1,0)
00104   bounds[4] = bounds_[4]; bounds[5] = center_[2];
00105   // Update the center
00106   center[2] -= childside;
00107   // Save the results
00108   children_[2].setBounds(bounds);
00109   children_[2].setCenter(center);
00110 
00111   // Compute bounds and center for child 6, i.e., for (1,1,0)
00112   bounds[0] = center_[0]; bounds[1] = bounds_[1];
00113   // Update the center
00114   center[0] += childside;
00115   // Save the results
00116   children_[6].setBounds(bounds);
00117   children_[6].setCenter(center);
00118 
00119   // Compute bounds and center for child 7, i.e., for (1,1,1)
00120   bounds[4] = center_[2]; bounds[5] = bounds_[5];
00121   // Update the center
00122   center[2] += childside;
00123   // Save the results
00124   children_[7].setBounds(bounds);
00125   children_[7].setCenter(center);
00126 
00127   // Compute bounds and center for child 5, i.e., for (1,0,1)
00128   bounds[2] = bounds_[2]; bounds[3] = center_[1];
00129   // Update the center
00130   center[1] -= childside;
00131   // Save the results
00132   children_[5].setBounds(bounds);
00133   children_[5].setCenter(center);
00134 
00135   // Compute bounds and center for child 4, i.e., for (1,0,0)
00136   bounds[4] = bounds_[4]; bounds[5] = center_[2];
00137   // Update the center
00138   center[2] -= childside;
00139   // Save the results
00140   children_[4].setBounds(bounds);
00141   children_[4].setCenter(center);
00142 
00143   for ( int i = 0 ; i < 8 ; ++i )
00144   {
00145     children_[i].computeRadius();
00146     children_[i].setParent(this);
00147   }
00148 
00149   return (true);
00150 }
00151 
00152 //===============================================================================================================================
00153 
00154 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00155 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteChildren ()
00156 {
00157   if ( children_ )
00158   {
00159     delete[] children_;
00160     children_ = 0;
00161   }
00162 }
00163 
00164 //===============================================================================================================================
00165 
00166 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00167 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::deleteData ()
00168 {
00169   if ( data_ )
00170   {
00171     delete data_;
00172     data_ = 0;
00173   }
00174 }
00175 
00176 //===============================================================================================================================
00177 
00178 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00179 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node::makeNeighbors (Node* node)
00180 {
00181   if ( !this->hasData () || !node->hasData () )
00182     return;
00183 
00184   this->full_leaf_neighbors_.insert (node);
00185   node->full_leaf_neighbors_.insert (this);
00186 }
00187 
00188 //===============================================================================================================================
00189 
00190 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00191 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::SimpleOctree ()
00192 : tree_levels_ (0),
00193   root_ (0)
00194 {
00195 }
00196 
00197 //===============================================================================================================================
00198 
00199 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00200 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::~SimpleOctree ()
00201 {
00202   this->clear ();
00203 }
00204 
00205 //===============================================================================================================================
00206 
00207 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00208 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::clear ()
00209 {
00210   if ( root_ )
00211   {
00212     delete root_;
00213     root_ = 0;
00214   }
00215 
00216   full_leaves_.clear();
00217 }
00218 
00219 //===============================================================================================================================
00220 
00221 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00222 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::build (const Scalar* bounds, Scalar voxel_size,
00223     NodeDataCreator* node_data_creator)
00224 {
00225   if ( voxel_size <= 0 )
00226     return;
00227 
00228   this->clear();
00229 
00230   voxel_size_ = voxel_size;
00231   node_data_creator_ = node_data_creator;
00232 
00233   Scalar extent = std::max (std::max (bounds[1]-bounds[0], bounds[3]-bounds[2]), bounds[5]-bounds[4]);
00234   Scalar center[3] = {static_cast<Scalar> (0.5)*(bounds[0]+bounds[1]),
00235                       static_cast<Scalar> (0.5)*(bounds[2]+bounds[3]),
00236                       static_cast<Scalar> (0.5)*(bounds[4]+bounds[5])};
00237 
00238   Scalar arg = extent/voxel_size;
00239 
00240   // Compute the number of tree levels
00241   if ( arg > 1 )
00242     tree_levels_ = static_cast<int> (ceil (log (arg)/log (2.0)) + 0.5);
00243   else
00244     tree_levels_ = 0;
00245 
00246   // Compute the number of octree levels and the bounds of the root
00247   Scalar half_root_side = static_cast<Scalar> (0.5f*pow (2.0, tree_levels_)*voxel_size);
00248 
00249   // Determine the bounding box of the octree
00250   bounds_[0] = center[0] - half_root_side;
00251   bounds_[1] = center[0] + half_root_side;
00252   bounds_[2] = center[1] - half_root_side;
00253   bounds_[3] = center[1] + half_root_side;
00254   bounds_[4] = center[2] - half_root_side;
00255   bounds_[5] = center[2] + half_root_side;
00256 
00257   // Create and initialize the root
00258   root_ = new Node ();
00259   root_->setCenter (center);
00260   root_->setBounds (bounds_);
00261   root_->setParent (NULL);
00262   root_->computeRadius ();
00263 }
00264 
00265 //===============================================================================================================================
00266 
00267 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00268 typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
00269 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::createLeaf (Scalar x, Scalar y, Scalar z)
00270 {
00271   // Make sure that the input point is within the octree bounds
00272   if ( x < bounds_[0] || x > bounds_[1] ||
00273        y < bounds_[2] || y > bounds_[3] ||
00274        z < bounds_[4] || z > bounds_[5] )
00275   {
00276     return (NULL);
00277   }
00278 
00279   Node* node = root_;
00280   const Scalar *c;
00281   int id;
00282 
00283   // Go down to the right leaf
00284   for ( int l = 0 ; l < tree_levels_ ; ++l )
00285   {
00286     node->createChildren ();
00287     c = node->getCenter ();
00288     id = 0;
00289 
00290     if ( x >= c[0] ) id |= 4;
00291     if ( y >= c[1] ) id |= 2;
00292     if ( z >= c[2] ) id |= 1;
00293 
00294     node = node->getChild (id);
00295   }
00296 
00297   if ( !node->hasData () )
00298   {
00299     node->setData (node_data_creator_->create (node));
00300     this->insertNeighbors (node);
00301     full_leaves_.push_back (node);
00302   }
00303 
00304   return (node);
00305 }
00306 
00307 //===============================================================================================================================
00308 
00309 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00310 typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
00311 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (int i, int j, int k)
00312 {
00313   Scalar offset = 0.5f*voxel_size_;
00314   Scalar p[3] = {bounds_[0] + offset + static_cast<Scalar> (i)*voxel_size_,
00315                  bounds_[2] + offset + static_cast<Scalar> (j)*voxel_size_,
00316                  bounds_[4] + offset + static_cast<Scalar> (k)*voxel_size_};
00317 
00318   return (this->getFullLeaf (p[0], p[1], p[2]));
00319 }
00320 
00321 //===============================================================================================================================
00322 
00323 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline
00324 typename pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::Node*
00325 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::getFullLeaf (Scalar x, Scalar y, Scalar z)
00326 {
00327   // Make sure that the input point is within the octree bounds
00328   if ( x < bounds_[0] || x > bounds_[1] ||
00329        y < bounds_[2] || y > bounds_[3] ||
00330        z < bounds_[4] || z > bounds_[5] )
00331   {
00332     return (NULL);
00333   }
00334 
00335   Node* node = root_;
00336   const Scalar *c;
00337   int id;
00338 
00339   // Go down to the right leaf
00340   for ( int l = 0 ; l < tree_levels_ ; ++l )
00341   {
00342     if ( !node->hasChildren () )
00343       return (NULL);
00344 
00345     c = node->getCenter ();
00346     id = 0;
00347 
00348     if ( x >= c[0] ) id |= 4;
00349     if ( y >= c[1] ) id |= 2;
00350     if ( z >= c[2] ) id |= 1;
00351 
00352     node = node->getChild (id);
00353   }
00354 
00355   if ( !node->hasData () )
00356     return (NULL);
00357 
00358   return (node);
00359 }
00360 
00361 //===============================================================================================================================
00362 
00363 template<typename NodeData, typename NodeDataCreator, typename Scalar> inline void
00364 pcl::recognition::SimpleOctree<NodeData, NodeDataCreator, Scalar>::insertNeighbors (Node* node)
00365 {
00366   const Scalar* c = node->getCenter ();
00367   Scalar s = static_cast<Scalar> (0.5)*voxel_size_;
00368   Node *neigh;
00369 
00370   neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00371   neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00372   neigh = this->getFullLeaf (c[0]+s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00373   neigh = this->getFullLeaf (c[0]+s, c[1]  , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00374   neigh = this->getFullLeaf (c[0]+s, c[1]  , c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00375   neigh = this->getFullLeaf (c[0]+s, c[1]  , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00376   neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00377   neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00378   neigh = this->getFullLeaf (c[0]+s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00379 
00380   neigh = this->getFullLeaf (c[0]  , c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00381   neigh = this->getFullLeaf (c[0]  , c[1]+s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00382   neigh = this->getFullLeaf (c[0]  , c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00383   neigh = this->getFullLeaf (c[0]  , c[1]  , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00384 //neigh = this->getFullLeaf (c[0]  , c[1]  , c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00385   neigh = this->getFullLeaf (c[0]  , c[1]  , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00386   neigh = this->getFullLeaf (c[0]  , c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00387   neigh = this->getFullLeaf (c[0]  , c[1]-s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00388   neigh = this->getFullLeaf (c[0]  , c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00389 
00390   neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00391   neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00392   neigh = this->getFullLeaf (c[0]-s, c[1]+s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00393   neigh = this->getFullLeaf (c[0]-s, c[1]  , c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00394   neigh = this->getFullLeaf (c[0]-s, c[1]  , c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00395   neigh = this->getFullLeaf (c[0]-s, c[1]  , c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00396   neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]+s); if ( neigh ) node->makeNeighbors (neigh);
00397   neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]  ); if ( neigh ) node->makeNeighbors (neigh);
00398   neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh);
00399 }
00400 
00401 //===============================================================================================================================
00402 
00403 #endif /* SIMPLE_OCTREE_HPP_ */