39 #ifndef PCL_OCTREE_SEARCH_H_
40 #define PCL_OCTREE_SEARCH_H_
42 #include <pcl/point_cloud.h>
44 #include <pcl/octree/octree_pointcloud.h>
56 template<
typename Po
intT,
typename LeafContainerT = OctreeContainerPo
intIndices ,
typename BranchContainerT = OctreeContainerEmpty >
69 typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >
Ptr;
70 typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >
ConstPtr;
107 voxelSearch (
const int index, std::vector<int>& point_idx_data);
119 nearestKSearch (
const PointCloud &cloud,
int index,
int k, std::vector<int> &k_indices,
120 std::vector<float> &k_sqr_distances)
122 return (
nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
134 std::vector<float> &k_sqr_distances);
146 nearestKSearch (
int index,
int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
189 radiusSearch (
const PointCloud &cloud,
int index,
double radius, std::vector<int> &k_indices,
190 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
192 return (
radiusSearch (cloud.
points[index], radius, k_indices, k_sqr_distances, max_nn));
205 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
const;
217 radiusSearch (
int index,
const double radius, std::vector<int> &k_indices,
218 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
const;
229 AlignedPointTVector &voxel_center_list,
int max_voxel_count = 0)
const;
240 std::vector<int> &k_indices,
241 int max_voxel_count = 0)
const;
252 boxSearch (
const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt, std::vector<int> &k_indices)
const;
364 const BranchNode* node,
const OctreeKey& key,
365 unsigned int tree_depth, std::vector<int>& k_indices,
366 std::vector<float>& k_sqr_distances,
unsigned int max_nn)
const;
380 const OctreeKey& key,
unsigned int tree_depth,
381 const double squared_search_radius,
382 std::vector<prioPointQueueEntry>& point_candidates)
const;
394 unsigned int tree_depth,
int& result_index,
float& sqr_distance);
414 double max_z,
unsigned char a,
const OctreeNode* node,
415 const OctreeKey& key, AlignedPointTVector &voxel_center_list,
416 int max_voxel_count)
const;
428 boxSearchRecursive (
const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt,
const BranchNode* node,
429 const OctreeKey& key,
unsigned int tree_depth, std::vector<int>& k_indices)
const;
449 double max_x,
double max_y,
double max_z,
451 std::vector<int> &k_indices,
452 int max_voxel_count)
const;
467 double &min_x,
double &min_y,
double &min_z,
468 double &max_x,
double &max_y,
double &max_z,
469 unsigned char &a)
const
472 const float epsilon = 1e-10f;
473 if (direction.x () == 0.0)
474 direction.x () = epsilon;
475 if (direction.y () == 0.0)
476 direction.y () = epsilon;
477 if (direction.z () == 0.0)
478 direction.z () = epsilon;
484 if (direction.x () < 0.0)
486 origin.x () =
static_cast<float> (this->
min_x_) + static_cast<float> (this->
max_x_) - origin.x ();
487 direction.x () = -direction.x ();
490 if (direction.y () < 0.0)
492 origin.y () =
static_cast<float> (this->
min_y_) + static_cast<float> (this->
max_y_) - origin.y ();
493 direction.y () = -direction.y ();
496 if (direction.z () < 0.0)
498 origin.z () =
static_cast<float> (this->
min_z_) + static_cast<float> (this->
max_z_) - origin.z ();
499 direction.z () = -direction.z ();
502 min_x = (this->
min_x_ - origin.x ()) / direction.x ();
503 max_x = (this->
max_x_ - origin.x ()) / direction.x ();
504 min_y = (this->
min_y_ - origin.y ()) / direction.y ();
505 max_y = (this->
max_y_ - origin.y ()) / direction.y ();
506 min_z = (this->
min_z_ - origin.z ()) / direction.z ();
507 max_z = (this->
max_z_ - origin.z ()) / direction.z ();
603 #ifdef PCL_NO_PRECOMPILE
604 #include <pcl/octree/impl/octree_search.hpp>
607 #endif // PCL_OCTREE_SEARCH_H_
int getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices...
prioBranchQueueEntry(OctreeNode *_node, OctreeKey &_key, float _point_distance)
Constructor for initializing priority queue entry.
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
Priority queue entry for branch nodes
int getFirstIntersectedNode(double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const
Find first child node ray will enter.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius...
boost::shared_ptr< std::vector< int > > IndicesPtr
OctreeT::BranchNode BranchNode
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
This file defines compatibility wrappers for low level I/O functions.
int getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const
Get the next visited node given the current node upper bounding box corner.
int getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, const OctreeNode *node, const OctreeKey &key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers...
Priority queue entry for point candidates
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area...
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
virtual ~OctreePointCloudSearch()
Empty class destructor.
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, const double squared_search_radius, std::vector< prioPointQueueEntry > &point_candidates) const
Recursive search method that explores the octree and finds the K nearest neighbors.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
int point_idx_
Index representing a point in the dataset given by setInputCloud.
const OctreeNode * node
Pointer to octree node.
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor...
boost::shared_ptr< PointCloud > PointCloudPtr
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
prioPointQueueEntry()
Empty constructor.
OctreeT::LeafNode LeafNode
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
pcl::PointCloud< PointT > PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float point_distance
Distance to query point.
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
Octree pointcloud search class
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
OctreeT::LeafNode LeafNode
OctreeT::BranchNode BranchNode
OctreePointCloudSearch(const double resolution)
Constructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
float point_distance_
Distance to query point.
Abstract octree node class
prioBranchQueueEntry()
Empty constructor.
void initIntersectedVoxel(Eigen::Vector3f &origin, Eigen::Vector3f &direction, double &min_x, double &min_y, double &min_z, double &max_x, double &max_y, double &max_z, unsigned char &a) const
Initialize raytracing algorithm.
boost::shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr