00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
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
00074 typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafT, BranchT> > Ptr;
00075 typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafT, BranchT> > ConstPtr;
00076
00077
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
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
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
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
00488 a = 0;
00489
00490
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
00536 if (midY < minX)
00537 currNode |= 2;
00538 if (midZ < minX)
00539 currNode |= 1;
00540 }
00541 else
00542 {
00543
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
00555 if (midX < minY)
00556 currNode |= 4;
00557 if (midZ < minY)
00558 currNode |= 1;
00559 }
00560 else
00561 {
00562
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_