Point Cloud Library (PCL)  1.3.1
octree_search.h
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.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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines