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