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>>;
120 std::vector<float>& k_sqr_distances)
122 return (
nearestKSearch(cloud[index], k, k_indices, k_sqr_distances));
138 std::vector<float>& k_sqr_distances);
155 std::vector<float>& k_sqr_distances);
207 std::vector<float>& k_sqr_distances,
210 return (
radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
226 std::vector<float>& k_sqr_distances,
244 std::vector<float>& k_sqr_distances,
258 Eigen::Vector3f direction,
260 uindex_t max_voxel_count = 0)
const;
273 Eigen::Vector3f direction,
275 uindex_t max_voxel_count = 0)
const;
286 const Eigen::Vector3f& max_pt,
390 const double radiusSquared,
395 std::vector<float>& k_sqr_distances,
416 const double squared_search_radius,
417 std::vector<prioPointQueueEntry>& point_candidates)
const;
434 float& sqr_distance);
478 const Eigen::Vector3f& max_pt,
527 Eigen::Vector3f& direction,
534 unsigned char& a)
const
537 constexpr
float epsilon = 1e-10f;
538 if (direction.x() == 0.0)
539 direction.x() = epsilon;
540 if (direction.y() == 0.0)
541 direction.y() = epsilon;
542 if (direction.z() == 0.0)
543 direction.z() = epsilon;
549 if (direction.x() < 0.0) {
550 origin.x() =
static_cast<float>(this->
min_x_) + static_cast<float>(this->
max_x_) -
552 direction.x() = -direction.x();
555 if (direction.y() < 0.0) {
556 origin.y() =
static_cast<float>(this->
min_y_) + static_cast<float>(this->
max_y_) -
558 direction.y() = -direction.y();
561 if (direction.z() < 0.0) {
562 origin.z() =
static_cast<float>(this->
min_z_) + static_cast<float>(this->
max_z_) -
564 direction.z() = -direction.z();
567 min_x = (this->
min_x_ - origin.x()) / direction.x();
568 max_x = (this->
max_x_ - origin.x()) / direction.x();
569 min_y = (this->
min_y_ - origin.y()) / direction.y();
570 max_y = (this->
max_y_ - origin.y()) / direction.y();
571 min_z = (this->
min_z_ - origin.z()) / direction.z();
572 max_z = (this->
max_z_ - origin.z()) / direction.z();
658 #ifdef PCL_NO_PRECOMPILE
659 #include <pcl/octree/impl/octree_search.hpp>
void boxSearchRecursive(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &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.
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.
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
typename OctreeBase< LeafContainerT, BranchContainerT >::BranchNode BranchNode
std::vector< PointT, Eigen::aligned_allocator< PointT >> AlignedPointTVector
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.
Priority queue entry for point candidates
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
uindex_t 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, Indices &k_indices, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices...
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > >> ConstPtr
shared_ptr< Indices > 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.
uindex_t getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, Indices &k_indices, uindex_t max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
typename PointCloud::Ptr PointCloudPtr
bool operator<(const prioPointQueueEntry &rhs) const
Operator< for comparing priority queue entries with each other.
const OctreeNode * node
Pointer to octree node.
uindex_t getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, uindex_t max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
uindex_t nearestKSearch(const PointCloud &cloud, uindex_t index, uindex_t k, Indices &k_indices, std::vector< float > &k_sqr_distances)
Search for k-nearest neighbors at the query point.
IndicesAllocator<> Indices
Type used for indices in PCL.
prioPointQueueEntry()
Empty constructor.
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.
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, index_t &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor...
prioPointQueueEntry(uindex_t point_idx, float point_distance)
Constructor for initializing priority queue entry.
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, uindex_t tree_depth, Indices &k_indices, std::vector< float > &k_sqr_distances, uindex_t max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius...
Octree pointcloud search class
std::vector< PointT, Eigen::aligned_allocator< PointT >> AlignedPointTVector
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< const Indices > IndicesConstPtr
OctreePointCloudSearch(const double resolution)
Constructor.
typename OctreeBase< LeafContainerT, BranchContainerT >::LeafNode LeafNode
uindex_t 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, uindex_t max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers...
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
float point_distance_
Distance to query point.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
uindex_t radiusSearch(const PointCloud &cloud, uindex_t index, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, index_t max_nn=0)
Search for all neighbors of query point that are within a given radius.
uindex_t boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, Indices &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
Abstract octree node class
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
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.
uindex_t point_idx_
Index representing a point in the dataset given by setInputCloud.
double getKNearestNeighborRecursive(const PointT &point, uindex_t K, const BranchNode *node, const OctreeKey &key, uindex_t 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.