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