39#ifndef PCL_OCTREE_SEARCH_IMPL_H_
40#define PCL_OCTREE_SEARCH_IMPL_H_
48template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
54 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
59 this->genOctreeKeyforPoint(point, key);
61 LeafContainerT*
leaf = this->findLeaf(key);
71template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
80template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
88 assert(this->leaf_count_ > 0);
90 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
102 key.
x = key.
y = key.
z = 0;
107 getKNearestNeighborRecursive(
123template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
132template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
137 assert(this->leaf_count_ > 0);
139 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
142 key.
x = key.
y = key.
z = 0;
144 approxNearestSearchRecursive(
150template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
160template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
170 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
172 key.
x = key.
y = key.
z = 0;
177 getNeighborsWithinRadiusRecursive(
p_q,
189template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
203template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
206 const Eigen::Vector3f&
min_pt,
207 const Eigen::Vector3f&
max_pt,
212 key.
x = key.
y = key.
z = 0;
221template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
245 if (this->branchHasChild(*node,
child_idx)) {
253 this->genVoxelCenterFromOctreeKey(
265 std::sort(search_heap.begin(), search_heap.end());
270 while ((!search_heap.empty()) &&
283 getKNearestNeighborRecursive(point,
318 std::sort(point_candidates.begin(), point_candidates.end());
320 if (point_candidates.size() >
K)
321 point_candidates.resize(
K);
323 if (point_candidates.size() ==
K)
324 smallest_squared_dist = point_candidates.back().point_distance_;
327 search_heap.pop_back();
330 return (smallest_squared_dist);
333template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
350 if (!this->branchHasChild(*node,
child_idx))
378 getNeighborsWithinRadiusRecursive(point,
420template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
442 if (!this->branchHasChild(*node,
child_idx))
473 approxNearestSearchRecursive(point,
509template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
514 return (
point_a.getVector3fMap() -
point_b.getVector3fMap()).squaredNorm();
517template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
520 const Eigen::Vector3f&
min_pt,
521 const Eigen::Vector3f&
max_pt,
546 this->genVoxelBoundsFromOctreeKey(
557 boxSearchRecursive(
min_pt,
592template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
596 Eigen::Vector3f direction,
601 key.
x = key.
y = key.
z = 0;
613 return getIntersectedVoxelCentersRecursive(
min_x,
628template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
632 Eigen::Vector3f direction,
637 key.
x = key.
y = key.
z = 0;
648 return getIntersectedVoxelIndicesRecursive(
min_x,
662template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
684 this->genLeafNodeCenterFromOctreeKey(key,
newPoint);
858template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
1054#define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
1055 template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
typename OctreeT::LeafNode LeafNode
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.
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).
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.
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).
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
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 ...
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.
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.
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.
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.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
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.
typename OctreeT::BranchNode BranchNode
void approxNearestSearch(const PointCloud &cloud, uindex_t query_index, index_t &result_index, float &sqr_distance)
Search for approx.
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.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.