Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/octree/include/pcl/octree/octree_search.h
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$
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 namespace pcl
00048 {
00049   namespace octree
00050   {
00051     /** \brief @b Octree pointcloud search class
00052       * \note This class provides several methods for spatial neighbor search based on octree structure
00053       * \note typename: PointT: type of point used in pointcloud
00054       * \ingroup octree
00055       * \author Julius Kammerl (julius@kammerl.de)
00056       */
00057     template<typename PointT, typename LeafContainerT = OctreeContainerPointIndices ,  typename BranchContainerT = OctreeContainerEmpty >
00058     class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
00059     {
00060       public:
00061         // public typedefs
00062         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00063         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00064 
00065         typedef pcl::PointCloud<PointT> PointCloud;
00066         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00067         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00068 
00069         // Boost shared pointers
00070         typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > Ptr;
00071         typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> > ConstPtr;
00072 
00073         // Eigen aligned allocator
00074         typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
00075 
00076         typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT> OctreeT;
00077         typedef typename OctreeT::LeafNode LeafNode;
00078         typedef typename OctreeT::BranchNode BranchNode;
00079 
00080         /** \brief Constructor.
00081           * \param[in] resolution octree resolution at lowest octree level
00082           */
00083         OctreePointCloudSearch (const double resolution) :
00084           OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
00085         {
00086         }
00087 
00088         /** \brief Empty class constructor. */
00089         virtual
00090         ~OctreePointCloudSearch ()
00091         {
00092         }
00093 
00094         /** \brief Search for neighbors within a voxel at given point
00095           * \param[in] point point addressing a leaf node voxel
00096           * \param[out] point_idx_data the resultant indices of the neighboring voxel points
00097           * \return "true" if leaf node exist; "false" otherwise
00098           */
00099         bool
00100         voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
00101 
00102         /** \brief Search for neighbors within a voxel at given point referenced by a point index
00103           * \param[in] index the index in input cloud defining the query point
00104           * \param[out] point_idx_data the resultant indices of the neighboring voxel points
00105           * \return "true" if leaf node exist; "false" otherwise
00106           */
00107         bool
00108         voxelSearch (const int index, std::vector<int>& point_idx_data);
00109 
00110         /** \brief Search for k-nearest neighbors at the query point.
00111           * \param[in] cloud the point cloud data
00112           * \param[in] index the index in \a cloud representing the query point
00113           * \param[in] k the number of neighbors to search for
00114           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00115           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00116           * a priori!)
00117           * \return number of neighbors found
00118           */
00119         inline int
00120         nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
00121                         std::vector<float> &k_sqr_distances)
00122         {
00123           return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
00124         }
00125 
00126         /** \brief Search for k-nearest neighbors at given query point.
00127           * \param[in] p_q the given query point
00128           * \param[in] k the number of neighbors to search for
00129           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!)
00130           * \param[out] k_sqr_distances  the resultant squared distances to the neighboring points (must be resized to k a priori!)
00131           * \return number of neighbors found
00132           */
00133         int
00134         nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
00135                         std::vector<float> &k_sqr_distances);
00136 
00137         /** \brief Search for k-nearest neighbors at query point
00138           * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
00139           *        If indices were given in setInputCloud, index will be the position in the indices vector.
00140           * \param[in] k the number of neighbors to search for
00141           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00142           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00143           * a priori!)
00144          * \return number of neighbors found
00145          */
00146         int
00147         nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
00148 
00149         /** \brief Search for approx. nearest neighbor at the query point.
00150           * \param[in] cloud the point cloud data
00151           * \param[in] query_index the index in \a cloud representing the query point
00152           * \param[out] result_index the resultant index of the neighbor point
00153           * \param[out] sqr_distance the resultant squared distance to the neighboring point
00154           * \return number of neighbors found
00155           */
00156         inline void
00157         approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
00158         {
00159           return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
00160         }
00161 
00162         /** \brief Search for approx. nearest neighbor at the query point.
00163           * \param[in] p_q the given query point
00164           * \param[out] result_index the resultant index of the neighbor point
00165           * \param[out] sqr_distance the resultant squared distance to the neighboring point
00166           */
00167         void
00168         approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
00169 
00170         /** \brief Search for approx. nearest neighbor at the query point.
00171           * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud.
00172           *        If indices were given in setInputCloud, index will be the position in the indices vector.
00173           * \param[out] result_index the resultant index of the neighbor point
00174           * \param[out] sqr_distance the resultant squared distance to the neighboring point
00175           * \return number of neighbors found
00176           */
00177         void
00178         approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
00179 
00180         /** \brief Search for all neighbors of query point that are within a given radius.
00181           * \param[in] cloud the point cloud data
00182           * \param[in] index the index in \a cloud representing the query point
00183           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00184           * \param[out] k_indices the resultant indices of the neighboring points
00185           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00186           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
00187           * \return number of neighbors found in radius
00188           */
00189         int
00190         radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
00191                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0)
00192         {
00193           return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
00194         }
00195 
00196         /** \brief Search for all neighbors of query point that are within a given radius.
00197           * \param[in] p_q the given query point
00198           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00199           * \param[out] k_indices the resultant indices of the neighboring points
00200           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00201           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
00202           * \return number of neighbors found in radius
00203           */
00204         int
00205         radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
00206                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
00207 
00208         /** \brief Search for all neighbors of query point that are within a given radius.
00209           * \param[in] index index representing the query point in the dataset given by \a setInputCloud.
00210           *        If indices were given in setInputCloud, index will be the position in the indices vector
00211           * \param[in] radius radius of the sphere bounding all of p_q's neighbors
00212           * \param[out] k_indices the resultant indices of the neighboring points
00213           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00214           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
00215           * \return number of neighbors found in radius
00216           */
00217         int
00218         radiusSearch (int index, const double radius, std::vector<int> &k_indices,
00219                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
00220 
00221         /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
00222           * \param[in] origin ray origin
00223           * \param[in] direction ray direction vector
00224           * \param[out] voxel_center_list results are written to this vector of PointT elements
00225           * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
00226           * \return number of intersected voxels
00227          */
00228         int
00229         getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
00230                                     AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const;
00231 
00232         /** \brief Get indices of all voxels that are intersected by a ray (origin, direction).
00233           * \param[in] origin ray origin
00234           * \param[in] direction ray direction vector
00235           * \param[out] k_indices resulting point indices from intersected voxels
00236           * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
00237          * \return number of intersected voxels
00238          */
00239         int
00240         getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
00241                                     std::vector<int> &k_indices,
00242                                     int max_voxel_count = 0) const;
00243 
00244 
00245         /** \brief Search for points within rectangular search area
00246          * \param[in] min_pt lower corner of search area
00247          * \param[in] max_pt upper corner of search area
00248          * \param[out] k_indices the resultant point indices
00249          * \return number of points found within search area
00250          */
00251         int
00252         boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
00253 
00254       protected:
00255         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00256         // Octree-based search routines & helpers
00257         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00258         /** \brief @b Priority queue entry for branch nodes
00259          *  \note This class defines priority queue entries for the nearest neighbor search.
00260          *  \author Julius Kammerl (julius@kammerl.de)
00261          */
00262         class prioBranchQueueEntry
00263         {
00264         public:
00265           /** \brief Empty constructor  */
00266           prioBranchQueueEntry () :
00267               node (), point_distance (0), key ()
00268           {
00269           }
00270 
00271           /** \brief Constructor for initializing priority queue entry.
00272            * \param _node pointer to octree node
00273            * \param _key octree key addressing voxel in octree structure
00274            * \param[in] _point_distance distance of query point to voxel center
00275            */
00276           prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
00277               node (_node), point_distance (_point_distance), key (_key)
00278           {
00279           }
00280 
00281           /** \brief Operator< for comparing priority queue entries with each other.
00282            * \param[in] rhs the priority queue to compare this against
00283            */
00284           bool
00285           operator < (const prioBranchQueueEntry rhs) const
00286           {
00287             return (this->point_distance > rhs.point_distance);
00288           }
00289 
00290           /** \brief Pointer to octree node. */
00291           const OctreeNode* node;
00292 
00293           /** \brief Distance to query point. */
00294           float point_distance;
00295 
00296           /** \brief Octree key. */
00297           OctreeKey key;
00298         };
00299 
00300         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00301         /** \brief @b Priority queue entry for point candidates
00302           * \note This class defines priority queue entries for the nearest neighbor point candidates.
00303           * \author Julius Kammerl (julius@kammerl.de)
00304          */
00305         class prioPointQueueEntry
00306         {
00307         public:
00308 
00309           /** \brief Empty constructor  */
00310           prioPointQueueEntry () :
00311               point_idx_ (0), point_distance_ (0)
00312           {
00313           }
00314 
00315           /** \brief Constructor for initializing priority queue entry.
00316            * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud
00317            * \param[in] point_distance distance of query point to voxel center
00318            */
00319           prioPointQueueEntry (unsigned int& point_idx, float point_distance) :
00320               point_idx_ (point_idx), point_distance_ (point_distance)
00321           {
00322           }
00323 
00324           /** \brief Operator< for comparing priority queue entries with each other.
00325            * \param[in] rhs priority queue to compare this against
00326            */
00327           bool
00328           operator< (const prioPointQueueEntry& rhs) const
00329           {
00330             return (this->point_distance_ < rhs.point_distance_);
00331           }
00332 
00333           /** \brief Index representing a point in the dataset given by \a setInputCloud. */
00334           int point_idx_;
00335 
00336           /** \brief Distance to query point. */
00337           float point_distance_;
00338         };
00339 
00340         /** \brief Helper function to calculate the squared distance between two points
00341           * \param[in] point_a point A
00342           * \param[in] point_b point B
00343           * \return squared distance between point A and point B
00344           */
00345         float
00346         pointSquaredDist (const PointT& point_a, const PointT& point_b) const;
00347 
00348         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00349         // Recursive search routine methods
00350         //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00351 
00352         /** \brief Recursive search method that explores the octree and finds neighbors within a given radius
00353           * \param[in] point query point
00354           * \param[in] radiusSquared squared search radius
00355           * \param[in] node current octree node to be explored
00356           * \param[in] key octree key addressing a leaf node.
00357           * \param[in] tree_depth current depth/level in the octree
00358           * \param[out] k_indices vector of indices found to be neighbors of query point
00359           * \param[out] k_sqr_distances squared distances of neighbors to query point
00360           * \param[in] max_nn maximum of neighbors to be found
00361           */
00362         void
00363         getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
00364                                            const BranchNode* node, const OctreeKey& key,
00365                                            unsigned int tree_depth, std::vector<int>& k_indices,
00366                                            std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
00367 
00368         /** \brief Recursive search method that explores the octree and finds the K nearest neighbors
00369           * \param[in] point query point
00370           * \param[in] K amount of nearest neighbors to be found
00371           * \param[in] node current octree node to be explored
00372           * \param[in] key octree key addressing a leaf node.
00373           * \param[in] tree_depth current depth/level in the octree
00374           * \param[in] squared_search_radius squared search radius distance
00375           * \param[out] point_candidates priority queue of nearest neigbor point candidates
00376           * \return squared search radius based on current point candidate set found
00377           */
00378         double
00379         getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
00380                                       const OctreeKey& key, unsigned int tree_depth,
00381                                       const double squared_search_radius,
00382                                       std::vector<prioPointQueueEntry>& point_candidates) const;
00383 
00384         /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor
00385           * \param[in] point query point
00386           * \param[in] node current octree node to be explored
00387           * \param[in] key octree key addressing a leaf node.
00388           * \param[in] tree_depth current depth/level in the octree
00389           * \param[out] result_index result index is written to this reference
00390           * \param[out] sqr_distance squared distance to search
00391           */
00392         void
00393         approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
00394                                       unsigned int tree_depth, int& result_index, float& sqr_distance);
00395 
00396         /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers.
00397           * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
00398           * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
00399           * \param[in] min_x octree nodes X coordinate of lower bounding box corner
00400           * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
00401           * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
00402           * \param[in] max_x octree nodes X coordinate of upper bounding box corner
00403           * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
00404           * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
00405           * \param[in] a
00406           * \param[in] node current octree node to be explored
00407           * \param[in] key octree key addressing a leaf node.
00408           * \param[out] voxel_center_list results are written to this vector of PointT elements
00409           * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
00410           * \return number of voxels found
00411           */
00412         int
00413         getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y,
00414                                              double max_z, unsigned char a, const OctreeNode* node,
00415                                              const OctreeKey& key, AlignedPointTVector &voxel_center_list,
00416                                              int max_voxel_count) const;
00417 
00418 
00419         /** \brief Recursive search method that explores the octree and finds points within a rectangular search area
00420          * \param[in] min_pt lower corner of search area
00421          * \param[in] max_pt upper corner of search area
00422          * \param[in] node current octree node to be explored
00423          * \param[in] key octree key addressing a leaf node.
00424          * \param[in] tree_depth current depth/level in the octree
00425          * \param[out] k_indices the resultant point indices
00426          */
00427         void
00428         boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
00429                             const OctreeKey& key, unsigned int tree_depth, std::vector<int>& k_indices) const;
00430 
00431         /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices.
00432           * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal:
00433           * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf
00434           * \param[in] min_x octree nodes X coordinate of lower bounding box corner
00435           * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
00436           * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
00437           * \param[in] max_x octree nodes X coordinate of upper bounding box corner
00438           * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
00439           * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
00440           * \param[in] a
00441           * \param[in] node current octree node to be explored
00442           * \param[in] key octree key addressing a leaf node.
00443           * \param[out] k_indices resulting indices
00444           * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable)
00445           * \return number of voxels found
00446           */
00447         int
00448         getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z,
00449                                              double max_x, double max_y, double max_z,
00450                                              unsigned char a, const OctreeNode* node, const OctreeKey& key,
00451                                              std::vector<int> &k_indices,
00452                                              int max_voxel_count) const;
00453 
00454         /** \brief Initialize raytracing algorithm
00455           * \param origin
00456           * \param direction
00457           * \param[in] min_x octree nodes X coordinate of lower bounding box corner
00458           * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
00459           * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
00460           * \param[in] max_x octree nodes X coordinate of upper bounding box corner
00461           * \param[in] max_y octree nodes Y coordinate of upper bounding box corner
00462           * \param[in] max_z octree nodes Z coordinate of upper bounding box corner
00463           * \param a
00464           */
00465         inline void
00466         initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
00467                               double &min_x, double &min_y, double &min_z,
00468                               double &max_x, double &max_y, double &max_z,
00469                               unsigned char &a) const
00470         {
00471           // Account for division by zero when direction vector is 0.0
00472           const float epsilon = 1e-10f;
00473           if (direction.x () == 0.0)
00474             direction.x () = epsilon;
00475           if (direction.y () == 0.0)
00476             direction.y () = epsilon;
00477           if (direction.z () == 0.0)
00478             direction.z () = epsilon;
00479 
00480           // Voxel childIdx remapping
00481           a = 0;
00482 
00483           // Handle negative axis direction vector
00484           if (direction.x () < 0.0)
00485           {
00486             origin.x () = static_cast<float> (this->min_x_) + static_cast<float> (this->max_x_) - origin.x ();
00487             direction.x () = -direction.x ();
00488             a |= 4;
00489           }
00490           if (direction.y () < 0.0)
00491           {
00492             origin.y () = static_cast<float> (this->min_y_) + static_cast<float> (this->max_y_) - origin.y ();
00493             direction.y () = -direction.y ();
00494             a |= 2;
00495           }
00496           if (direction.z () < 0.0)
00497           {
00498             origin.z () = static_cast<float> (this->min_z_) + static_cast<float> (this->max_z_) - origin.z ();
00499             direction.z () = -direction.z ();
00500             a |= 1;
00501           }
00502           min_x = (this->min_x_ - origin.x ()) / direction.x ();
00503           max_x = (this->max_x_ - origin.x ()) / direction.x ();
00504           min_y = (this->min_y_ - origin.y ()) / direction.y ();
00505           max_y = (this->max_y_ - origin.y ()) / direction.y ();
00506           min_z = (this->min_z_ - origin.z ()) / direction.z ();
00507           max_z = (this->max_z_ - origin.z ()) / direction.z ();
00508         }
00509 
00510         /** \brief Find first child node ray will enter
00511           * \param[in] min_x octree nodes X coordinate of lower bounding box corner
00512           * \param[in] min_y octree nodes Y coordinate of lower bounding box corner
00513           * \param[in] min_z octree nodes Z coordinate of lower bounding box corner
00514           * \param[in] mid_x octree nodes X coordinate of bounding box mid line
00515           * \param[in] mid_y octree nodes Y coordinate of bounding box mid line
00516           * \param[in] mid_z octree nodes Z coordinate of bounding box mid line
00517           * \return the first child node ray will enter
00518           */
00519         inline int
00520         getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
00521         {
00522           int currNode = 0;
00523 
00524           if (min_x > min_y)
00525           {
00526             if (min_x > min_z)
00527             {
00528               // max(min_x, min_y, min_z) is min_x. Entry plane is YZ.
00529               if (mid_y < min_x)
00530                 currNode |= 2;
00531               if (mid_z < min_x)
00532                 currNode |= 1;
00533             }
00534             else
00535             {
00536               // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
00537               if (mid_x < min_z)
00538                 currNode |= 4;
00539               if (mid_y < min_z)
00540                 currNode |= 2;
00541             }
00542           }
00543           else
00544           {
00545             if (min_y > min_z)
00546             {
00547               // max(min_x, min_y, min_z) is min_y. Entry plane is XZ.
00548               if (mid_x < min_y)
00549                 currNode |= 4;
00550               if (mid_z < min_y)
00551                 currNode |= 1;
00552             }
00553             else
00554             {
00555               // max(min_x, min_y, min_z) is min_z. Entry plane is XY.
00556               if (mid_x < min_z)
00557                 currNode |= 4;
00558               if (mid_y < min_z)
00559                 currNode |= 2;
00560             }
00561           }
00562 
00563           return currNode;
00564         }
00565 
00566         /** \brief Get the next visited node given the current node upper
00567           *   bounding box corner. This function accepts three float values, and
00568           *   three int values. The function returns the ith integer where the
00569           *   ith float value is the minimum of the three float values.
00570           * \param[in] x current nodes X coordinate of upper bounding box corner
00571           * \param[in] y current nodes Y coordinate of upper bounding box corner
00572           * \param[in] z current nodes Z coordinate of upper bounding box corner
00573           * \param[in] a next node if exit Plane YZ
00574           * \param[in] b next node if exit Plane XZ
00575           * \param[in] c next node if exit Plane XY
00576           * \return the next child node ray will enter or 8 if exiting
00577           */
00578         inline int
00579         getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
00580         {
00581           if (x < y)
00582           {
00583             if (x < z)
00584               return a;
00585             else
00586               return c;
00587           }
00588           else
00589           {
00590             if (y < z)
00591               return b;
00592             else
00593               return c;
00594           }
00595 
00596           return 0;
00597         }
00598 
00599       };
00600   }
00601 }
00602 
00603 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
00604 
00605 #endif    // PCL_OCTREE_SEARCH_H_