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-2012, 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 6119 2012-07-03 18:50:04Z aichim $
00037  */
00038 
00039 #ifndef PCL_OCTREE_SEARCH_H_
00040 #define PCL_OCTREE_SEARCH_H_
00041 
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044 
00045 #include "octree_pointcloud.h"
00046 
00047 #include "octree_base.h"
00048 #include "octree2buf_base.h"
00049 #include "octree_nodes.h"
00050 
00051 namespace pcl
00052 {
00053   namespace octree
00054   {
00061     template<typename PointT, typename LeafT = OctreeContainerDataTVector<int> ,  typename BranchT = OctreeContainerEmpty<int> >
00062     class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafT, BranchT>
00063     {
00064       public:
00065         // public typedefs
00066         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00067         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00068 
00069         typedef pcl::PointCloud<PointT> PointCloud;
00070         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00071         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00072 
00073         // Boost shared pointers
00074         typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafT, BranchT> > Ptr;
00075         typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafT, BranchT> > ConstPtr;
00076 
00077         // Eigen aligned allocator
00078         typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
00079 
00080         typedef OctreePointCloud<PointT, LeafT, BranchT> OctreeT;
00081         typedef typename OctreeT::LeafNode LeafNode;
00082         typedef typename OctreeT::BranchNode BranchNode;
00083 
00087         OctreePointCloudSearch (const double resolution) :
00088           OctreePointCloud<PointT, LeafT, BranchT> (resolution)
00089         {
00090         }
00091 
00093         virtual
00094         ~OctreePointCloudSearch ()
00095         {
00096         }
00097 
00103         bool
00104         voxelSearch (const PointT& point, std::vector<int>& pointIdx_data);
00105 
00111         bool
00112         voxelSearch (const int index, std::vector<int>& pointIdx_data);
00113 
00123         inline int
00124         nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
00125                         std::vector<float> &k_sqr_distances)
00126         {
00127           return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
00128         }
00129 
00137         int
00138         nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
00139                         std::vector<float> &k_sqr_distances);
00140 
00150         int
00151         nearestKSearch (int index, int k, std::vector<int> &k_indices,
00152                         std::vector<float> &k_sqr_distances);
00153 
00161         inline void
00162         approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index,
00163                              float &sqr_distance)
00164         {
00165           return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
00166         }
00167 
00173         void
00174         approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
00175 
00183         void
00184         approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
00185 
00195         int
00196         radiusSearch (const PointCloud &cloud, int index, double radius,
00197                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00198                       unsigned int max_nn = 0)
00199         {
00200           return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
00201         }
00202 
00211         int
00212         radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
00213                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
00214 
00224         int
00225         radiusSearch (int index, const double radius, std::vector<int> &k_indices,
00226                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
00227 
00235         int
00236         getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
00237                                     AlignedPointTVector &voxelCenterList,
00238                                     int maxVoxelCount = 0) const;
00239 
00247         int
00248         getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
00249                                     std::vector<int> &k_indices,
00250                                     int maxVoxelCount = 0) const;
00251 
00252 
00259         int
00260         boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
00261 
00262       protected:
00264         // Octree-based search routines & helpers
00266 
00270         class prioBranchQueueEntry
00271         {
00272           public:
00274             prioBranchQueueEntry () : node (), pointDistance (0), key ()
00275             {
00276             }
00277 
00283             prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
00284               node (_node), pointDistance (_point_distance), key (_key)
00285             {
00286             }
00287 
00291             bool
00292             operator < (const prioBranchQueueEntry rhs) const
00293             {
00294               return (this->pointDistance > rhs.pointDistance);
00295             }
00296 
00298             const OctreeNode* node;
00299 
00301             float pointDistance;
00302 
00304             OctreeKey key;
00305         };
00306 
00308 
00312         class prioPointQueueEntry
00313         {
00314           public:
00315 
00317             prioPointQueueEntry () :
00318               pointIdx_ (0), pointDistance_ (0)
00319             {
00320             }
00321 
00326             prioPointQueueEntry (unsigned int& pointIdx, float pointDistance) :
00327               pointIdx_ (pointIdx), pointDistance_ (pointDistance)
00328             {
00329             }
00330 
00334             bool
00335             operator< (const prioPointQueueEntry& rhs) const
00336             {
00337               return (this->pointDistance_ < rhs.pointDistance_);
00338             }
00339 
00341             int pointIdx_;
00342 
00344             float pointDistance_;
00345         };
00346 
00352         float
00353         pointSquaredDist (const PointT& pointA, const PointT& pointB) const;
00354 
00356         // Recursive search routine methods
00358 
00369         void
00370         getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
00371                                            const BranchNode* node, const OctreeKey& key,
00372                                            unsigned int treeDepth, std::vector<int>& k_indices,
00373                                            std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
00374 
00385         double
00386         getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
00387                                       const OctreeKey& key, unsigned int treeDepth,
00388                                       const double squaredSearchRadius,
00389                                       std::vector<prioPointQueueEntry>& pointCandidates) const;
00390 
00399         void
00400         approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
00401                                       unsigned int treeDepth, int& result_index, float& sqr_distance);
00402 
00419         int
00420         getIntersectedVoxelCentersRecursive (double minX, double minY, double minZ, double maxX, double maxY,
00421                                              double maxZ, unsigned char a, const OctreeNode* node,
00422                                              const OctreeKey& key, AlignedPointTVector &voxelCenterList,
00423                                              int maxVoxelCount) const;
00424 
00425 
00434         void
00435         boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
00436                             const OctreeKey& key, unsigned int treeDepth, std::vector<int>& k_indices) const;
00437 
00454         int
00455         getIntersectedVoxelIndicesRecursive (double minX, double minY, double minZ,
00456                                              double maxX, double maxY, double maxZ,
00457                                              unsigned char a, const OctreeNode* node, const OctreeKey& key,
00458                                              std::vector<int> &k_indices,
00459                                              int maxVoxelCount) const;
00460 
00472         inline void
00473         initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
00474                               double &minX, double &minY, double &minZ,
00475                               double &maxX, double &maxY, double &maxZ,
00476                               unsigned char &a) const
00477         {
00478           // Account for division by zero when direction vector is 0.0
00479           const float epsilon = 1e-10f;
00480           if (direction.x () == 0.0)
00481             direction.x () = epsilon;
00482           if (direction.y () == 0.0)
00483             direction.y () = epsilon;
00484           if (direction.z () == 0.0)
00485             direction.z () = epsilon;
00486 
00487           // Voxel childIdx remapping
00488           a = 0;
00489 
00490           // Handle negative axis direction vector
00491           if (direction.x () < 0.0)
00492           {
00493             origin.x () = static_cast<float> (this->minX_) + static_cast<float> (this->maxX_) - origin.x ();
00494             direction.x () = -direction.x ();
00495             a |= 4;
00496           }
00497           if (direction.y () < 0.0)
00498           {
00499             origin.y () = static_cast<float> (this->minY_) + static_cast<float> (this->maxY_) - origin.y ();
00500             direction.y () = -direction.y ();
00501             a |= 2;
00502           }
00503           if (direction.z () < 0.0)
00504           {
00505             origin.z () = static_cast<float> (this->minZ_) + static_cast<float> (this->maxZ_) - origin.z ();
00506             direction.z () = -direction.z ();
00507             a |= 1;
00508           }
00509           minX = (this->minX_ - origin.x ()) / direction.x ();
00510           maxX = (this->maxX_ - origin.x ()) / direction.x ();
00511           minY = (this->minY_ - origin.y ()) / direction.y ();
00512           maxY = (this->maxY_ - origin.y ()) / direction.y ();
00513           minZ = (this->minZ_ - origin.z ()) / direction.z ();
00514           maxZ = (this->maxZ_ - origin.z ()) / direction.z ();
00515         }
00516 
00526         inline int
00527         getFirstIntersectedNode (double minX, double minY, double minZ, double midX, double midY, double midZ) const
00528         {
00529           int currNode = 0;
00530 
00531           if (minX > minY)
00532           {
00533             if (minX > minZ)
00534             {
00535               // max(minX, minY, minZ) is minX. Entry plane is YZ.
00536               if (midY < minX)
00537                 currNode |= 2;
00538               if (midZ < minX)
00539                 currNode |= 1;
00540             }
00541             else
00542             {
00543               // max(minX, minY, minZ) is minZ. Entry plane is XY.
00544               if (midX < minZ)
00545                 currNode |= 4;
00546               if (midY < minZ)
00547                 currNode |= 2;
00548             }
00549           }
00550           else
00551           {
00552             if (minY > minZ)
00553             {
00554               // max(minX, minY, minZ) is minY. Entry plane is XZ.
00555               if (midX < minY)
00556                 currNode |= 4;
00557               if (midZ < minY)
00558                 currNode |= 1;
00559             }
00560             else
00561             {
00562               // max(minX, minY, minZ) is minZ. Entry plane is XY.
00563               if (midX < minZ)
00564                 currNode |= 4;
00565               if (midY < minZ)
00566                 currNode |= 2;
00567             }
00568           }
00569 
00570           return currNode;
00571         }
00572 
00585         inline int
00586         getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
00587         {
00588           if (x < y)
00589           {
00590             if (x < z)
00591               return a;
00592             else
00593               return c;
00594           }
00595           else
00596           {
00597             if (y < z)
00598               return b;
00599             else
00600               return c;
00601           }
00602 
00603           return 0;
00604         }
00605 
00606       };
00607   }
00608 }
00609 
00610 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
00611 
00612 #endif    // PCL_OCTREE_SEARCH_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:58