39 #ifndef PCL_OCTREE_SEARCH_H_ 40 #define PCL_OCTREE_SEARCH_H_ 42 #include <pcl/point_cloud.h> 43 #include <pcl/point_types.h> 45 #include "octree_pointcloud.h" 57 template<
typename Po
intT,
typename LeafContainerT = OctreeContainerPo
intIndices ,
typename BranchContainerT = OctreeContainerEmpty >
70 typedef boost::shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >
Ptr;
71 typedef boost::shared_ptr<const OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT> >
ConstPtr;
108 voxelSearch (
const int index, std::vector<int>& point_idx_data);
120 nearestKSearch (
const PointCloud &cloud,
int index,
int k, std::vector<int> &k_indices,
121 std::vector<float> &k_sqr_distances)
123 return (
nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
135 std::vector<float> &k_sqr_distances);
147 nearestKSearch (
int index,
int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances);
190 radiusSearch (
const PointCloud &cloud,
int index,
double radius, std::vector<int> &k_indices,
191 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
193 return (
radiusSearch (cloud.
points[index], radius, k_indices, k_sqr_distances, max_nn));
206 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
const;
218 radiusSearch (
int index,
const double radius, std::vector<int> &k_indices,
219 std::vector<float> &k_sqr_distances,
unsigned int max_nn = 0)
const;
230 AlignedPointTVector &voxel_center_list,
int max_voxel_count = 0)
const;
241 std::vector<int> &k_indices,
242 int max_voxel_count = 0)
const;
252 boxSearch (
const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt, std::vector<int> &k_indices)
const;
311 point_idx_ (0), point_distance_ (0)
320 point_idx_ (point_idx), point_distance_ (point_distance)
365 unsigned int tree_depth, std::vector<int>& k_indices,
366 std::vector<float>& k_sqr_distances,
unsigned int max_nn)
const;
381 const double squared_search_radius,
382 std::vector<prioPointQueueEntry>& point_candidates)
const;
394 unsigned int tree_depth,
int& result_index,
float& sqr_distance);
415 const OctreeKey&
key, AlignedPointTVector &voxel_center_list,
416 int max_voxel_count)
const;
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> 606 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>; 607 #endif // PCL_NO_PRECOMPILE 609 #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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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
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 constructor.
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.
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.
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
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointT > PointCloud
float point_distance
Distance to query point.
Abstract octree leaf class
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.
OctreePointCloudSearch(const double resolution)
Constructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
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