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