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-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_