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