00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
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
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
00226 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth);
00227
00228
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
00240 this->genVoxelCenterFromOctreeKey (searchEntryHeap[childIdx].key, treeDepth, voxelCenter);
00241
00242
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
00255
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
00264 childNode = searchEntryHeap.back ().node;
00265 newKey = searchEntryHeap.back ().key;
00266
00267 if (treeDepth < this->octreeDepth_)
00268 {
00269
00270 smallestSquaredDist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (childNode), newKey, treeDepth + 1,
00271 smallestSquaredDist, pointCandidates);
00272 }
00273 else
00274 {
00275
00276
00277 float squaredDist;
00278 size_t i;
00279 vector<int> decodedPointVector;
00280
00281 const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
00282
00283
00284 childLeaf->getData (decodedPointVector);
00285
00286
00287 for (i = 0; i < decodedPointVector.size (); i++)
00288 {
00289
00290 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]);
00291
00292
00293 squaredDist = pointSquaredDist (candidatePoint, point);
00294
00295
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
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
00329 unsigned char childIdx;
00330
00331
00332 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth);
00333
00334
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
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
00353 this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter);
00354
00355
00356 squaredDist = pointSquaredDist (static_cast<const PointT&> (voxelCenter), point);
00357
00358
00359 if (squaredDist + this->epsilon_
00360 <= voxelSquaredDiameter / 4.0 + radiusSquared + sqrt (voxelSquaredDiameter * radiusSquared))
00361 {
00362
00363 if (treeDepth < this->octreeDepth_)
00364 {
00365
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
00374
00375 size_t i;
00376 const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
00377 vector<int> decodedPointVector;
00378
00379
00380 childLeaf->getData (decodedPointVector);
00381
00382
00383 for (i = 0; i < decodedPointVector.size (); i++)
00384 {
00385 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]);
00386
00387
00388 squaredDist = pointSquaredDist (candidatePoint, point);
00389
00390
00391 if (squaredDist > radiusSquared)
00392 continue;
00393
00394
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
00425 minVoxelCenterDistance = numeric_limits<double>::max ();
00426
00427 minChildIdx = 0xFF;
00428
00429
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
00443 this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter);
00444
00445 voxelPointDist = pointSquaredDist (voxelCenter, point);
00446
00447
00448 if (voxelPointDist >= minVoxelCenterDistance)
00449 continue;
00450
00451 minVoxelCenterDistance = voxelPointDist;
00452 minChildIdx = childIdx;
00453 minChildKey = newKey;
00454 }
00455
00456
00457 assert(minChildIdx<8);
00458
00459 childNode = this->getBranchChildPtr (*node, minChildIdx);
00460
00461 if (treeDepth < this->octreeDepth_)
00462 {
00463
00464 approxNearestSearchRecursive (point, static_cast<const BranchNode*> (childNode), minChildKey, treeDepth + 1, result_index,
00465 sqr_distance);
00466 }
00467 else
00468 {
00469
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
00481 childLeaf->getData (decodedPointVector);
00482
00483
00484 for (i = 0; i < decodedPointVector.size (); i++)
00485 {
00486 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]);
00487
00488
00489 squaredDist = pointSquaredDist (candidatePoint, point);
00490
00491
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
00520 unsigned char childIdx;
00521
00522
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
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
00539 Eigen::Vector3f lowerVoxelCorner;
00540 Eigen::Vector3f upperVoxelCorner;
00541
00542 this->genVoxelBoundsFromOctreeKey (newKey, treeDepth, lowerVoxelCorner, upperVoxelCorner);
00543
00544
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
00554 boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (childNode), newKey, treeDepth + 1, k_indices);
00555 }
00556 else
00557 {
00558
00559 size_t i;
00560 vector<int> decodedPointVector;
00561 bool bInBox;
00562
00563 const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
00564
00565
00566 childLeaf->getData (decodedPointVector);
00567
00568
00569 for (i = 0; i < decodedPointVector.size (); i++)
00570 {
00571 const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]);
00572
00573
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
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
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
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
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
00657 int voxelCount = 0;
00658
00659
00660 double midX = 0.5 * (minX + maxX);
00661 double midY = 0.5 * (minY + maxY);
00662 double midZ = 0.5 * (minZ + maxZ);
00663
00664
00665 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ);
00666
00667
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
00680 childNode = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), childIdx);
00681
00682
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
00688
00689
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
00763 if (node->getNodeType () == LEAF_NODE)
00764 {
00765 const LeafNode* leaf = static_cast<const LeafNode*> (node);
00766
00767
00768 leaf->getData (k_indices);
00769
00770 return (1);
00771 }
00772
00773
00774 int voxelCount = 0;
00775
00776
00777 double midX = 0.5 * (minX + maxX);
00778 double midY = 0.5 * (minY + maxY);
00779 double midZ = 0.5 * (minZ + maxZ);
00780
00781
00782 int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ);
00783
00784
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
00796 childNode = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), childIdx);
00797
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
00803
00804
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_