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_search.hpp 3018 2011-11-01 03:24:12Z svn $ 00037 */ 00038 00039 #ifndef PCL_OCTREE_SEARCH_IMPL_H_ 00040 #define PCL_OCTREE_SEARCH_IMPL_H_ 00041 00042 #include <pcl/common/common.h> 00043 #include <assert.h> 00044 00046 template<typename PointT, typename LeafT, typename OctreeT> bool 00047 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::voxelSearch ( 00048 const PointT& point, std::vector<int>& pointIdx_data) 00049 { 00050 OctreeKey key; 00051 bool b_success = false; 00052 00053 // generate key 00054 genOctreeKeyforPoint (point, key); 00055 00056 LeafT* leaf = getLeaf (key); 00057 00058 if (leaf) 00059 { 00060 leaf->getData (pointIdx_data); 00061 b_success = true; 00062 } 00063 00064 return (b_success); 00065 } 00066 00068 template<typename PointT, typename LeafT, typename OctreeT> bool 00069 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::voxelSearch ( 00070 const int index, std::vector<int>& pointIdx_data) 00071 { 00072 const PointT search_point = this->getPointByIndex (index); 00073 return (this->voxelSearch (search_point, pointIdx_data)); 00074 } 00075 00077 template<typename PointT, typename LeafT, typename OctreeT> int 00078 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::nearestKSearch ( 00079 const PointT &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) 00080 { 00081 unsigned int i; 00082 unsigned int resultCount; 00083 00084 prioPointQueueEntry pointEntry; 00085 std::vector<prioPointQueueEntry> pointCandidates; 00086 00087 assert (this->leafCount_>0); 00088 00089 OctreeKey key; 00090 key.x = key.y = key.z = 0; 00091 00092 // initalize smallest point distance in search with high value 00093 double smallestDist = numeric_limits<double>::max (); 00094 00095 k_indices.clear (); 00096 k_sqr_distances.clear (); 00097 00098 getKNearestNeighborRecursive (p_q, k, this->rootNode_, key, 1, smallestDist, pointCandidates); 00099 00100 resultCount = pointCandidates.size (); 00101 00102 for (i = 0; i < resultCount; i++) 00103 { 00104 pointEntry = pointCandidates.back (); 00105 00106 k_indices.push_back (pointEntry.pointIdx_); 00107 k_sqr_distances.push_back (pointEntry.pointDistance_); 00108 00109 pointCandidates.pop_back (); 00110 } 00111 00112 return k_indices.size (); 00113 00114 } 00115 00117 template<typename PointT, typename LeafT, typename OctreeT> int 00118 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::nearestKSearch ( 00119 int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) 00120 { 00121 const PointT search_point = this->getPointByIndex (index); 00122 return (nearestKSearch (search_point, k, k_indices, k_sqr_distances)); 00123 } 00124 00126 template<typename PointT, typename LeafT, typename OctreeT> void 00127 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearch ( 00128 const PointT &p_q, int &result_index, float &sqr_distance) 00129 { 00130 assert (this->leafCount_>0); 00131 00132 OctreeKey key; 00133 key.x = key.y = key.z = 0; 00134 00135 approxNearestSearchRecursive (p_q, this->rootNode_, key, 1, result_index, sqr_distance); 00136 } 00137 00139 template<typename PointT, typename LeafT, typename OctreeT> void 00140 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearch ( 00141 int query_index, int &result_index, float &sqr_distance) 00142 { 00143 const PointT searchPoint = this->getPointByIndex (query_index); 00144 00145 return (approxNearestSearch (searchPoint, result_index, sqr_distance)); 00146 } 00147 00149 template<typename PointT, typename LeafT, typename OctreeT> int 00150 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::radiusSearch ( 00151 const PointT &p_q, const double radius, std::vector<int> &k_indices, 00152 std::vector<float> &k_sqr_distances, int max_nn) const 00153 { 00154 OctreeKey key; 00155 key.x = key.y = key.z = 0; 00156 00157 k_indices.clear (); 00158 k_sqr_distances.clear (); 00159 00160 getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->rootNode_, key, 1, k_indices, 00161 k_sqr_distances, max_nn); 00162 00163 return (k_indices.size ()); 00164 } 00165 00167 template<typename PointT, typename LeafT, typename OctreeT> int 00168 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::radiusSearch ( 00169 int index, const double radius, std::vector<int> &k_indices, 00170 std::vector<float> &k_sqr_distances, int max_nn) const 00171 { 00172 const PointT search_point = this->getPointByIndex (index); 00173 00174 return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn)); 00175 } 00176 00178 template<typename PointT, typename LeafT, typename OctreeT> double 00179 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getKNearestNeighborRecursive ( 00180 const PointT & point, unsigned int K, const OctreeBranch* node, const OctreeKey& key, 00181 unsigned int treeDepth, const double squaredSearchRadius, 00182 std::vector<prioPointQueueEntry>& pointCandidates) const 00183 { 00184 std::vector<prioBranchQueueEntry> searchEntryHeap; 00185 searchEntryHeap.resize (8); 00186 00187 unsigned char childIdx; 00188 00189 OctreeKey newKey; 00190 00191 double smallestSquaredDist = squaredSearchRadius; 00192 00193 // get spatial voxel information 00194 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth); 00195 00196 // iterate over all children 00197 for (childIdx = 0; childIdx < 8; childIdx++) 00198 { 00199 if (branchHasChild (*node, childIdx)) 00200 { 00201 PointT voxelCenter; 00202 00203 searchEntryHeap[childIdx].key.x = (key.x << 1) + (!!(childIdx & (1 << 2))); 00204 searchEntryHeap[childIdx].key.y = (key.y << 1) + (!!(childIdx & (1 << 1))); 00205 searchEntryHeap[childIdx].key.z = (key.z << 1) + (!!(childIdx & (1 << 0))); 00206 00207 // generate voxel center point for voxel at key 00208 genVoxelCenterFromOctreeKey (searchEntryHeap[childIdx].key, treeDepth, voxelCenter); 00209 00210 // generate new priority queue element 00211 searchEntryHeap[childIdx].node = getBranchChild (*node, childIdx); 00212 searchEntryHeap[childIdx].pointDistance = pointSquaredDist (voxelCenter, point); 00213 } 00214 else 00215 { 00216 searchEntryHeap[childIdx].pointDistance = numeric_limits<double>::infinity (); 00217 } 00218 } 00219 00220 std::sort (searchEntryHeap.begin (), searchEntryHeap.end ()); 00221 00222 // iterate over all children in priority queue 00223 // check if the distance to seach candidate is smaller than the best point distance (smallestSquaredDist) 00224 while ((!searchEntryHeap.empty ()) && (searchEntryHeap.back ().pointDistance < smallestSquaredDist 00225 + voxelSquaredDiameter / 4.0 + sqrt (smallestSquaredDist * voxelSquaredDiameter) - this->epsilon_)) 00226 { 00227 const OctreeNode* childNode; 00228 00229 // read from priority queue element 00230 childNode = searchEntryHeap.back ().node; 00231 newKey = searchEntryHeap.back ().key; 00232 00233 if (treeDepth < this->octreeDepth_) 00234 { 00235 // we have not reached maximum tree depth 00236 smallestSquaredDist = getKNearestNeighborRecursive (point, K, (OctreeBranch*)childNode, newKey, 00237 treeDepth + 1, smallestSquaredDist, 00238 pointCandidates); 00239 } 00240 else 00241 { 00242 // we reached leaf node level 00243 00244 double squaredDist; 00245 size_t i; 00246 vector<int> decodedPointVector; 00247 00248 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00249 00250 // decode leaf node into decodedPointVector 00251 childLeaf->getData (decodedPointVector); 00252 00253 // Linearly iterate over all decoded (unsorted) points 00254 for (i = 0; i < decodedPointVector.size (); i++) 00255 { 00256 00257 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); 00258 00259 // calculate point distance to search point 00260 squaredDist = pointSquaredDist (candidatePoint, point); 00261 00262 // check if a closer match is found 00263 if (squaredDist < smallestSquaredDist) 00264 { 00265 prioPointQueueEntry pointEntry; 00266 00267 pointEntry.pointDistance_ = squaredDist; 00268 pointEntry.pointIdx_ = decodedPointVector[i]; 00269 pointCandidates.push_back (pointEntry); 00270 } 00271 } 00272 00273 std::sort (pointCandidates.begin (), pointCandidates.end ()); 00274 00275 if (pointCandidates.size () > K) 00276 pointCandidates.resize (K); 00277 00278 if (pointCandidates.size () == K) 00279 { 00280 smallestSquaredDist = pointCandidates.back ().pointDistance_; 00281 } 00282 } 00283 // pop element from priority queue 00284 searchEntryHeap.pop_back (); 00285 } 00286 00287 return (smallestSquaredDist); 00288 } 00289 00291 template<typename PointT, typename LeafT, typename OctreeT> void 00292 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getNeighborsWithinRadiusRecursive ( 00293 const PointT & point, const double radiusSquared, const OctreeBranch* node, 00294 const OctreeKey& key, unsigned int treeDepth, std::vector<int>& k_indices, 00295 std::vector<float>& k_sqr_distances, int max_nn) const 00296 { 00297 // child iterator 00298 unsigned char childIdx; 00299 00300 // get spatial voxel information 00301 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth); 00302 00303 // iterate over all children 00304 for (childIdx = 0; childIdx < 8; childIdx++) 00305 { 00306 if (!branchHasChild (*node, childIdx)) 00307 continue; 00308 00309 const OctreeNode* childNode; 00310 childNode = getBranchChild (*node, childIdx); 00311 00312 OctreeKey newKey; 00313 PointT voxelCenter; 00314 double squaredDist; 00315 00316 // generate new key for current branch voxel 00317 newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); 00318 newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); 00319 newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); 00320 00321 // generate voxel center point for voxel at key 00322 genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); 00323 00324 // calculate distance to search point 00325 squaredDist = pointSquaredDist ((const PointT &)voxelCenter, point); 00326 00327 // if distance is smaller than search radius 00328 if (squaredDist + this->epsilon_ <= voxelSquaredDiameter / 4.0 + radiusSquared 00329 + sqrt (voxelSquaredDiameter * radiusSquared)) 00330 { 00331 00332 if (treeDepth < this->octreeDepth_) 00333 { 00334 // we have not reached maximum tree depth 00335 getNeighborsWithinRadiusRecursive (point, radiusSquared, (OctreeBranch*)childNode, newKey, 00336 treeDepth + 1, k_indices, k_sqr_distances, max_nn); 00337 if (k_indices.size () == (unsigned int)max_nn) 00338 return; 00339 } 00340 else 00341 { 00342 // we reached leaf node level 00343 00344 size_t i; 00345 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00346 vector<int> decodedPointVector; 00347 00348 // decode leaf node into decodedPointVector 00349 childLeaf->getData (decodedPointVector); 00350 00351 // Linearly iterate over all decoded (unsorted) points 00352 for (i = 0; i < decodedPointVector.size (); i++) 00353 { 00354 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); 00355 00356 // calculate point distance to search point 00357 squaredDist = pointSquaredDist (candidatePoint, point); 00358 00359 // check if a match is found 00360 if (squaredDist > radiusSquared) 00361 continue; 00362 00363 // add point to result vector 00364 k_indices.push_back (decodedPointVector[i]); 00365 k_sqr_distances.push_back (squaredDist); 00366 00367 if (k_indices.size () == (unsigned int)max_nn) 00368 return; 00369 } 00370 } 00371 } 00372 } 00373 } 00374 00376 template<typename PointT, typename LeafT, typename OctreeT> void 00377 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearchRecursive ( 00378 const PointT & point, const OctreeBranch* node, const OctreeKey& key, 00379 unsigned int treeDepth, int& result_index, float& sqr_distance) 00380 { 00381 unsigned char childIdx; 00382 unsigned char minChildIdx; 00383 double minVoxelCenterDistance; 00384 00385 OctreeKey minChildKey; 00386 OctreeKey newKey; 00387 00388 const OctreeNode* childNode; 00389 00390 // set minimum voxel distance to maximum value 00391 minVoxelCenterDistance = numeric_limits<double>::max (); 00392 00393 minChildIdx = 0xFF; 00394 00395 // iterate over all children 00396 for (childIdx = 0; childIdx < 8; childIdx++) 00397 { 00398 if (!branchHasChild (*node, childIdx)) 00399 continue; 00400 00401 PointT voxelCenter; 00402 double voxelPointDist; 00403 00404 newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); 00405 newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); 00406 newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); 00407 00408 // generate voxel center point for voxel at key 00409 genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); 00410 00411 voxelPointDist = pointSquaredDist (voxelCenter, point); 00412 00413 // search for child voxel with shortest distance to search point 00414 if (voxelPointDist >= minVoxelCenterDistance) 00415 continue; 00416 00417 minVoxelCenterDistance = voxelPointDist; 00418 minChildIdx = childIdx; 00419 minChildKey = newKey; 00420 } 00421 00422 // make sure we found at least one branch child 00423 assert (minChildIdx<8); 00424 00425 childNode = getBranchChild (*node, minChildIdx); 00426 00427 if (treeDepth < this->octreeDepth_) 00428 { 00429 // we have not reached maximum tree depth 00430 approxNearestSearchRecursive (point, (OctreeBranch*)childNode, minChildKey, treeDepth + 1, 00431 result_index, sqr_distance); 00432 } 00433 else 00434 { 00435 // we reached leaf node level 00436 00437 double squaredDist; 00438 double smallestSquaredDist; 00439 size_t i; 00440 vector<int> decodedPointVector; 00441 00442 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00443 00444 smallestSquaredDist = numeric_limits<double>::max (); 00445 00446 // decode leaf node into decodedPointVector 00447 childLeaf->getData (decodedPointVector); 00448 00449 // Linearly iterate over all decoded (unsorted) points 00450 for (i = 0; i < decodedPointVector.size (); i++) 00451 { 00452 00453 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); 00454 00455 // calculate point distance to search point 00456 squaredDist = pointSquaredDist (candidatePoint, point); 00457 00458 // check if a closer match is found 00459 if (squaredDist >= smallestSquaredDist) 00460 continue; 00461 00462 result_index = decodedPointVector[i]; 00463 sqr_distance = smallestSquaredDist = squaredDist; 00464 } 00465 } 00466 } 00467 00469 template<typename PointT, typename LeafT, typename OctreeT> double 00470 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::pointSquaredDist ( 00471 const PointT & pointA, const PointT & pointB) const 00472 { 00473 double distX, distY, distZ; 00474 00475 // distance between pointA and pointB for each axis 00476 distX = pointA.x - pointB.x; 00477 distY = pointA.y - pointB.y; 00478 distZ = pointA.z - pointB.z; 00479 00480 // return squared absolute distance between pointA and pointB 00481 return (distX * distX + distY * distY + distZ * distZ); 00482 } 00483 00485 template<typename PointT, typename LeafT, typename OctreeT> int 00486 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCenters ( 00487 Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxelCenterList) const 00488 { 00489 OctreeKey key; 00490 key.x = key.y = key.z = 0; 00491 00492 voxelCenterList.clear (); 00493 voxelCenterList.reserve (this->leafCount_); 00494 00495 // Voxel childIdx remapping 00496 unsigned char a = 0; 00497 00498 double minX, minY, minZ, maxX, maxY, maxZ; 00499 00500 initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); 00501 00502 if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) 00503 return getIntersectedVoxelCentersRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, 00504 voxelCenterList); 00505 00506 return (0); 00507 } 00508 00510 template<typename PointT, typename LeafT, typename OctreeT> int 00511 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndices ( 00512 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices) const 00513 { 00514 OctreeKey key; 00515 key.x = key.y = key.z = 0; 00516 00517 k_indices.clear (); 00518 k_indices.reserve (this->leafCount_); 00519 00520 // Voxel childIdx remapping 00521 unsigned char a = 0; 00522 double minX, minY, minZ, maxX, maxY, maxZ; 00523 00524 initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); 00525 00526 if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) 00527 return getIntersectedVoxelIndicesRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, 00528 k_indices); 00529 return 0; 00530 } 00531 00533 template<typename PointT, typename LeafT, typename OctreeT> int 00534 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCentersRecursive ( 00535 double minX, double minY, double minZ, 00536 double maxX, double maxY, double maxZ, 00537 unsigned char a, const OctreeNode* node, const OctreeKey& key, 00538 AlignedPointTVector &voxelCenterList) const 00539 { 00540 if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) 00541 return (0); 00542 00543 // If leaf node, get voxel center and increment intersection count 00544 if (node->getNodeType () == LEAF_NODE) 00545 { 00546 PointT newPoint; 00547 00548 genLeafNodeCenterFromOctreeKey (key, newPoint); 00549 00550 voxelCenterList.push_back (newPoint); 00551 00552 return (1); 00553 } 00554 00555 // Voxel intersection count for branches children 00556 int voxelCount = 0; 00557 00558 // Voxel mid lines 00559 double midX = 0.5 * (minX + maxX); 00560 double midY = 0.5 * (minY + maxY); 00561 double midZ = 0.5 * (minZ + maxZ); 00562 00563 // First voxel node ray will intersect 00564 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); 00565 00566 // Child index, node and key 00567 unsigned char childIdx; 00568 const OctreeNode *childNode; 00569 OctreeKey childKey; 00570 00571 do 00572 { 00573 if (currNode != 0) 00574 childIdx = currNode ^ a; 00575 else 00576 childIdx = a; 00577 00578 // childNode == 0 if childNode doesn't exist 00579 childNode = getBranchChild ((OctreeBranch&)*node, childIdx); 00580 00581 // Generate new key for current branch voxel 00582 childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); 00583 childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); 00584 childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); 00585 00586 // Recursively call each intersected child node, selecting the next 00587 // node intersected by the ray. Children that do not intersect will 00588 // not be traversed. 00589 00590 switch(currNode) 00591 { 00592 case 0: 00593 if (childNode) 00594 voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a, 00595 childNode, childKey, voxelCenterList); 00596 currNode = getNextIntersectedNode(midX, midY, midZ, 4, 2, 1); 00597 break; 00598 00599 case 1: 00600 if (childNode) 00601 voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, midZ, midX, midY, maxZ, a, 00602 childNode, childKey, voxelCenterList); 00603 currNode = getNextIntersectedNode(midX, midY, maxZ, 5, 3, 8); 00604 break; 00605 00606 case 2: 00607 if (childNode) 00608 voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, minZ, midX, maxY, midZ, a, 00609 childNode, childKey, voxelCenterList); 00610 currNode = getNextIntersectedNode(midX, maxY, midZ, 6, 8, 3); 00611 break; 00612 00613 case 3: 00614 if (childNode) 00615 voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, midZ, midX, maxY, maxZ, a, 00616 childNode, childKey, voxelCenterList); 00617 currNode = getNextIntersectedNode(midX, maxY, maxZ, 7, 8, 8); 00618 break; 00619 00620 case 4: 00621 if (childNode) 00622 voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, minZ, maxX, midY, midZ, a, 00623 childNode, childKey, voxelCenterList); 00624 currNode = getNextIntersectedNode(maxX, midY, midZ, 8, 6, 5); 00625 break; 00626 00627 case 5: 00628 if (childNode) 00629 voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, midZ, maxX, midY, maxZ, a, 00630 childNode, childKey, voxelCenterList); 00631 currNode = getNextIntersectedNode(maxX, midY, maxZ, 8, 7, 8); 00632 break; 00633 00634 case 6: 00635 if (childNode) 00636 voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, minZ, maxX, maxY, midZ, a, 00637 childNode, childKey, voxelCenterList); 00638 currNode = getNextIntersectedNode(maxX, maxY, midZ, 8, 8, 7); 00639 break; 00640 00641 case 7: 00642 if (childNode) 00643 voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, 00644 childNode, childKey, voxelCenterList); 00645 currNode = 8; 00646 break; 00647 } 00648 } 00649 while (currNode < 8); 00650 return (voxelCount); 00651 } 00652 00654 template<typename PointT, typename LeafT, typename OctreeT> int 00655 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndicesRecursive ( 00656 double minX, double minY, double minZ, 00657 double maxX, double maxY, double maxZ, 00658 unsigned char a, const OctreeNode* node, const OctreeKey& key, 00659 std::vector<int> &k_indices) const 00660 { 00661 if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) 00662 return 0; 00663 00664 // If leaf node, get voxel center and increment intersection count 00665 if (node->getNodeType () == LEAF_NODE) 00666 { 00667 OctreeLeaf* leaf = (OctreeLeaf*)node; 00668 vector<int> indices; 00669 00670 // decode leaf node into decodedPointVector 00671 leaf->getData (indices); 00672 for (size_t i = 0; i < indices.size (); i++) 00673 { 00674 k_indices.push_back (indices[i]); 00675 } 00676 00677 return 1; 00678 } 00679 00680 // Voxel intersection count for branches children 00681 int voxelCount = 0; 00682 00683 // Voxel mid lines 00684 double midX = 0.5 * (minX + maxX); 00685 double midY = 0.5 * (minY + maxY); 00686 double midZ = 0.5 * (minZ + maxZ); 00687 00688 // First voxel node ray will intersect 00689 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); 00690 00691 // Child index, node and key 00692 unsigned char childIdx; 00693 const OctreeNode *childNode; 00694 OctreeKey childKey; 00695 do 00696 { 00697 if (currNode != 0) 00698 childIdx = currNode ^ a; 00699 else 00700 childIdx = a; 00701 00702 // childNode == 0 if childNode doesn't exist 00703 childNode = getBranchChild ((OctreeBranch&)*node, childIdx); 00704 // Generate new key for current branch voxel 00705 childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); 00706 childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); 00707 childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); 00708 00709 // Recursively call each intersected child node, selecting the next 00710 // node intersected by the ray. Children that do not intersect will 00711 // not be traversed. 00712 switch(currNode) 00713 { 00714 case 0: 00715 if (childNode) 00716 voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, minZ, midX, midY, midZ, a, 00717 childNode, childKey, k_indices); 00718 currNode = getNextIntersectedNode(midX, midY, midZ, 4, 2, 1); 00719 break; 00720 00721 case 1: 00722 if (childNode) 00723 voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, midZ, midX, midY, maxZ, a, 00724 childNode, childKey, k_indices); 00725 currNode = getNextIntersectedNode(midX, midY, maxZ, 5, 3, 8); 00726 break; 00727 00728 case 2: 00729 if (childNode) 00730 voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, minZ, midX, maxY, midZ, a, 00731 childNode, childKey, k_indices); 00732 currNode = getNextIntersectedNode(midX, maxY, midZ, 6, 8, 3); 00733 break; 00734 00735 case 3: 00736 if (childNode) 00737 voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, midZ, midX, maxY, maxZ, a, 00738 childNode, childKey, k_indices); 00739 currNode = getNextIntersectedNode(midX, maxY, maxZ, 7, 8, 8); 00740 break; 00741 00742 case 4: 00743 if (childNode) 00744 voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, minZ, maxX, midY, midZ, a, 00745 childNode, childKey, k_indices); 00746 currNode = getNextIntersectedNode(maxX, midY, midZ, 8, 6, 5); 00747 break; 00748 00749 case 5: 00750 if (childNode) 00751 voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, midZ, maxX, midY, maxZ, a, 00752 childNode, childKey, k_indices); 00753 currNode = getNextIntersectedNode(maxX, midY, maxZ, 8, 7, 8); 00754 break; 00755 00756 case 6: 00757 if (childNode) 00758 voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, minZ, maxX, maxY, midZ, a, 00759 childNode, childKey, k_indices); 00760 currNode = getNextIntersectedNode(maxX, maxY, midZ, 8, 8, 7); 00761 break; 00762 00763 case 7: 00764 if (childNode) 00765 voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, 00766 childNode, childKey, k_indices); 00767 currNode = 8; 00768 break; 00769 } 00770 } while (currNode < 8); 00771 00772 return voxelCount; 00773 } 00774 00775 #endif // PCL_OCTREE_SEARCH_IMPL_H_