Point Cloud Library (PCL)
1.7.0
|
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_ */