Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, 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: octree_pointcloud.hpp 3017 2011-11-01 03:24:04Z rusu $ 00037 */ 00038 00039 #ifndef OCTREE_POINTCLOUD_HPP_ 00040 #define OCTREE_POINTCLOUD_HPP_ 00041 00042 #include <vector> 00043 #include <assert.h> 00044 00045 #include "pcl/common/common.h" 00046 00047 namespace pcl 00048 { 00049 namespace octree 00050 { 00051 00052 using namespace std; 00053 00055 template<typename PointT, typename LeafT, typename OctreeT> 00056 OctreePointCloud<PointT, LeafT, OctreeT>::OctreePointCloud (const double resolution) : 00057 OctreeT (), epsilon_ (0), resolution_ (resolution), minX_ (0.0f), maxX_ (resolution), minY_ (0.0f), 00058 maxY_ (resolution), minZ_ (0.0f), maxZ_ (resolution), maxKeys_ (1), boundingBoxDefined_ (false) 00059 { 00060 assert ( resolution > 0.0f ); 00061 input_ = PointCloudConstPtr (); 00062 } 00063 00065 template<typename PointT, typename LeafT, typename OctreeT> 00066 OctreePointCloud<PointT, LeafT, OctreeT>::~OctreePointCloud () 00067 { 00068 00069 } 00070 00072 template<typename PointT, typename LeafT, typename OctreeT> 00073 void 00074 OctreePointCloud<PointT, LeafT, OctreeT>::addPointsFromInputCloud () 00075 { 00076 size_t i; 00077 00078 assert (this->leafCount_==0); 00079 00080 { 00081 if (indices_) 00082 { 00083 std::vector<int>::const_iterator current = indices_->begin (); 00084 while (current != indices_->end ()) 00085 { 00086 if ((input_->points[*current].x == input_->points[*current].x) && (input_->points[*current].y 00087 == input_->points[*current].y) && (input_->points[*current].z == input_->points[*current].z)) 00088 { 00089 // add points to octree 00090 this->addPointIdx (*current); 00091 ++current; 00092 } 00093 } 00094 } 00095 else 00096 { 00097 for (i = 0; i < input_->points.size (); i++) 00098 { 00099 if ((input_->points[i].x == input_->points[i].x) && (input_->points[i].y == input_->points[i].y) 00100 && (input_->points[i].z == input_->points[i].z)) 00101 { 00102 // add points to octree 00103 this->addPointIdx ((unsigned int)i); 00104 } 00105 } 00106 } 00107 00108 } 00109 } 00110 00112 template<typename PointT, typename LeafT, typename OctreeT> 00113 void 00114 OctreePointCloud<PointT, LeafT, OctreeT>::addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg) 00115 { 00116 00117 this->addPointIdx (pointIdx_arg); 00118 00119 if (indices_arg) 00120 { 00121 indices_arg->push_back (pointIdx_arg); 00122 } 00123 00124 } 00125 00127 template<typename PointT, typename LeafT, typename OctreeT> 00128 void 00129 OctreePointCloud<PointT, LeafT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg) 00130 { 00131 assert (cloud_arg==input_); 00132 00133 cloud_arg->points.push_back (point_arg); 00134 00135 this->addPointIdx (cloud_arg->points.size () - 1); 00136 00137 } 00138 00139 template<typename PointT, typename LeafT, typename OctreeT> 00140 void 00141 OctreePointCloud<PointT, LeafT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, 00142 IndicesPtr indices_arg) 00143 { 00144 00145 assert (cloud_arg==input_); 00146 assert (indices_arg==indices_); 00147 00148 cloud_arg->points.push_back (point_arg); 00149 00150 this->addPointFromCloud (cloud_arg->points.size () - 1, indices_arg); 00151 00152 } 00153 00155 template<typename PointT, typename LeafT, typename OctreeT> 00156 bool 00157 OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const PointT& point_arg) const 00158 { 00159 OctreeKey key; 00160 00161 // generate key for point 00162 this->genOctreeKeyforPoint (point_arg, key); 00163 00164 // search for key in octree 00165 return this->existLeaf (key); 00166 00167 } 00168 00170 template<typename PointT, typename LeafT, typename OctreeT> 00171 bool 00172 OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const int& pointIdx_arg) const 00173 { 00174 00175 // retrieve point from input cloud 00176 const PointT& point = this->input_->points[pointIdx_arg]; 00177 00178 // search for voxel at point in octree 00179 return this->isVoxelOccupiedAtPoint (point); 00180 00181 } 00182 00184 template<typename PointT, typename LeafT, typename OctreeT> 00185 bool 00186 OctreePointCloud<PointT, LeafT, OctreeT>::isVoxelOccupiedAtPoint (const double pointX_arg, 00187 const double pointY_arg, 00188 const double pointZ_arg) const 00189 { 00190 OctreeKey key; 00191 00192 // generate key for point 00193 this->genOctreeKeyforPoint (pointX_arg, pointY_arg, pointZ_arg, key); 00194 00195 return this->existLeaf (key); 00196 } 00197 00199 template<typename PointT, typename LeafT, typename OctreeT> 00200 void 00201 OctreePointCloud<PointT, LeafT, OctreeT>::deleteVoxelAtPoint (const PointT& point_arg) 00202 { 00203 OctreeKey key; 00204 00205 // generate key for point 00206 this->genOctreeKeyforPoint (point_arg, key); 00207 00208 this->removeLeaf (key); 00209 } 00210 00212 template<typename PointT, typename LeafT, typename OctreeT> 00213 void 00214 OctreePointCloud<PointT, LeafT, OctreeT>::deleteVoxelAtPoint (const int& pointIdx_arg) 00215 { 00216 // retrieve point from input cloud 00217 const PointT& point = this->input_->points[pointIdx_arg]; 00218 00219 // delete leaf at point 00220 this->deleteVoxelAtPoint (point); 00221 00222 } 00223 00225 template<typename PointT, typename LeafT, typename OctreeT> 00226 int 00227 OctreePointCloud<PointT, LeafT, OctreeT>::getOccupiedVoxelCenters ( 00228 std::vector<PointT, Eigen::aligned_allocator< 00229 PointT> > &voxelCenterList_arg) const 00230 { 00231 OctreeKey key; 00232 key.x = key.y = key.z = 0; 00233 00234 voxelCenterList_arg.clear (); 00235 voxelCenterList_arg.reserve (this->leafCount_); 00236 00237 return getOccupiedVoxelCentersRecursive (this->rootNode_, key, voxelCenterList_arg); 00238 00239 } 00240 00242 template<typename PointT, typename LeafT, typename OctreeT> 00243 void 00244 OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox () 00245 { 00246 00247 double minX, minY, minZ, maxX, maxY, maxZ; 00248 00249 PointT min_pt; 00250 PointT max_pt; 00251 00252 // bounding box cannot be changed once the octree contains elements 00253 assert ( this->leafCount_ == 0 ); 00254 00255 pcl::getMinMax3D (*input_, min_pt, max_pt); 00256 00257 minX = min_pt.x; 00258 minY = min_pt.y; 00259 minZ = min_pt.z; 00260 00261 maxX = max_pt.x; 00262 maxY = max_pt.y; 00263 maxZ = max_pt.z; 00264 00265 minX -= this->resolution_ * 0.5f; 00266 minY -= this->resolution_ * 0.5f; 00267 minZ -= this->resolution_ * 0.5f; 00268 00269 maxX += this->resolution_ * 0.5f; 00270 maxY += this->resolution_ * 0.5f; 00271 maxZ += this->resolution_ * 0.5f; 00272 00273 // generate bit masks for octree 00274 defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); 00275 00276 } 00277 00279 template<typename PointT, typename LeafT, typename OctreeT> 00280 void 00281 OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double minX_arg, const double minY_arg, 00282 const double minZ_arg, const double maxX_arg, 00283 const double maxY_arg, const double maxZ_arg) 00284 { 00285 00286 // bounding box cannot be changed once the octree contains elements 00287 assert ( this->leafCount_ == 0 ); 00288 00289 assert ( maxX_arg >= minX_arg ); 00290 assert ( maxY_arg >= minY_arg ); 00291 assert ( maxZ_arg >= minZ_arg ); 00292 00293 minX_ = minX_arg; 00294 maxX_ = maxX_arg; 00295 00296 minY_ = minY_arg; 00297 maxY_ = maxY_arg; 00298 00299 minZ_ = minZ_arg; 00300 maxZ_ = maxZ_arg; 00301 00302 minX_ = min (minX_, maxX_); 00303 minY_ = min (minY_, maxY_); 00304 minZ_ = min (minZ_, maxZ_); 00305 00306 maxX_ = max (minX_, maxX_); 00307 maxY_ = max (minY_, maxY_); 00308 maxZ_ = max (minZ_, maxZ_); 00309 00310 // generate bit masks for octree 00311 getKeyBitSize (); 00312 00313 boundingBoxDefined_ = true; 00314 00315 } 00316 00318 template<typename PointT, typename LeafT, typename OctreeT> 00319 void 00320 OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double maxX_arg, const double maxY_arg, 00321 const double maxZ_arg) 00322 { 00323 00324 // bounding box cannot be changed once the octree contains elements 00325 assert ( this->leafCount_ == 0 ); 00326 00327 assert ( maxX_arg >= 0.0f ); 00328 assert ( maxY_arg >= 0.0f ); 00329 assert ( maxZ_arg >= 0.0f ); 00330 00331 minX_ = 0.0f; 00332 maxX_ = maxX_arg; 00333 00334 minY_ = 0.0f; 00335 maxY_ = maxY_arg; 00336 00337 minZ_ = 0.0f; 00338 maxZ_ = maxZ_arg; 00339 00340 minX_ = min (minX_, maxX_); 00341 minY_ = min (minY_, maxY_); 00342 minZ_ = min (minZ_, maxZ_); 00343 00344 maxX_ = max (minX_, maxX_); 00345 maxY_ = max (minY_, maxY_); 00346 maxZ_ = max (minZ_, maxZ_); 00347 00348 // generate bit masks for octree 00349 getKeyBitSize (); 00350 00351 boundingBoxDefined_ = true; 00352 00353 } 00354 00356 template<typename PointT, typename LeafT, typename OctreeT> 00357 void 00358 OctreePointCloud<PointT, LeafT, OctreeT>::defineBoundingBox (const double cubeLen_arg) 00359 { 00360 00361 // bounding box cannot be changed once the octree contains elements 00362 assert ( this->leafCount_ == 0 ); 00363 00364 assert ( cubeLen_arg >= 0.0f ); 00365 00366 minX_ = 0.0f; 00367 maxX_ = cubeLen_arg; 00368 00369 minY_ = 0.0f; 00370 maxY_ = cubeLen_arg; 00371 00372 minZ_ = 0.0f; 00373 maxZ_ = cubeLen_arg; 00374 00375 minX_ = min (minX_, maxX_); 00376 minY_ = min (minY_, maxY_); 00377 minZ_ = min (minZ_, maxZ_); 00378 00379 maxX_ = max (minX_, maxX_); 00380 maxY_ = max (minY_, maxY_); 00381 maxZ_ = max (minZ_, maxZ_); 00382 00383 // generate bit masks for octree 00384 getKeyBitSize (); 00385 00386 boundingBoxDefined_ = true; 00387 00388 } 00389 00391 template<typename PointT, typename LeafT, typename OctreeT> 00392 void 00393 OctreePointCloud<PointT, LeafT, OctreeT>::getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, 00394 double& maxX_arg, double& maxY_arg, double& maxZ_arg) const 00395 { 00396 minX_arg = minX_; 00397 minY_arg = minY_; 00398 minZ_arg = minZ_; 00399 00400 maxX_arg = maxX_; 00401 maxY_arg = maxY_; 00402 maxZ_arg = maxZ_; 00403 } 00404 00406 template<typename PointT, typename LeafT, typename OctreeT> 00407 void 00408 OctreePointCloud<PointT, LeafT, OctreeT>::adoptBoundingBoxToPoint (const PointT& pointIdx_arg) 00409 { 00410 00411 const double minValue = 1e-10; 00412 00413 // increase octree size until point fits into bounding box 00414 while (true) 00415 { 00416 00417 bool bLowerBoundViolationX = (pointIdx_arg.x < minX_); 00418 bool bLowerBoundViolationY = (pointIdx_arg.y < minY_); 00419 bool bLowerBoundViolationZ = (pointIdx_arg.z < minZ_); 00420 00421 bool bUpperBoundViolationX = (pointIdx_arg.x >= maxX_); 00422 bool bUpperBoundViolationY = (pointIdx_arg.y >= maxY_); 00423 bool bUpperBoundViolationZ = (pointIdx_arg.z >= maxZ_); 00424 00425 // do we violate any bounds? 00426 if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX 00427 || bUpperBoundViolationY || bUpperBoundViolationZ || (!boundingBoxDefined_)) 00428 { 00429 00430 double octreeSideLen; 00431 unsigned char childIdx; 00432 00433 if (this->leafCount_ > 0) 00434 { 00435 00436 // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2 00437 childIdx = ((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) | ((!bUpperBoundViolationZ)); 00438 00439 OctreeBranch* newRootBranch; 00440 00441 this->createBranch (newRootBranch); 00442 this->branchCount_++; 00443 00444 this->setBranchChild (*newRootBranch, childIdx, this->rootNode_); 00445 00446 this->rootNode_ = newRootBranch; 00447 00448 octreeSideLen = maxX_ - minX_ - minValue; 00449 00450 if (bUpperBoundViolationX) 00451 { 00452 maxX_ += octreeSideLen; 00453 } 00454 else 00455 { 00456 minX_ -= octreeSideLen; 00457 } 00458 00459 if (bUpperBoundViolationY) 00460 { 00461 maxY_ += octreeSideLen; 00462 } 00463 else 00464 { 00465 minY_ -= octreeSideLen; 00466 } 00467 00468 if (bUpperBoundViolationZ) 00469 { 00470 maxZ_ += octreeSideLen; 00471 } 00472 else 00473 { 00474 minZ_ -= octreeSideLen; 00475 } 00476 00477 } 00478 else 00479 { 00480 00481 // octree is empty - we set the center of the bounding box to our first pixel 00482 this->minX_ = pointIdx_arg.x - this->resolution_ / 2 + minValue; 00483 this->minY_ = pointIdx_arg.y - this->resolution_ / 2 + minValue; 00484 this->minZ_ = pointIdx_arg.z - this->resolution_ / 2 + minValue; 00485 00486 this->maxX_ = pointIdx_arg.x + this->resolution_ / 2 - minValue; 00487 this->maxY_ = pointIdx_arg.y + this->resolution_ / 2 - minValue; 00488 this->maxZ_ = pointIdx_arg.z + this->resolution_ / 2 - minValue; 00489 00490 } 00491 00492 getKeyBitSize (); 00493 00494 boundingBoxDefined_ = true; 00495 00496 } 00497 else 00498 { 00499 // no bound violations anymore - leave while loop 00500 break; 00501 } 00502 } 00503 00504 } 00505 00507 template<typename PointT, typename LeafT, typename OctreeT> 00508 void 00509 OctreePointCloud<PointT, LeafT, OctreeT>::addPointIdx (const int pointIdx_arg) 00510 { 00511 OctreeKey key; 00512 00513 assert (pointIdx_arg < (int) input_->points.size()); 00514 00515 const PointT& point = input_->points[pointIdx_arg]; 00516 00517 // make sure bounding box is big enough 00518 adoptBoundingBoxToPoint (point); 00519 00520 // generate key 00521 genOctreeKeyforPoint (point, key); 00522 00523 // add point to octree at key 00524 this->add (key, pointIdx_arg); 00525 00526 } 00527 00529 template<typename PointT, typename LeafT, typename OctreeT> 00530 const PointT& 00531 OctreePointCloud<PointT, LeafT, OctreeT>::getPointByIndex (const unsigned int index_arg) const 00532 { 00533 // retrieve point from input cloud 00534 00535 if (indices_ == 0) 00536 { 00537 assert (index_arg < (unsigned int)input_->points.size ()); 00538 return this->input_->points[index_arg]; 00539 } 00540 else 00541 { 00542 assert (index_arg < (unsigned int)indices_->size ()); 00543 return input_->points[(*indices_)[index_arg]]; 00544 } 00545 00546 } 00547 00549 template<typename PointT, typename LeafT, typename OctreeT> 00550 LeafT* 00551 OctreePointCloud<PointT, LeafT, OctreeT>::findLeafAtPoint (const PointT& point) const 00552 { 00553 OctreeKey key; 00554 00555 // generate key for point 00556 this->genOctreeKeyforPoint (point, key); 00557 00558 return this->findLeaf (key); 00559 } 00560 00562 template<typename PointT, typename LeafT, typename OctreeT> 00563 void 00564 OctreePointCloud<PointT, LeafT, OctreeT>::getKeyBitSize () 00565 { 00566 unsigned int maxVoxels; 00567 00568 unsigned int maxKeyX; 00569 unsigned int maxKeyY; 00570 unsigned int maxKeyZ; 00571 00572 double octreeSideLen; 00573 00574 const double minValue = 1e-10; 00575 00576 // find maximum key values for x, y, z 00577 maxKeyX = ceil ((maxX_ - minX_) / resolution_); 00578 maxKeyY = ceil ((maxY_ - minY_) / resolution_); 00579 maxKeyZ = ceil ((maxZ_ - minZ_) / resolution_); 00580 00581 // find maximum amount of keys 00582 maxVoxels = max (max (max (maxKeyX, maxKeyY), maxKeyZ), (unsigned int)2); 00583 00584 // tree depth == amount of bits of maxVoxels 00585 this->octreeDepth_ = max ((min ((unsigned int)OCT_MAXTREEDEPTH, (unsigned int)ceil (this->Log2 (maxVoxels)))), 00586 (unsigned int)0); 00587 00588 maxKeys_ = (1 << this->octreeDepth_); 00589 00590 octreeSideLen = (double)maxKeys_ * resolution_ - minValue; 00591 00592 if (this->leafCount_ == 0) 00593 { 00594 00595 double octreeOversizeX; 00596 double octreeOversizeY; 00597 double octreeOversizeZ; 00598 00599 octreeOversizeX = (octreeSideLen - (maxX_ - minX_)) / 2.0; 00600 octreeOversizeY = (octreeSideLen - (maxY_ - minY_)) / 2.0; 00601 octreeOversizeZ = (octreeSideLen - (maxZ_ - minZ_)) / 2.0; 00602 00603 minX_ -= octreeOversizeX; 00604 minY_ -= octreeOversizeY; 00605 minZ_ -= octreeOversizeZ; 00606 00607 maxX_ += octreeOversizeX; 00608 maxY_ += octreeOversizeY; 00609 maxZ_ += octreeOversizeZ; 00610 00611 } 00612 else 00613 { 00614 00615 maxX_ = minX_ + octreeSideLen; 00616 maxY_ = minY_ + octreeSideLen; 00617 maxZ_ = minZ_ + octreeSideLen; 00618 00619 } 00620 00621 // configure tree depth of octree 00622 this->setTreeDepth (this->octreeDepth_); 00623 00624 return; 00625 00626 } 00627 00629 template<typename PointT, typename LeafT, typename OctreeT> 00630 void 00631 OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyforPoint (const PointT& point_arg, OctreeKey & key_arg) const 00632 { 00633 00634 // calculate integer key for point coordinates 00635 key_arg.x = (unsigned int)((point_arg.x - this->minX_) / this->resolution_); 00636 key_arg.y = (unsigned int)((point_arg.y - this->minY_) / this->resolution_); 00637 key_arg.z = (unsigned int)((point_arg.z - this->minZ_) / this->resolution_); 00638 00639 } 00640 00642 template<typename PointT, typename LeafT, typename OctreeT> 00643 void 00644 OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, 00645 const double pointZ_arg, OctreeKey & key_arg) const 00646 { 00647 PointT tempPoint; 00648 00649 tempPoint.x = pointX_arg; 00650 tempPoint.y = pointY_arg; 00651 tempPoint.z = pointZ_arg; 00652 00653 // generate key for point 00654 genOctreeKeyforPoint (tempPoint, key_arg); 00655 } 00656 00658 template<typename PointT, typename LeafT, typename OctreeT> 00659 bool 00660 OctreePointCloud<PointT, LeafT, OctreeT>::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const 00661 { 00662 00663 const PointT tempPoint = getPointByIndex (data_arg); 00664 00665 // generate key for point 00666 genOctreeKeyforPoint (tempPoint, key_arg); 00667 00668 return true; 00669 00670 } 00671 00673 template<typename PointT, typename LeafT, typename OctreeT> 00674 void 00675 OctreePointCloud<PointT, LeafT, OctreeT>::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const 00676 { 00677 00678 // define point to leaf node voxel center 00679 point.x = ((double)key.x + 0.5f) * this->resolution_ + this->minX_; 00680 point.y = ((double)key.y + 0.5f) * this->resolution_ + this->minY_; 00681 point.z = ((double)key.z + 0.5f) * this->resolution_ + this->minZ_; 00682 00683 } 00684 00686 template<typename PointT, typename LeafT, typename OctreeT> 00687 void 00688 OctreePointCloud<PointT, LeafT, OctreeT>::genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, 00689 unsigned int treeDepth_arg, 00690 PointT& point_arg) const 00691 { 00692 00693 // generate point for voxel center defined by treedepth (bitLen) and key 00694 point_arg.x = ((double)(key_arg.x) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_ 00695 - treeDepth_arg))) + this->minX_; 00696 point_arg.y = ((double)(key_arg.y) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_ 00697 - treeDepth_arg))) + this->minY_; 00698 point_arg.z = ((double)(key_arg.z) + 0.5f) * (this->resolution_ * (double)(1 << (this->octreeDepth_ 00699 - treeDepth_arg))) + this->minZ_; 00700 } 00701 00703 template<typename PointT, typename LeafT, typename OctreeT> 00704 void 00705 OctreePointCloud<PointT, LeafT, OctreeT>::genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg, 00706 unsigned int treeDepth_arg, 00707 Eigen::Vector3f &min_pt, 00708 Eigen::Vector3f &max_pt) const 00709 { 00710 // calculate voxel size of current tree depth 00711 double voxel_side_len = this->resolution_ * (double)(1 << (this->octreeDepth_ - treeDepth_arg)); 00712 00713 // calculate voxel bounds 00714 min_pt (0) = (double)(key_arg.x) * voxel_side_len + this->minX_; 00715 min_pt (1) = (double)(key_arg.y) * voxel_side_len + this->minY_; 00716 min_pt (2) = (double)(key_arg.z) * voxel_side_len + this->minZ_; 00717 00718 max_pt (0) = (double)(key_arg.x + 1) * voxel_side_len + this->minX_; 00719 max_pt (1) = (double)(key_arg.y + 1) * voxel_side_len + this->minY_; 00720 max_pt (2) = (double)(key_arg.z + 1) * voxel_side_len + this->minZ_; 00721 00722 } 00723 00725 template<typename PointT, typename LeafT, typename OctreeT> 00726 double 00727 OctreePointCloud<PointT, LeafT, OctreeT>::getVoxelSquaredSideLen (unsigned int treeDepth_arg) const 00728 { 00729 double sideLen; 00730 00731 // side length of the voxel cube increases exponentially with the octree depth 00732 sideLen = this->resolution_ * (double)(1 << (this->octreeDepth_ - treeDepth_arg)); 00733 00734 // squared voxel side length 00735 sideLen *= sideLen; 00736 00737 return sideLen; 00738 } 00739 00741 template<typename PointT, typename LeafT, typename OctreeT> 00742 double 00743 OctreePointCloud<PointT, LeafT, OctreeT>::getVoxelSquaredDiameter (unsigned int treeDepth_arg) const 00744 { 00745 // return the squared side length of the voxel cube as a function of the octree depth 00746 return getVoxelSquaredSideLen (treeDepth_arg) * 3; 00747 } 00748 00750 template<typename PointT, typename LeafT, typename OctreeT> 00751 int 00752 OctreePointCloud<PointT, LeafT, OctreeT>::getOccupiedVoxelCentersRecursive ( 00753 const OctreeBranch* node_arg, 00754 const OctreeKey& key_arg, 00755 std::vector<PointT, 00756 Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const 00757 { 00758 // child iterator 00759 unsigned char childIdx; 00760 00761 int voxelCount = 0; 00762 00763 // iterate over all children 00764 for (childIdx = 0; childIdx < 8; childIdx++) 00765 { 00766 if (branchHasChild (*node_arg, childIdx)) 00767 { 00768 const OctreeNode * childNode; 00769 childNode = getBranchChild (*node_arg, childIdx); 00770 00771 // generate new key for current branch voxel 00772 OctreeKey newKey; 00773 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00774 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00775 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00776 00777 switch (childNode->getNodeType ()) 00778 { 00779 case BRANCH_NODE: 00780 00781 // recursively proceed with indexed child branch 00782 voxelCount += getOccupiedVoxelCentersRecursive ((OctreeBranch*)childNode, newKey, voxelCenterList_arg); 00783 break; 00784 00785 case LEAF_NODE: 00786 PointT newPoint; 00787 00788 genLeafNodeCenterFromOctreeKey (newKey, newPoint); 00789 00790 voxelCenterList_arg.push_back (newPoint); 00791 00792 voxelCount++; 00793 break; 00794 } 00795 00796 } 00797 00798 } 00799 00800 return voxelCount; 00801 } 00802 00803 } 00804 } 00805 00806 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataTVector<int> > >; 00807 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafDataTVector<int> > >; 00808 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataTVector<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafDataTVector<int> > >; 00809 00810 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataT<int> > >; 00811 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafDataT<int> > >; 00812 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafDataT<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafDataT<int> > >; 00813 00814 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafEmpty<int> > >; 00815 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::Octree2BufBase<int, pcl::octree::OctreeLeafEmpty<int> > >; 00816 #define PCL_INSTANTIATE_OctreePointCloudLowMemWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty<int> , pcl::octree::OctreeLowMemBase<int, pcl::octree::OctreeLeafEmpty<int> > >; 00817 00818 #endif /* OCTREE_POINTCLOUD_HPP_ */