|
Point Cloud Library (PCL)
1.4.0
|
|
Point Cloud Library (PCL)
1.4.0
|
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 3749 2011-12-31 22:58:01Z rusu $ 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 this->genOctreeKeyforPoint (point, key); 00055 00056 LeafT* leaf = this->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 (this->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 this->genVoxelCenterFromOctreeKey (searchEntryHeap[childIdx].key, treeDepth, voxelCenter); 00209 00210 // generate new priority queue element 00211 searchEntryHeap[childIdx].node = this->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 smallestSquaredDist = pointCandidates.back ().pointDistance_; 00280 } 00281 // pop element from priority queue 00282 searchEntryHeap.pop_back (); 00283 } 00284 00285 return (smallestSquaredDist); 00286 } 00287 00289 template<typename PointT, typename LeafT, typename OctreeT> void 00290 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getNeighborsWithinRadiusRecursive ( 00291 const PointT & point, const double radiusSquared, const OctreeBranch* node, 00292 const OctreeKey& key, unsigned int treeDepth, std::vector<int>& k_indices, 00293 std::vector<float>& k_sqr_distances, int max_nn) const 00294 { 00295 // child iterator 00296 unsigned char childIdx; 00297 00298 // get spatial voxel information 00299 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth); 00300 00301 // iterate over all children 00302 for (childIdx = 0; childIdx < 8; childIdx++) 00303 { 00304 if (!this->branchHasChild (*node, childIdx)) 00305 continue; 00306 00307 const OctreeNode* childNode; 00308 childNode = this->getBranchChild (*node, childIdx); 00309 00310 OctreeKey newKey; 00311 PointT voxelCenter; 00312 double squaredDist; 00313 00314 // generate new key for current branch voxel 00315 newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); 00316 newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); 00317 newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); 00318 00319 // generate voxel center point for voxel at key 00320 this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); 00321 00322 // calculate distance to search point 00323 squaredDist = pointSquaredDist ((const PointT &)voxelCenter, point); 00324 00325 // if distance is smaller than search radius 00326 if (squaredDist + this->epsilon_ <= voxelSquaredDiameter / 4.0 + radiusSquared 00327 + sqrt (voxelSquaredDiameter * radiusSquared)) 00328 { 00329 00330 if (treeDepth < this->octreeDepth_) 00331 { 00332 // we have not reached maximum tree depth 00333 getNeighborsWithinRadiusRecursive (point, radiusSquared, (OctreeBranch*)childNode, newKey, 00334 treeDepth + 1, k_indices, k_sqr_distances, max_nn); 00335 if (k_indices.size () == (unsigned int)max_nn) 00336 return; 00337 } 00338 else 00339 { 00340 // we reached leaf node level 00341 00342 size_t i; 00343 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00344 vector<int> decodedPointVector; 00345 00346 // decode leaf node into decodedPointVector 00347 childLeaf->getData (decodedPointVector); 00348 00349 // Linearly iterate over all decoded (unsorted) points 00350 for (i = 0; i < decodedPointVector.size (); i++) 00351 { 00352 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); 00353 00354 // calculate point distance to search point 00355 squaredDist = pointSquaredDist (candidatePoint, point); 00356 00357 // check if a match is found 00358 if (squaredDist > radiusSquared) 00359 continue; 00360 00361 // add point to result vector 00362 k_indices.push_back (decodedPointVector[i]); 00363 k_sqr_distances.push_back (squaredDist); 00364 00365 if (k_indices.size () == (unsigned int)max_nn) 00366 return; 00367 } 00368 } 00369 } 00370 } 00371 } 00372 00374 template<typename PointT, typename LeafT, typename OctreeT> void 00375 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::approxNearestSearchRecursive ( 00376 const PointT & point, const OctreeBranch* node, const OctreeKey& key, 00377 unsigned int treeDepth, int& result_index, float& sqr_distance) 00378 { 00379 unsigned char childIdx; 00380 unsigned char minChildIdx; 00381 double minVoxelCenterDistance; 00382 00383 OctreeKey minChildKey; 00384 OctreeKey newKey; 00385 00386 const OctreeNode* childNode; 00387 00388 // set minimum voxel distance to maximum value 00389 minVoxelCenterDistance = numeric_limits<double>::max (); 00390 00391 minChildIdx = 0xFF; 00392 00393 // iterate over all children 00394 for (childIdx = 0; childIdx < 8; childIdx++) 00395 { 00396 if (!this->branchHasChild (*node, childIdx)) 00397 continue; 00398 00399 PointT voxelCenter; 00400 double voxelPointDist; 00401 00402 newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); 00403 newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); 00404 newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); 00405 00406 // generate voxel center point for voxel at key 00407 this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); 00408 00409 voxelPointDist = pointSquaredDist (voxelCenter, point); 00410 00411 // search for child voxel with shortest distance to search point 00412 if (voxelPointDist >= minVoxelCenterDistance) 00413 continue; 00414 00415 minVoxelCenterDistance = voxelPointDist; 00416 minChildIdx = childIdx; 00417 minChildKey = newKey; 00418 } 00419 00420 // make sure we found at least one branch child 00421 assert (minChildIdx<8); 00422 00423 childNode = this->getBranchChild (*node, minChildIdx); 00424 00425 if (treeDepth < this->octreeDepth_) 00426 { 00427 // we have not reached maximum tree depth 00428 approxNearestSearchRecursive (point, (OctreeBranch*)childNode, minChildKey, treeDepth + 1, 00429 result_index, sqr_distance); 00430 } 00431 else 00432 { 00433 // we reached leaf node level 00434 00435 double squaredDist; 00436 double smallestSquaredDist; 00437 size_t i; 00438 vector<int> decodedPointVector; 00439 00440 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00441 00442 smallestSquaredDist = numeric_limits<double>::max (); 00443 00444 // decode leaf node into decodedPointVector 00445 childLeaf->getData (decodedPointVector); 00446 00447 // Linearly iterate over all decoded (unsorted) points 00448 for (i = 0; i < decodedPointVector.size (); i++) 00449 { 00450 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); 00451 00452 // calculate point distance to search point 00453 squaredDist = pointSquaredDist (candidatePoint, point); 00454 00455 // check if a closer match is found 00456 if (squaredDist >= smallestSquaredDist) 00457 continue; 00458 00459 result_index = decodedPointVector[i]; 00460 sqr_distance = smallestSquaredDist = squaredDist; 00461 } 00462 } 00463 } 00464 00466 template<typename PointT, typename LeafT, typename OctreeT> double 00467 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::pointSquaredDist ( 00468 const PointT & pointA, const PointT & pointB) const 00469 { 00470 double distX, distY, distZ; 00471 00472 // distance between pointA and pointB for each axis 00473 distX = pointA.x - pointB.x; 00474 distY = pointA.y - pointB.y; 00475 distZ = pointA.z - pointB.z; 00476 00477 // return squared absolute distance between pointA and pointB 00478 return (distX * distX + distY * distY + distZ * distZ); 00479 } 00480 00482 template<typename PointT, typename LeafT, typename OctreeT> int 00483 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCenters ( 00484 Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxelCenterList) const 00485 { 00486 OctreeKey key; 00487 key.x = key.y = key.z = 0; 00488 00489 voxelCenterList.clear (); 00490 00491 // Voxel childIdx remapping 00492 unsigned char a = 0; 00493 00494 double minX, minY, minZ, maxX, maxY, maxZ; 00495 00496 initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); 00497 00498 if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) 00499 return getIntersectedVoxelCentersRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, 00500 voxelCenterList); 00501 00502 return (0); 00503 } 00504 00506 template<typename PointT, typename LeafT, typename OctreeT> int 00507 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndices ( 00508 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices) const 00509 { 00510 OctreeKey key; 00511 key.x = key.y = key.z = 0; 00512 00513 k_indices.clear (); 00514 00515 // Voxel childIdx remapping 00516 unsigned char a = 0; 00517 double minX, minY, minZ, maxX, maxY, maxZ; 00518 00519 initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); 00520 00521 if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) 00522 return getIntersectedVoxelIndicesRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, 00523 k_indices); 00524 return (0); 00525 } 00526 00528 template<typename PointT, typename LeafT, typename OctreeT> int 00529 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelCentersRecursive ( 00530 double minX, double minY, double minZ, 00531 double maxX, double maxY, double maxZ, 00532 unsigned char a, const OctreeNode* node, const OctreeKey& key, 00533 AlignedPointTVector &voxelCenterList) const 00534 { 00535 if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) 00536 return (0); 00537 00538 // If leaf node, get voxel center and increment intersection count 00539 if (node->getNodeType () == LEAF_NODE) 00540 { 00541 PointT newPoint; 00542 00543 this->genLeafNodeCenterFromOctreeKey (key, newPoint); 00544 00545 voxelCenterList.push_back (newPoint); 00546 00547 return (1); 00548 } 00549 00550 // Voxel intersection count for branches children 00551 int voxelCount = 0; 00552 00553 // Voxel mid lines 00554 double midX = 0.5 * (minX + maxX); 00555 double midY = 0.5 * (minY + maxY); 00556 double midZ = 0.5 * (minZ + maxZ); 00557 00558 // First voxel node ray will intersect 00559 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); 00560 00561 // Child index, node and key 00562 unsigned char childIdx; 00563 const OctreeNode *childNode; 00564 OctreeKey childKey; 00565 00566 do 00567 { 00568 if (currNode != 0) 00569 childIdx = currNode ^ a; 00570 else 00571 childIdx = a; 00572 00573 // childNode == 0 if childNode doesn't exist 00574 childNode = this->getBranchChild ((OctreeBranch&)*node, childIdx); 00575 00576 // Generate new key for current branch voxel 00577 childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); 00578 childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); 00579 childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); 00580 00581 // Recursively call each intersected child node, selecting the next 00582 // node intersected by the ray. Children that do not intersect will 00583 // not be traversed. 00584 00585 switch(currNode) 00586 { 00587 case 0: 00588 if (childNode) 00589 voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a, 00590 childNode, childKey, voxelCenterList); 00591 currNode = getNextIntersectedNode(midX, midY, midZ, 4, 2, 1); 00592 break; 00593 00594 case 1: 00595 if (childNode) 00596 voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, midZ, midX, midY, maxZ, a, 00597 childNode, childKey, voxelCenterList); 00598 currNode = getNextIntersectedNode(midX, midY, maxZ, 5, 3, 8); 00599 break; 00600 00601 case 2: 00602 if (childNode) 00603 voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, minZ, midX, maxY, midZ, a, 00604 childNode, childKey, voxelCenterList); 00605 currNode = getNextIntersectedNode(midX, maxY, midZ, 6, 8, 3); 00606 break; 00607 00608 case 3: 00609 if (childNode) 00610 voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, midZ, midX, maxY, maxZ, a, 00611 childNode, childKey, voxelCenterList); 00612 currNode = getNextIntersectedNode(midX, maxY, maxZ, 7, 8, 8); 00613 break; 00614 00615 case 4: 00616 if (childNode) 00617 voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, minZ, maxX, midY, midZ, a, 00618 childNode, childKey, voxelCenterList); 00619 currNode = getNextIntersectedNode(maxX, midY, midZ, 8, 6, 5); 00620 break; 00621 00622 case 5: 00623 if (childNode) 00624 voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, midZ, maxX, midY, maxZ, a, 00625 childNode, childKey, voxelCenterList); 00626 currNode = getNextIntersectedNode(maxX, midY, maxZ, 8, 7, 8); 00627 break; 00628 00629 case 6: 00630 if (childNode) 00631 voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, minZ, maxX, maxY, midZ, a, 00632 childNode, childKey, voxelCenterList); 00633 currNode = getNextIntersectedNode(maxX, maxY, midZ, 8, 8, 7); 00634 break; 00635 00636 case 7: 00637 if (childNode) 00638 voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, 00639 childNode, childKey, voxelCenterList); 00640 currNode = 8; 00641 break; 00642 } 00643 } 00644 while (currNode < 8); 00645 return (voxelCount); 00646 } 00647 00649 template<typename PointT, typename LeafT, typename OctreeT> int 00650 pcl::octree::OctreePointCloudSearch<PointT, LeafT, OctreeT>::getIntersectedVoxelIndicesRecursive ( 00651 double minX, double minY, double minZ, 00652 double maxX, double maxY, double maxZ, 00653 unsigned char a, const OctreeNode* node, const OctreeKey& key, 00654 std::vector<int> &k_indices) const 00655 { 00656 if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) 00657 return (0); 00658 00659 // If leaf node, get voxel center and increment intersection count 00660 if (node->getNodeType () == LEAF_NODE) 00661 { 00662 OctreeLeaf* leaf = (OctreeLeaf*)node; 00663 vector<int> indices; 00664 00665 // decode leaf node into decodedPointVector 00666 leaf->getData (indices); 00667 for (size_t i = 0; i < indices.size (); i++) 00668 { 00669 k_indices.push_back (indices[i]); 00670 } 00671 00672 return (1); 00673 } 00674 00675 // Voxel intersection count for branches children 00676 int voxelCount = 0; 00677 00678 // Voxel mid lines 00679 double midX = 0.5 * (minX + maxX); 00680 double midY = 0.5 * (minY + maxY); 00681 double midZ = 0.5 * (minZ + maxZ); 00682 00683 // First voxel node ray will intersect 00684 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); 00685 00686 // Child index, node and key 00687 unsigned char childIdx; 00688 const OctreeNode *childNode; 00689 OctreeKey childKey; 00690 do 00691 { 00692 if (currNode != 0) 00693 childIdx = currNode ^ a; 00694 else 00695 childIdx = a; 00696 00697 // childNode == 0 if childNode doesn't exist 00698 childNode = this->getBranchChild ((OctreeBranch&)*node, childIdx); 00699 // Generate new key for current branch voxel 00700 childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); 00701 childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); 00702 childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); 00703 00704 // Recursively call each intersected child node, selecting the next 00705 // node intersected by the ray. Children that do not intersect will 00706 // not be traversed. 00707 switch(currNode) 00708 { 00709 case 0: 00710 if (childNode) 00711 voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, minZ, midX, midY, midZ, a, 00712 childNode, childKey, k_indices); 00713 currNode = getNextIntersectedNode(midX, midY, midZ, 4, 2, 1); 00714 break; 00715 00716 case 1: 00717 if (childNode) 00718 voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, midZ, midX, midY, maxZ, a, 00719 childNode, childKey, k_indices); 00720 currNode = getNextIntersectedNode(midX, midY, maxZ, 5, 3, 8); 00721 break; 00722 00723 case 2: 00724 if (childNode) 00725 voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, minZ, midX, maxY, midZ, a, 00726 childNode, childKey, k_indices); 00727 currNode = getNextIntersectedNode(midX, maxY, midZ, 6, 8, 3); 00728 break; 00729 00730 case 3: 00731 if (childNode) 00732 voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, midZ, midX, maxY, maxZ, a, 00733 childNode, childKey, k_indices); 00734 currNode = getNextIntersectedNode(midX, maxY, maxZ, 7, 8, 8); 00735 break; 00736 00737 case 4: 00738 if (childNode) 00739 voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, minZ, maxX, midY, midZ, a, 00740 childNode, childKey, k_indices); 00741 currNode = getNextIntersectedNode(maxX, midY, midZ, 8, 6, 5); 00742 break; 00743 00744 case 5: 00745 if (childNode) 00746 voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, midZ, maxX, midY, maxZ, a, 00747 childNode, childKey, k_indices); 00748 currNode = getNextIntersectedNode(maxX, midY, maxZ, 8, 7, 8); 00749 break; 00750 00751 case 6: 00752 if (childNode) 00753 voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, minZ, maxX, maxY, midZ, a, 00754 childNode, childKey, k_indices); 00755 currNode = getNextIntersectedNode(maxX, maxY, midZ, 8, 8, 7); 00756 break; 00757 00758 case 7: 00759 if (childNode) 00760 voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, 00761 childNode, childKey, k_indices); 00762 currNode = 8; 00763 break; 00764 } 00765 } while (currNode < 8); 00766 00767 return (voxelCount); 00768 } 00769 00770 #endif // PCL_OCTREE_SEARCH_IMPL_H_
1.7.6.1
