41 #include <pcl/octree/octree_pointcloud.h>
42 #include <pcl/point_cloud.h>
55 typename LeafContainerT = OctreeContainerPointIndices,
56 typename BranchContainerT = OctreeContainerEmpty>
70 shared_ptr<OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>>;
103 voxelSearch(
const int index, std::vector<int>& point_idx_data);
119 std::vector<int>& k_indices,
120 std::vector<float>& k_sqr_distances)
122 return (
nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
137 std::vector<int>& k_indices,
138 std::vector<float>& k_sqr_distances);
154 std::vector<int>& k_indices,
155 std::vector<float>& k_sqr_distances);
206 std::vector<int>& k_indices,
207 std::vector<float>& k_sqr_distances,
208 unsigned int max_nn = 0)
226 std::vector<int>& k_indices,
227 std::vector<float>& k_sqr_distances,
228 unsigned int max_nn = 0)
const;
244 std::vector<int>& k_indices,
245 std::vector<float>& k_sqr_distances,
246 unsigned int max_nn = 0)
const;
259 Eigen::Vector3f direction,
261 int max_voxel_count = 0)
const;
273 Eigen::Vector3f direction,
274 std::vector<int>& k_indices,
275 int max_voxel_count = 0)
const;
286 const Eigen::Vector3f& max_pt,
287 std::vector<int>& k_indices)
const;
389 const double radiusSquared,
392 unsigned int tree_depth,
393 std::vector<int>& k_indices,
394 std::vector<float>& k_sqr_distances,
395 unsigned int max_nn)
const;
414 unsigned int tree_depth,
415 const double squared_search_radius,
416 std::vector<prioPointQueueEntry>& point_candidates)
const;
431 unsigned int tree_depth,
433 float& sqr_distance);
464 int max_voxel_count)
const;
477 const Eigen::Vector3f& max_pt,
480 unsigned int tree_depth,
481 std::vector<int>& k_indices)
const;
510 std::vector<int>& k_indices,
511 int max_voxel_count)
const;
526 Eigen::Vector3f& direction,
533 unsigned char& a)
const
536 const float epsilon = 1e-10f;
537 if (direction.x() == 0.0)
538 direction.x() = epsilon;
539 if (direction.y() == 0.0)
540 direction.y() = epsilon;
541 if (direction.z() == 0.0)
542 direction.z() = epsilon;
548 if (direction.x() < 0.0) {
549 origin.x() =
static_cast<float>(this->
min_x_) + static_cast<float>(this->
max_x_) -
551 direction.x() = -direction.x();
554 if (direction.y() < 0.0) {
555 origin.y() =
static_cast<float>(this->
min_y_) + static_cast<float>(this->
max_y_) -
557 direction.y() = -direction.y();
560 if (direction.z() < 0.0) {
561 origin.z() =
static_cast<float>(this->
min_z_) + static_cast<float>(this->
max_z_) -
563 direction.z() = -direction.z();
566 min_x = (this->
min_x_ - origin.x()) / direction.x();
567 max_x = (this->
max_x_ - origin.x()) / direction.x();
568 min_y = (this->
min_y_ - origin.y()) / direction.y();
569 max_y = (this->
max_y_ - origin.y()) / direction.y();
570 min_z = (this->
min_z_ - origin.z()) / direction.z();
571 max_z = (this->
max_z_ - origin.z()) / direction.z();
657 #ifdef PCL_NO_PRECOMPILE
658 #include <pcl/octree/impl/octree_search.hpp>
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.
shared_ptr< PointCloud< PointT > > Ptr
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.
typename OctreeBase< LeafContainerT, BranchContainerT >::BranchNode BranchNode
std::vector< PointT, Eigen::aligned_allocator< PointT >> AlignedPointTVector
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...
typename PointCloud::ConstPtr PointCloudConstPtr
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.
shared_ptr< const std::vector< int >> IndicesConstPtr
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.
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > >> ConstPtr
shared_ptr< std::vector< int >> IndicesPtr
shared_ptr< OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > >> Ptr
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
typename PointCloud::Ptr PointCloudPtr
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...
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.
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).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
std::vector< PointT, Eigen::aligned_allocator< PointT >> AlignedPointTVector
shared_ptr< const PointCloud< PointT > > ConstPtr
OctreePointCloudSearch(const double resolution)
Constructor.
typename OctreeBase< LeafContainerT, BranchContainerT >::LeafNode LeafNode
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.