Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
octree_search.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: octree_search.h 6119 2012-07-03 18:50:04Z aichim $
37  */
38 
39 #ifndef PCL_OCTREE_SEARCH_H_
40 #define PCL_OCTREE_SEARCH_H_
41 
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
44 
45 #include "octree_pointcloud.h"
46 
47 #include "octree_base.h"
48 #include "octree2buf_base.h"
49 #include "octree_nodes.h"
50 
51 namespace pcl
52 {
53  namespace octree
54  {
61  template<typename PointT, typename LeafT = OctreeContainerDataTVector<int> , typename BranchT = OctreeContainerEmpty<int> >
62  class OctreePointCloudSearch : public OctreePointCloud<PointT, LeafT, BranchT>
63  {
64  public:
65  // public typedefs
66  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
67  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
68 
69  typedef pcl::PointCloud<PointT> PointCloud;
70  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
71  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
72 
73  // Boost shared pointers
74  typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafT, BranchT> > Ptr;
75  typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafT, BranchT> > ConstPtr;
76 
77  // Eigen aligned allocator
78  typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
79 
80  typedef OctreePointCloud<PointT, LeafT, BranchT> OctreeT;
81  typedef typename OctreeT::LeafNode LeafNode;
82  typedef typename OctreeT::BranchNode BranchNode;
83 
87  OctreePointCloudSearch (const double resolution) :
88  OctreePointCloud<PointT, LeafT, BranchT> (resolution)
89  {
90  }
91 
93  virtual
94  ~OctreePointCloudSearch ()
95  {
96  }
97 
103  bool
104  voxelSearch (const PointT& point, std::vector<int>& pointIdx_data);
105 
111  bool
112  voxelSearch (const int index, std::vector<int>& pointIdx_data);
113 
123  inline int
124  nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
125  std::vector<float> &k_sqr_distances)
126  {
127  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
128  }
129 
137  int
138  nearestKSearch (const PointT &p_q, int k, std::vector<int> &k_indices,
139  std::vector<float> &k_sqr_distances);
140 
150  int
151  nearestKSearch (int index, int k, std::vector<int> &k_indices,
152  std::vector<float> &k_sqr_distances);
153 
161  inline void
162  approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index,
163  float &sqr_distance)
164  {
165  return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance));
166  }
167 
173  void
174  approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance);
175 
183  void
184  approxNearestSearch (int query_index, int &result_index, float &sqr_distance);
185 
195  int
196  radiusSearch (const PointCloud &cloud, int index, double radius,
197  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
198  unsigned int max_nn = 0)
199  {
200  return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn));
201  }
202 
211  int
212  radiusSearch (const PointT &p_q, const double radius, std::vector<int> &k_indices,
213  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
214 
224  int
225  radiusSearch (int index, const double radius, std::vector<int> &k_indices,
226  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
227 
235  int
236  getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction,
237  AlignedPointTVector &voxelCenterList,
238  int maxVoxelCount = 0) const;
239 
247  int
248  getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction,
249  std::vector<int> &k_indices,
250  int maxVoxelCount = 0) const;
251 
252 
259  int
260  boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector<int> &k_indices) const;
261 
262  protected:
264  // Octree-based search routines & helpers
266 
270  class prioBranchQueueEntry
271  {
272  public:
274  prioBranchQueueEntry () : node (), pointDistance (0), key ()
275  {
276  }
277 
283  prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) :
284  node (_node), pointDistance (_point_distance), key (_key)
285  {
286  }
287 
291  bool
292  operator < (const prioBranchQueueEntry rhs) const
293  {
294  return (this->pointDistance > rhs.pointDistance);
295  }
296 
298  const OctreeNode* node;
299 
301  float pointDistance;
302 
304  OctreeKey key;
305  };
306 
308 
312  class prioPointQueueEntry
313  {
314  public:
315 
317  prioPointQueueEntry () :
318  pointIdx_ (0), pointDistance_ (0)
319  {
320  }
321 
326  prioPointQueueEntry (unsigned int& pointIdx, float pointDistance) :
327  pointIdx_ (pointIdx), pointDistance_ (pointDistance)
328  {
329  }
330 
334  bool
335  operator< (const prioPointQueueEntry& rhs) const
336  {
337  return (this->pointDistance_ < rhs.pointDistance_);
338  }
339 
341  int pointIdx_;
342 
344  float pointDistance_;
345  };
346 
352  float
353  pointSquaredDist (const PointT& pointA, const PointT& pointB) const;
354 
356  // Recursive search routine methods
358 
369  void
370  getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared,
371  const BranchNode* node, const OctreeKey& key,
372  unsigned int treeDepth, std::vector<int>& k_indices,
373  std::vector<float>& k_sqr_distances, unsigned int max_nn) const;
374 
385  double
386  getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node,
387  const OctreeKey& key, unsigned int treeDepth,
388  const double squaredSearchRadius,
389  std::vector<prioPointQueueEntry>& pointCandidates) const;
390 
399  void
400  approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key,
401  unsigned int treeDepth, int& result_index, float& sqr_distance);
402 
419  int
420  getIntersectedVoxelCentersRecursive (double minX, double minY, double minZ, double maxX, double maxY,
421  double maxZ, unsigned char a, const OctreeNode* node,
422  const OctreeKey& key, AlignedPointTVector &voxelCenterList,
423  int maxVoxelCount) const;
424 
425 
434  void
435  boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node,
436  const OctreeKey& key, unsigned int treeDepth, std::vector<int>& k_indices) const;
437 
454  int
455  getIntersectedVoxelIndicesRecursive (double minX, double minY, double minZ,
456  double maxX, double maxY, double maxZ,
457  unsigned char a, const OctreeNode* node, const OctreeKey& key,
458  std::vector<int> &k_indices,
459  int maxVoxelCount) const;
460 
472  inline void
473  initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction,
474  double &minX, double &minY, double &minZ,
475  double &maxX, double &maxY, double &maxZ,
476  unsigned char &a) const
477  {
478  // Account for division by zero when direction vector is 0.0
479  const float epsilon = 1e-10f;
480  if (direction.x () == 0.0)
481  direction.x () = epsilon;
482  if (direction.y () == 0.0)
483  direction.y () = epsilon;
484  if (direction.z () == 0.0)
485  direction.z () = epsilon;
486 
487  // Voxel childIdx remapping
488  a = 0;
489 
490  // Handle negative axis direction vector
491  if (direction.x () < 0.0)
492  {
493  origin.x () = static_cast<float> (this->minX_) + static_cast<float> (this->maxX_) - origin.x ();
494  direction.x () = -direction.x ();
495  a |= 4;
496  }
497  if (direction.y () < 0.0)
498  {
499  origin.y () = static_cast<float> (this->minY_) + static_cast<float> (this->maxY_) - origin.y ();
500  direction.y () = -direction.y ();
501  a |= 2;
502  }
503  if (direction.z () < 0.0)
504  {
505  origin.z () = static_cast<float> (this->minZ_) + static_cast<float> (this->maxZ_) - origin.z ();
506  direction.z () = -direction.z ();
507  a |= 1;
508  }
509  minX = (this->minX_ - origin.x ()) / direction.x ();
510  maxX = (this->maxX_ - origin.x ()) / direction.x ();
511  minY = (this->minY_ - origin.y ()) / direction.y ();
512  maxY = (this->maxY_ - origin.y ()) / direction.y ();
513  minZ = (this->minZ_ - origin.z ()) / direction.z ();
514  maxZ = (this->maxZ_ - origin.z ()) / direction.z ();
515  }
516 
526  inline int
527  getFirstIntersectedNode (double minX, double minY, double minZ, double midX, double midY, double midZ) const
528  {
529  int currNode = 0;
530 
531  if (minX > minY)
532  {
533  if (minX > minZ)
534  {
535  // max(minX, minY, minZ) is minX. Entry plane is YZ.
536  if (midY < minX)
537  currNode |= 2;
538  if (midZ < minX)
539  currNode |= 1;
540  }
541  else
542  {
543  // max(minX, minY, minZ) is minZ. Entry plane is XY.
544  if (midX < minZ)
545  currNode |= 4;
546  if (midY < minZ)
547  currNode |= 2;
548  }
549  }
550  else
551  {
552  if (minY > minZ)
553  {
554  // max(minX, minY, minZ) is minY. Entry plane is XZ.
555  if (midX < minY)
556  currNode |= 4;
557  if (midZ < minY)
558  currNode |= 1;
559  }
560  else
561  {
562  // max(minX, minY, minZ) is minZ. Entry plane is XY.
563  if (midX < minZ)
564  currNode |= 4;
565  if (midY < minZ)
566  currNode |= 2;
567  }
568  }
569 
570  return currNode;
571  }
572 
585  inline int
586  getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const
587  {
588  if (x < y)
589  {
590  if (x < z)
591  return a;
592  else
593  return c;
594  }
595  else
596  {
597  if (y < z)
598  return b;
599  else
600  return c;
601  }
602 
603  return 0;
604  }
605 
606  };
607  }
608 }
609 
610 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
611 
612 #endif // PCL_OCTREE_SEARCH_H_
Generated on Sat Aug 24 2013 17:28:34 for Point Cloud Library (PCL) by   doxygen 1.8.4