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 * $Id$ 00037 */ 00038 00039 #ifndef PCL_OCTREE_POINTCLOUD_HPP_ 00040 #define PCL_OCTREE_POINTCLOUD_HPP_ 00041 00042 #include <vector> 00043 #include <assert.h> 00044 00045 #include <pcl/common/common.h> 00046 00047 using namespace std; 00048 00049 ////////////////////////////////////////////////////////////////////////////////////////////// 00050 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> 00051 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::OctreePointCloud (const double resolution) : 00052 OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()), 00053 epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f), 00054 max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0) 00055 { 00056 assert (resolution > 0.0f); 00057 } 00058 00059 ////////////////////////////////////////////////////////////////////////////////////////////// 00060 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> 00061 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::~OctreePointCloud () 00062 { 00063 } 00064 00065 ////////////////////////////////////////////////////////////////////////////////////////////// 00066 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00067 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointsFromInputCloud () 00068 { 00069 size_t i; 00070 00071 if (indices_) 00072 { 00073 for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current) 00074 { 00075 assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ()))); 00076 00077 if (isFinite (input_->points[*current])) 00078 { 00079 // add points to octree 00080 this->addPointIdx (*current); 00081 } 00082 } 00083 } 00084 else 00085 { 00086 for (i = 0; i < input_->points.size (); i++) 00087 { 00088 if (isFinite (input_->points[i])) 00089 { 00090 // add points to octree 00091 this->addPointIdx (static_cast<unsigned int> (i)); 00092 } 00093 } 00094 } 00095 } 00096 00097 ////////////////////////////////////////////////////////////////////////////////////////////// 00098 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00099 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg) 00100 { 00101 this->addPointIdx (point_idx_arg); 00102 if (indices_arg) 00103 indices_arg->push_back (point_idx_arg); 00104 } 00105 00106 ////////////////////////////////////////////////////////////////////////////////////////////// 00107 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00108 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg) 00109 { 00110 assert (cloud_arg==input_); 00111 00112 cloud_arg->push_back (point_arg); 00113 00114 this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1); 00115 } 00116 00117 ////////////////////////////////////////////////////////////////////////////////////////////// 00118 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00119 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, 00120 IndicesPtr indices_arg) 00121 { 00122 assert (cloud_arg==input_); 00123 assert (indices_arg==indices_); 00124 00125 cloud_arg->push_back (point_arg); 00126 00127 this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg); 00128 } 00129 00130 ////////////////////////////////////////////////////////////////////////////////////////////// 00131 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool 00132 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint (const PointT& point_arg) const 00133 { 00134 OctreeKey key; 00135 00136 // generate key for point 00137 this->genOctreeKeyforPoint (point_arg, key); 00138 00139 // search for key in octree 00140 return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key)); 00141 } 00142 00143 ////////////////////////////////////////////////////////////////////////////////////////////// 00144 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool 00145 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint (const int& point_idx_arg) const 00146 { 00147 // retrieve point from input cloud 00148 const PointT& point = this->input_->points[point_idx_arg]; 00149 00150 // search for voxel at point in octree 00151 return (this->isVoxelOccupiedAtPoint (point)); 00152 } 00153 00154 ////////////////////////////////////////////////////////////////////////////////////////////// 00155 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool 00156 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::isVoxelOccupiedAtPoint ( 00157 const double point_x_arg, const double point_y_arg, const double point_z_arg) const 00158 { 00159 OctreeKey key; 00160 00161 // generate key for point 00162 this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key); 00163 00164 return (this->existLeaf (key)); 00165 } 00166 00167 ////////////////////////////////////////////////////////////////////////////////////////////// 00168 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00169 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::deleteVoxelAtPoint (const PointT& point_arg) 00170 { 00171 OctreeKey key; 00172 00173 // generate key for point 00174 this->genOctreeKeyforPoint (point_arg, key); 00175 00176 this->removeLeaf (key); 00177 } 00178 00179 ////////////////////////////////////////////////////////////////////////////////////////////// 00180 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00181 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::deleteVoxelAtPoint (const int& point_idx_arg) 00182 { 00183 // retrieve point from input cloud 00184 const PointT& point = this->input_->points[point_idx_arg]; 00185 00186 // delete leaf at point 00187 this->deleteVoxelAtPoint (point); 00188 } 00189 00190 ////////////////////////////////////////////////////////////////////////////////////////////// 00191 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int 00192 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getOccupiedVoxelCenters ( 00193 AlignedPointTVector &voxel_center_list_arg) const 00194 { 00195 OctreeKey key; 00196 key.x = key.y = key.z = 0; 00197 00198 voxel_center_list_arg.clear (); 00199 00200 return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg); 00201 00202 } 00203 00204 ////////////////////////////////////////////////////////////////////////////////////////////// 00205 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int 00206 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getApproxIntersectedVoxelCentersBySegment ( 00207 const Eigen::Vector3f& origin, 00208 const Eigen::Vector3f& end, 00209 AlignedPointTVector &voxel_center_list, 00210 float precision) 00211 { 00212 Eigen::Vector3f direction = end - origin; 00213 float norm = direction.norm (); 00214 direction.normalize (); 00215 00216 const float step_size = static_cast<const float> (resolution_) * precision; 00217 // Ensure we get at least one step for the first voxel. 00218 const int nsteps = std::max (1, static_cast<int> (norm / step_size)); 00219 00220 OctreeKey prev_key; 00221 00222 bool bkeyDefined = false; 00223 00224 // Walk along the line segment with small steps. 00225 for (int i = 0; i < nsteps; ++i) 00226 { 00227 Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i)); 00228 00229 PointT octree_p; 00230 octree_p.x = p.x (); 00231 octree_p.y = p.y (); 00232 octree_p.z = p.z (); 00233 00234 OctreeKey key; 00235 this->genOctreeKeyforPoint (octree_p, key); 00236 00237 // Not a new key, still the same voxel. 00238 if ((key == prev_key) && (bkeyDefined) ) 00239 continue; 00240 00241 prev_key = key; 00242 bkeyDefined = true; 00243 00244 PointT center; 00245 genLeafNodeCenterFromOctreeKey (key, center); 00246 voxel_center_list.push_back (center); 00247 } 00248 00249 OctreeKey end_key; 00250 PointT end_p; 00251 end_p.x = end.x (); 00252 end_p.y = end.y (); 00253 end_p.z = end.z (); 00254 this->genOctreeKeyforPoint (end_p, end_key); 00255 if (!(end_key == prev_key)) 00256 { 00257 PointT center; 00258 genLeafNodeCenterFromOctreeKey (end_key, center); 00259 voxel_center_list.push_back (center); 00260 } 00261 00262 return (static_cast<int> (voxel_center_list.size ())); 00263 } 00264 00265 ////////////////////////////////////////////////////////////////////////////////////////////// 00266 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00267 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox () 00268 { 00269 00270 double minX, minY, minZ, maxX, maxY, maxZ; 00271 00272 PointT min_pt; 00273 PointT max_pt; 00274 00275 // bounding box cannot be changed once the octree contains elements 00276 assert (this->leaf_count_ == 0); 00277 00278 pcl::getMinMax3D (*input_, min_pt, max_pt); 00279 00280 float minValue = std::numeric_limits<float>::epsilon () * 512.0f; 00281 00282 minX = min_pt.x; 00283 minY = min_pt.y; 00284 minZ = min_pt.z; 00285 00286 maxX = max_pt.x + minValue; 00287 maxY = max_pt.y + minValue; 00288 maxZ = max_pt.z + minValue; 00289 00290 // generate bit masks for octree 00291 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); 00292 } 00293 00294 ////////////////////////////////////////////////////////////////////////////////////////////// 00295 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00296 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox (const double min_x_arg, 00297 const double min_y_arg, 00298 const double min_z_arg, 00299 const double max_x_arg, 00300 const double max_y_arg, 00301 const double max_z_arg) 00302 { 00303 // bounding box cannot be changed once the octree contains elements 00304 assert (this->leaf_count_ == 0); 00305 00306 assert (max_x_arg >= min_x_arg); 00307 assert (max_y_arg >= min_y_arg); 00308 assert (max_z_arg >= min_z_arg); 00309 00310 min_x_ = min_x_arg; 00311 max_x_ = max_x_arg; 00312 00313 min_y_ = min_y_arg; 00314 max_y_ = max_y_arg; 00315 00316 min_z_ = min_z_arg; 00317 max_z_ = max_z_arg; 00318 00319 min_x_ = min (min_x_, max_x_); 00320 min_y_ = min (min_y_, max_y_); 00321 min_z_ = min (min_z_, max_z_); 00322 00323 max_x_ = max (min_x_, max_x_); 00324 max_y_ = max (min_y_, max_y_); 00325 max_z_ = max (min_z_, max_z_); 00326 00327 // generate bit masks for octree 00328 getKeyBitSize (); 00329 00330 bounding_box_defined_ = true; 00331 } 00332 00333 ////////////////////////////////////////////////////////////////////////////////////////////// 00334 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00335 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox ( 00336 const double max_x_arg, const double max_y_arg, const double max_z_arg) 00337 { 00338 // bounding box cannot be changed once the octree contains elements 00339 assert (this->leaf_count_ == 0); 00340 00341 assert (max_x_arg >= 0.0f); 00342 assert (max_y_arg >= 0.0f); 00343 assert (max_z_arg >= 0.0f); 00344 00345 min_x_ = 0.0f; 00346 max_x_ = max_x_arg; 00347 00348 min_y_ = 0.0f; 00349 max_y_ = max_y_arg; 00350 00351 min_z_ = 0.0f; 00352 max_z_ = max_z_arg; 00353 00354 min_x_ = min (min_x_, max_x_); 00355 min_y_ = min (min_y_, max_y_); 00356 min_z_ = min (min_z_, max_z_); 00357 00358 max_x_ = max (min_x_, max_x_); 00359 max_y_ = max (min_y_, max_y_); 00360 max_z_ = max (min_z_, max_z_); 00361 00362 // generate bit masks for octree 00363 getKeyBitSize (); 00364 00365 bounding_box_defined_ = true; 00366 } 00367 00368 ////////////////////////////////////////////////////////////////////////////////////////////// 00369 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00370 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::defineBoundingBox (const double cubeLen_arg) 00371 { 00372 // bounding box cannot be changed once the octree contains elements 00373 assert (this->leaf_count_ == 0); 00374 00375 assert (cubeLen_arg >= 0.0f); 00376 00377 min_x_ = 0.0f; 00378 max_x_ = cubeLen_arg; 00379 00380 min_y_ = 0.0f; 00381 max_y_ = cubeLen_arg; 00382 00383 min_z_ = 0.0f; 00384 max_z_ = cubeLen_arg; 00385 00386 min_x_ = min (min_x_, max_x_); 00387 min_y_ = min (min_y_, max_y_); 00388 min_z_ = min (min_z_, max_z_); 00389 00390 max_x_ = max (min_x_, max_x_); 00391 max_y_ = max (min_y_, max_y_); 00392 max_z_ = max (min_z_, max_z_); 00393 00394 // generate bit masks for octree 00395 getKeyBitSize (); 00396 00397 bounding_box_defined_ = true; 00398 } 00399 00400 ////////////////////////////////////////////////////////////////////////////////////////////// 00401 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00402 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getBoundingBox ( 00403 double& min_x_arg, double& min_y_arg, double& min_z_arg, 00404 double& max_x_arg, double& max_y_arg, double& max_z_arg) const 00405 { 00406 min_x_arg = min_x_; 00407 min_y_arg = min_y_; 00408 min_z_arg = min_z_; 00409 00410 max_x_arg = max_x_; 00411 max_y_arg = max_y_; 00412 max_z_arg = max_z_; 00413 } 00414 00415 00416 ////////////////////////////////////////////////////////////////////////////////////////////// 00417 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> 00418 void 00419 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::adoptBoundingBoxToPoint (const PointT& point_idx_arg) 00420 { 00421 00422 const float minValue = std::numeric_limits<float>::epsilon (); 00423 00424 // increase octree size until point fits into bounding box 00425 while (true) 00426 { 00427 bool bLowerBoundViolationX = (point_idx_arg.x < min_x_); 00428 bool bLowerBoundViolationY = (point_idx_arg.y < min_y_); 00429 bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_); 00430 00431 bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_); 00432 bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_); 00433 bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_); 00434 00435 // do we violate any bounds? 00436 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX 00437 || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) ) 00438 { 00439 00440 if (bounding_box_defined_) 00441 { 00442 00443 double octreeSideLen; 00444 unsigned char child_idx; 00445 00446 // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2 00447 child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) 00448 | ((!bUpperBoundViolationZ))); 00449 00450 BranchNode* newRootBranch; 00451 00452 newRootBranch = new BranchNode(); 00453 this->branch_count_++; 00454 00455 this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_); 00456 00457 this->root_node_ = newRootBranch; 00458 00459 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_; 00460 00461 if (!bUpperBoundViolationX) 00462 min_x_ -= octreeSideLen; 00463 00464 if (!bUpperBoundViolationY) 00465 min_y_ -= octreeSideLen; 00466 00467 if (!bUpperBoundViolationZ) 00468 min_z_ -= octreeSideLen; 00469 00470 // configure tree depth of octree 00471 this->octree_depth_++; 00472 this->setTreeDepth (this->octree_depth_); 00473 00474 // recalculate bounding box width 00475 octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue; 00476 00477 // increase octree bounding box 00478 max_x_ = min_x_ + octreeSideLen; 00479 max_y_ = min_y_ + octreeSideLen; 00480 max_z_ = min_z_ + octreeSideLen; 00481 00482 } 00483 // bounding box is not defined - set it to point position 00484 else 00485 { 00486 // octree is empty - we set the center of the bounding box to our first pixel 00487 this->min_x_ = point_idx_arg.x - this->resolution_ / 2; 00488 this->min_y_ = point_idx_arg.y - this->resolution_ / 2; 00489 this->min_z_ = point_idx_arg.z - this->resolution_ / 2; 00490 00491 this->max_x_ = point_idx_arg.x + this->resolution_ / 2; 00492 this->max_y_ = point_idx_arg.y + this->resolution_ / 2; 00493 this->max_z_ = point_idx_arg.z + this->resolution_ / 2; 00494 00495 getKeyBitSize (); 00496 00497 bounding_box_defined_ = true; 00498 } 00499 00500 } 00501 else 00502 // no bound violations anymore - leave while loop 00503 break; 00504 } 00505 } 00506 00507 ////////////////////////////////////////////////////////////////////////////////////////////// 00508 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00509 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask) 00510 { 00511 00512 if (depth_mask) 00513 { 00514 // get amount of objects in leaf container 00515 size_t leaf_obj_count = (*leaf_node)->getSize (); 00516 00517 // copy leaf data 00518 std::vector<int> leafIndices; 00519 leafIndices.reserve(leaf_obj_count); 00520 00521 (*leaf_node)->getPointIndices(leafIndices); 00522 00523 // delete current leaf node 00524 this->deleteBranchChild(*parent_branch, child_idx); 00525 this->leaf_count_ --; 00526 00527 // create new branch node 00528 BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx); 00529 this->branch_count_ ++; 00530 00531 typename std::vector<int>::iterator it = leafIndices.begin(); 00532 typename std::vector<int>::const_iterator it_end = leafIndices.end(); 00533 00534 // add data to new branch 00535 OctreeKey new_index_key; 00536 00537 for (it = leafIndices.begin(); it!=it_end; ++it) 00538 { 00539 00540 const PointT& point_from_index = input_->points[*it]; 00541 // generate key 00542 genOctreeKeyforPoint (point_from_index, new_index_key); 00543 00544 LeafNode* newLeaf; 00545 BranchNode* newBranchParent; 00546 this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent); 00547 00548 (*newLeaf)->addPointIndex(*it); 00549 } 00550 } 00551 00552 00553 } 00554 00555 00556 ////////////////////////////////////////////////////////////////////////////////////////////// 00557 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00558 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointIdx (const int point_idx_arg) 00559 { 00560 OctreeKey key; 00561 00562 assert (point_idx_arg < static_cast<int> (input_->points.size ())); 00563 00564 const PointT& point = input_->points[point_idx_arg]; 00565 00566 // make sure bounding box is big enough 00567 adoptBoundingBoxToPoint (point); 00568 00569 // generate key 00570 genOctreeKeyforPoint (point, key); 00571 00572 LeafNode* leaf_node; 00573 BranchNode* parent_branch_of_leaf_node; 00574 unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); 00575 00576 if (this->dynamic_depth_enabled_ && depth_mask) 00577 { 00578 // get amount of objects in leaf container 00579 size_t leaf_obj_count = (*leaf_node)->getSize (); 00580 00581 while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask) 00582 { 00583 // index to branch child 00584 unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2); 00585 00586 expandLeafNode (leaf_node, 00587 parent_branch_of_leaf_node, 00588 child_idx, 00589 depth_mask); 00590 00591 depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); 00592 leaf_obj_count = (*leaf_node)->getSize (); 00593 } 00594 00595 } 00596 00597 (*leaf_node)->addPointIndex (point_idx_arg); 00598 } 00599 00600 ////////////////////////////////////////////////////////////////////////////////////////////// 00601 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> const PointT& 00602 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getPointByIndex (const unsigned int index_arg) const 00603 { 00604 // retrieve point from input cloud 00605 assert (index_arg < static_cast<unsigned int> (input_->points.size ())); 00606 return (this->input_->points[index_arg]); 00607 } 00608 00609 ////////////////////////////////////////////////////////////////////////////////////////////// 00610 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00611 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getKeyBitSize () 00612 { 00613 unsigned int max_voxels; 00614 00615 unsigned int max_key_x; 00616 unsigned int max_key_y; 00617 unsigned int max_key_z; 00618 00619 double octree_side_len; 00620 00621 const float minValue = std::numeric_limits<float>::epsilon(); 00622 00623 // find maximum key values for x, y, z 00624 max_key_x = static_cast<unsigned int> ((max_x_ - min_x_) / resolution_); 00625 max_key_y = static_cast<unsigned int> ((max_y_ - min_y_) / resolution_); 00626 max_key_z = static_cast<unsigned int> ((max_z_ - min_z_) / resolution_); 00627 00628 // find maximum amount of keys 00629 max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2)); 00630 00631 00632 // tree depth == amount of bits of max_voxels 00633 this->octree_depth_ = max ((min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))), 00634 static_cast<unsigned int> (0)); 00635 00636 octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_-minValue; 00637 00638 if (this->leaf_count_ == 0) 00639 { 00640 double octree_oversize_x; 00641 double octree_oversize_y; 00642 double octree_oversize_z; 00643 00644 octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0; 00645 octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0; 00646 octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0; 00647 00648 min_x_ -= octree_oversize_x; 00649 min_y_ -= octree_oversize_y; 00650 min_z_ -= octree_oversize_z; 00651 00652 max_x_ += octree_oversize_x; 00653 max_y_ += octree_oversize_y; 00654 max_z_ += octree_oversize_z; 00655 } 00656 else 00657 { 00658 max_x_ = min_x_ + octree_side_len; 00659 max_y_ = min_y_ + octree_side_len; 00660 max_z_ = min_z_ + octree_side_len; 00661 } 00662 00663 // configure tree depth of octree 00664 this->setTreeDepth (this->octree_depth_); 00665 00666 } 00667 00668 ////////////////////////////////////////////////////////////////////////////////////////////// 00669 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00670 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyforPoint (const PointT& point_arg, 00671 OctreeKey & key_arg) const 00672 { 00673 // calculate integer key for point coordinates 00674 key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_); 00675 key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_); 00676 key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_); 00677 } 00678 00679 ////////////////////////////////////////////////////////////////////////////////////////////// 00680 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00681 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyforPoint ( 00682 const double point_x_arg, const double point_y_arg, 00683 const double point_z_arg, OctreeKey & key_arg) const 00684 { 00685 PointT temp_point; 00686 00687 temp_point.x = static_cast<float> (point_x_arg); 00688 temp_point.y = static_cast<float> (point_y_arg); 00689 temp_point.z = static_cast<float> (point_z_arg); 00690 00691 // generate key for point 00692 genOctreeKeyforPoint (temp_point, key_arg); 00693 } 00694 00695 ////////////////////////////////////////////////////////////////////////////////////////////// 00696 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool 00697 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const 00698 { 00699 const PointT temp_point = getPointByIndex (data_arg); 00700 00701 // generate key for point 00702 genOctreeKeyforPoint (temp_point, key_arg); 00703 00704 return (true); 00705 } 00706 00707 ////////////////////////////////////////////////////////////////////////////////////////////// 00708 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00709 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const 00710 { 00711 // define point to leaf node voxel center 00712 point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_); 00713 point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_); 00714 point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_); 00715 } 00716 00717 ////////////////////////////////////////////////////////////////////////////////////////////// 00718 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00719 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genVoxelCenterFromOctreeKey ( 00720 const OctreeKey & key_arg, 00721 unsigned int tree_depth_arg, 00722 PointT& point_arg) const 00723 { 00724 // generate point for voxel center defined by treedepth (bitLen) and key 00725 point_arg.x = static_cast<float> ((static_cast <double> (key_arg.x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_); 00726 point_arg.y = static_cast<float> ((static_cast <double> (key_arg.y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_); 00727 point_arg.z = static_cast<float> ((static_cast <double> (key_arg.z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_); 00728 } 00729 00730 ////////////////////////////////////////////////////////////////////////////////////////////// 00731 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void 00732 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::genVoxelBoundsFromOctreeKey ( 00733 const OctreeKey & key_arg, 00734 unsigned int tree_depth_arg, 00735 Eigen::Vector3f &min_pt, 00736 Eigen::Vector3f &max_pt) const 00737 { 00738 // calculate voxel size of current tree depth 00739 double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg)); 00740 00741 // calculate voxel bounds 00742 min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->min_x_); 00743 min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->min_y_); 00744 min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->min_z_); 00745 00746 max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->min_x_); 00747 max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->min_y_); 00748 max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->min_z_); 00749 } 00750 00751 ////////////////////////////////////////////////////////////////////////////////////////////// 00752 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double 00753 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getVoxelSquaredSideLen (unsigned int tree_depth_arg) const 00754 { 00755 double side_len; 00756 00757 // side length of the voxel cube increases exponentially with the octree depth 00758 side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg)); 00759 00760 // squared voxel side length 00761 side_len *= side_len; 00762 00763 return (side_len); 00764 } 00765 00766 ////////////////////////////////////////////////////////////////////////////////////////////// 00767 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double 00768 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getVoxelSquaredDiameter (unsigned int tree_depth_arg) const 00769 { 00770 // return the squared side length of the voxel cube as a function of the octree depth 00771 return (getVoxelSquaredSideLen (tree_depth_arg) * 3); 00772 } 00773 00774 ////////////////////////////////////////////////////////////////////////////////////////////// 00775 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int 00776 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::getOccupiedVoxelCentersRecursive ( 00777 const BranchNode* node_arg, 00778 const OctreeKey& key_arg, 00779 AlignedPointTVector &voxel_center_list_arg) const 00780 { 00781 // child iterator 00782 unsigned char child_idx; 00783 00784 int voxel_count = 0; 00785 00786 // iterate over all children 00787 for (child_idx = 0; child_idx < 8; child_idx++) 00788 { 00789 if (!this->branchHasChild (*node_arg, child_idx)) 00790 continue; 00791 00792 const OctreeNode * child_node; 00793 child_node = this->getBranchChildPtr (*node_arg, child_idx); 00794 00795 // generate new key for current branch voxel 00796 OctreeKey new_key; 00797 new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2))); 00798 new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1))); 00799 new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0))); 00800 00801 switch (child_node->getNodeType ()) 00802 { 00803 case BRANCH_NODE: 00804 { 00805 // recursively proceed with indexed child branch 00806 voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg); 00807 break; 00808 } 00809 case LEAF_NODE: 00810 { 00811 PointT new_point; 00812 00813 genLeafNodeCenterFromOctreeKey (new_key, new_point); 00814 voxel_center_list_arg.push_back (new_point); 00815 00816 voxel_count++; 00817 break; 00818 } 00819 default: 00820 break; 00821 } 00822 } 00823 return (voxel_count); 00824 } 00825 00826 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >; 00827 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >; 00828 00829 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >; 00830 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >; 00831 00832 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >; 00833 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >; 00834 00835 #endif /* OCTREE_POINTCLOUD_HPP_ */