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);
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);
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;
240 std::vector<int> &k_indices,
241 int max_voxel_count = 0)
const;
251 boxSearch (
const Eigen::Vector3f &min_pt,
const Eigen::Vector3f &max_pt, std::vector<int> &k_indices)
const;
364 unsigned int tree_depth, std::vector<int>& k_indices,
365 std::vector<float>& k_sqr_distances,
unsigned int max_nn)
const;
379 const OctreeKey& key,
unsigned int tree_depth,
380 const double squared_search_radius,
381 std::vector<prioPointQueueEntry>& point_candidates)
const;
393 unsigned int tree_depth,
int& result_index,
float& sqr_distance);
413 double max_z,
unsigned char a,
const OctreeNode* node,
415 int max_voxel_count)
const;
428 const OctreeKey& key,
unsigned int tree_depth, std::vector<int>& k_indices)
const;
448 double max_x,
double max_y,
double max_z,
450 std::vector<int> &k_indices,
451 int max_voxel_count)
const;
466 double &min_x,
double &min_y,
double &min_z,
467 double &max_x,
double &max_y,
double &max_z,
468 unsigned char &a)
const 471 const float epsilon = 1e-10f;
472 if (direction.x () == 0.0)
473 direction.x () = epsilon;
474 if (direction.y () == 0.0)
475 direction.y () = epsilon;
476 if (direction.z () == 0.0)
477 direction.z () = epsilon;
483 if (direction.x () < 0.0)
485 origin.x () =
static_cast<float> (this->
min_x_) + static_cast<float> (this->
max_x_) - origin.x ();
486 direction.x () = -direction.x ();
489 if (direction.y () < 0.0)
491 origin.y () =
static_cast<float> (this->
min_y_) + static_cast<float> (this->
max_y_) - origin.y ();
492 direction.y () = -direction.y ();
495 if (direction.z () < 0.0)
497 origin.z () =
static_cast<float> (this->
min_z_) + static_cast<float> (this->
max_z_) - origin.z ();
498 direction.z () = -direction.z ();
501 min_x = (this->
min_x_ - origin.x ()) / direction.x ();
502 max_x = (this->
max_x_ - origin.x ()) / direction.x ();
503 min_y = (this->
min_y_ - origin.y ()) / direction.y ();
504 max_y = (this->
max_y_ - origin.y ()) / direction.y ();
505 min_z = (this->
min_z_ - origin.z ()) / direction.z ();
506 max_z = (this->
max_z_ - origin.z ()) / direction.z ();
602 #ifdef PCL_NO_PRECOMPILE 603 #include <pcl/octree/impl/octree_search.hpp> 606 #endif // PCL_OCTREE_SEARCH_H_ 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...
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
OctreeT::BranchNode BranchNode
OctreePointCloud< PointT, LeafContainerT, BranchContainerT > OctreeT
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
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 approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
virtual ~OctreePointCloudSearch()
Empty class destructor.
prioPointQueueEntry(unsigned int &point_idx, float point_distance)
Constructor for initializing priority queue entry.
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.
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 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).
boost::shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
int point_idx_
Index representing a point in the dataset given by setInputCloud.
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.
const OctreeNode * node
Pointer to octree node.
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).
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
prioPointQueueEntry()
Empty constructor.
OctreeT::LeafNode LeafNode
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
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...
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.
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...
bool operator<(const prioBranchQueueEntry rhs) const
Operator< for comparing priority queue entries with each other.
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
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.
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 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()
Empty constructor.
boost::shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr