Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: octree_search.h 3018 2011-11-01 03:24:12Z svn $ 00037 */ 00038 00039 #ifndef PCL_OCTREE_SEARCH_H_ 00040 #define PCL_OCTREE_SEARCH_H_ 00041 00042 #include "octree_pointcloud.h" 00043 00044 #include "octree_base.h" 00045 #include "octree2buf_base.h" 00046 00047 #include "octree_nodes.h" 00048 00049 namespace pcl 00050 { 00051 namespace octree 00052 { 00059 template<typename PointT, typename LeafT = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> > 00060 class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafT, OctreeT> 00061 { 00062 public: 00063 // public typedefs 00064 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00065 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00066 00067 typedef pcl::PointCloud<PointT> PointCloud; 00068 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00069 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00070 00071 // public typedefs for single/double buffering 00072 typedef OctreePointCloudSearch<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer; 00073 typedef OctreePointCloudSearch<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer; 00074 typedef OctreePointCloudSearch<PointT, LeafT, OctreeLowMemBase<int, LeafT> > LowMem; 00075 00076 // Boost shared pointers 00077 typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafT, OctreeT> > Ptr; 00078 typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafT, OctreeT> > ConstPtr; 00079 00080 // Eigen aligned allocator 00081 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector; 00082 00083 typedef typename OctreeT::OctreeBranch OctreeBranch; 00084 typedef typename OctreeT::OctreeKey OctreeKey; 00085 typedef typename OctreeT::OctreeLeaf OctreeLeaf; 00086 00090 OctreePointCloudSearch (const double resolution) : 00091 OctreePointCloud<PointT, LeafT, OctreeT> (resolution) 00092 { 00093 } 00094 00096 virtual 00097 ~OctreePointCloudSearch () 00098 { 00099 } 00100 00106 bool 00107 voxelSearch (const PointT& point, std::vector<int>& pointIdx_data); 00108 00114 bool 00115 voxelSearch (const int index, std::vector<int>& pointIdx_data); 00116 00126 inline int 00127 nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices, 00128 std::vector<float> &k_sqr_distances) 00129 { 00130 return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances)); 00131 } 00132 00140 int 00141 nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices, 00142 std::vector<float> &k_sqr_distances); 00143 00153 int 00154 nearestKSearch (int index, int k, std::vector<int> &k_indices, 00155 std::vector<float> &k_sqr_distances); 00156 00164 inline void 00165 approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, 00166 float &sqr_distance) 00167 { 00168 return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance)); 00169 } 00170 00176 void 00177 approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance); 00178 00186 void 00187 approxNearestSearch (int query_index, int &result_index, float &sqr_distance); 00188 00198 int 00199 radiusSearch (const PointCloud &cloud, int index, double radius, 00200 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances, 00201 int max_nn = INT_MAX) 00202 { 00203 return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); 00204 } 00205 00214 int 00215 radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices, 00216 std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const; 00217 00227 int 00228 radiusSearch (int index, const double radius, std::vector<int> &k_indices, 00229 std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const; 00230 00237 int 00238 getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction, 00239 AlignedPointTVector &voxelCenterList) const; 00240 00247 int 00248 getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction, 00249 std::vector<int> &k_indices) const; 00250 00251 00252 protected: 00253 00255 // Octree-based search routines & helpers 00257 00258 00260 00264 00265 class prioBranchQueueEntry 00266 { 00267 public: 00268 00270 prioBranchQueueEntry () 00271 { 00272 } 00273 00279 prioBranchQueueEntry (OctreeNode* node, OctreeKey& key, double pointDistance) 00280 { 00281 node = node; 00282 pointDistance = pointDistance; 00283 key = key; 00284 } 00285 00287 bool 00288 operator< (const prioBranchQueueEntry rhs) const 00289 { 00290 return (this->pointDistance > rhs.pointDistance); 00291 } 00292 00293 // pointer to octree node 00294 const OctreeNode* node; 00295 00296 // distance to query point 00297 double pointDistance; 00298 00299 // octree key 00300 OctreeKey key; 00301 00302 }; 00303 00305 00309 00310 class prioPointQueueEntry 00311 { 00312 public: 00313 00315 prioPointQueueEntry () 00316 { 00317 } 00318 00323 prioPointQueueEntry (unsigned int& pointIdx, double pointDistance) 00324 { 00325 pointIdx_ = pointIdx; 00326 pointDistance_ = pointDistance; 00327 } 00328 00330 bool 00331 operator< (const prioPointQueueEntry& rhs) const 00332 { 00333 return (this->pointDistance_ < rhs.pointDistance_); 00334 } 00335 00336 // index representing a point in the dataset given by \a setInputCloud 00337 int pointIdx_; 00338 00339 // distance to query point 00340 double pointDistance_; 00341 00342 }; 00343 00349 double 00350 pointSquaredDist (const PointT & pointA, const PointT & pointB) const; 00351 00353 // Recursive search routine methods 00355 00356 00367 void 00368 getNeighborsWithinRadiusRecursive (const PointT & point, const double radiusSquared, 00369 const OctreeBranch* node, const OctreeKey& key, 00370 unsigned int treeDepth, std::vector<int>& k_indices, 00371 std::vector<float>& k_sqr_distances, int max_nn) const; 00372 00383 double 00384 getKNearestNeighborRecursive (const PointT & point, unsigned int K, const OctreeBranch* node, 00385 const OctreeKey& key, unsigned int treeDepth, 00386 const double squaredSearchRadius, 00387 std::vector<prioPointQueueEntry>& pointCandidates) const; 00388 00397 void 00398 approxNearestSearchRecursive (const PointT & point, const OctreeBranch* node, const OctreeKey& key, 00399 unsigned int treeDepth, int& result_index, float& sqr_distance); 00400 00416 int 00417 getIntersectedVoxelCentersRecursive (double minX, double minY, double minZ, double maxX, double maxY, 00418 double maxZ, unsigned char a, const OctreeNode* node, 00419 const OctreeKey& key, AlignedPointTVector &voxelCenterList) const; 00420 00436 int 00437 getIntersectedVoxelIndicesRecursive (double minX, double minY, double minZ, 00438 double maxX, double maxY, double maxZ, 00439 unsigned char a, const OctreeNode* node, const OctreeKey& key, 00440 std::vector<int> &k_indices) const; 00450 inline void 00451 initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction, 00452 double &minX, double &minY, double &minZ, 00453 double &maxX, double &maxY, double &maxZ, 00454 unsigned char &a) const 00455 { 00456 // Account for division by zero when direction vector is 0.0 00457 const double epsilon = 1e-10; 00458 if (direction.x () == 0.0) 00459 direction.x () = epsilon; 00460 if (direction.y () == 0.0) 00461 direction.y () = epsilon; 00462 if (direction.z () == 0.0) 00463 direction.z () = epsilon; 00464 00465 // Voxel childIdx remapping 00466 a = 0; 00467 00468 // Handle negative axis direction vector 00469 if (direction.x () < 0.0) 00470 { 00471 origin.x () = this->minX_ + this->maxX_ - origin.x (); 00472 direction.x () = -direction.x (); 00473 a |= 4; 00474 } 00475 if (direction.y () < 0.0) 00476 { 00477 origin.y () = this->minY_ + this->maxY_ - origin.y (); 00478 direction.y () = -direction.y (); 00479 a |= 2; 00480 } 00481 if (direction.z () < 0.0) 00482 { 00483 origin.z () = this->minZ_ + this->maxZ_ - origin.z (); 00484 direction.z () = -direction.z (); 00485 a |= 1; 00486 } 00487 minX = (this->minX_ - origin.x ()) / direction.x (); 00488 maxX = (this->maxX_ - origin.x ()) / direction.x (); 00489 minY = (this->minY_ - origin.y ()) / direction.y (); 00490 maxY = (this->maxY_ - origin.y ()) / direction.y (); 00491 minZ = (this->minZ_ - origin.z ()) / direction.z (); 00492 maxZ = (this->maxZ_ - origin.z ()) / direction.z (); 00493 } 00494 00504 inline int 00505 getFirstIntersectedNode (double minX, double minY, double minZ, double midX, double midY, double midZ) const 00506 { 00507 int currNode = 0; 00508 00509 if (minX > minY) 00510 { 00511 if (minX > minZ) 00512 { 00513 // max(minX, minY, minZ) is minX. Entry plane is YZ. 00514 if (midY < minX) 00515 currNode |= 2; 00516 if (midZ < minX) 00517 currNode |= 1; 00518 } 00519 else 00520 { 00521 // max(minX, minY, minZ) is minZ. Entry plane is XY. 00522 if (midX < minZ) 00523 currNode |= 4; 00524 if (midY < minZ) 00525 currNode |= 2; 00526 } 00527 } 00528 else 00529 { 00530 if (minY > minZ) 00531 { 00532 // max(minX, minY, minZ) is minY. Entry plane is XZ. 00533 if (midX < minY) 00534 currNode |= 4; 00535 if (midZ < minY) 00536 currNode |= 1; 00537 } 00538 else 00539 { 00540 // max(minX, minY, minZ) is minZ. Entry plane is XY. 00541 if (midX < minZ) 00542 currNode |= 4; 00543 if (midY < minZ) 00544 currNode |= 2; 00545 } 00546 } 00547 00548 return currNode; 00549 } 00550 00563 inline int 00564 getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const 00565 { 00566 if (x < y) 00567 { 00568 if (x < z) 00569 return a; 00570 else 00571 return c; 00572 } 00573 else 00574 { 00575 if (y < z) 00576 return b; 00577 else 00578 return c; 00579 } 00580 00581 return 0; 00582 } 00583 00584 }; 00585 } 00586 } 00587 00588 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>; 00589 00590 #endif // PCL_OCTREE_SEARCH_H_