39 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
48 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
54 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
56 bool b_success =
false;
59 this->genOctreeKeyforPoint(point, key);
61 LeafContainerT* leaf = this->findLeaf(key);
64 (*leaf).getPointIndices(point_idx_data);
71 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
76 const PointT search_point = this->getPointByIndex(index);
77 return (this->voxelSearch(search_point, point_idx_data));
80 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
86 std::vector<float>& k_sqr_distances)
88 assert(this->leaf_count_ > 0);
90 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
93 k_sqr_distances.clear();
98 prioPointQueueEntry point_entry;
99 std::vector<prioPointQueueEntry> point_candidates;
102 key.
x = key.
y = key.
z = 0;
105 double smallest_dist = std::numeric_limits<double>::max();
107 getKNearestNeighborRecursive(
108 p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
110 const auto result_count =
static_cast<uindex_t>(point_candidates.size());
112 k_indices.resize(result_count);
113 k_sqr_distances.resize(result_count);
115 for (
uindex_t i = 0; i < result_count; ++i) {
116 k_indices[i] = point_candidates[i].point_idx_;
117 k_sqr_distances[i] = point_candidates[i].point_distance_;
120 return k_indices.size();
123 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
128 const PointT search_point = this->getPointByIndex(index);
129 return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
132 template <
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(
145 p_q, this->root_node_, key, 1, result_index, sqr_distance);
150 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
155 const PointT search_point = this->getPointByIndex(query_index);
157 return (approxNearestSearch(search_point, result_index, sqr_distance));
160 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
166 std::vector<float>& k_sqr_distances,
170 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
172 key.
x = key.
y = key.
z = 0;
175 k_sqr_distances.clear();
177 getNeighborsWithinRadiusRecursive(p_q,
186 return k_indices.size();
189 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
195 std::vector<float>& k_sqr_distances,
198 const PointT search_point = this->getPointByIndex(index);
200 return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
203 template <
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;
216 boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
218 return k_indices.size();
221 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
230 const double squared_search_radius,
231 std::vector<prioPointQueueEntry>& point_candidates)
const
233 std::vector<prioBranchQueueEntry> search_heap;
234 search_heap.resize(8);
238 double smallest_squared_dist = squared_search_radius;
241 double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
244 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
245 if (this->branchHasChild(*node, child_idx)) {
248 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
249 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
250 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
253 this->genVoxelCenterFromOctreeKey(
254 search_heap[child_idx].key, tree_depth, voxel_center);
257 search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
258 search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
261 search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
265 std::sort(search_heap.begin(), search_heap.end());
270 while ((!search_heap.empty()) &&
271 (search_heap.back().point_distance <
272 smallest_squared_dist + voxelSquaredDiameter / 4.0 +
273 sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
277 child_node = search_heap.back().node;
278 new_key = search_heap.back().key;
282 smallest_squared_dist =
283 getKNearestNeighborRecursive(point,
288 smallest_squared_dist,
295 const auto* child_leaf =
static_cast<const LeafNode*
>(child_node);
298 (*child_leaf)->getPointIndices(decoded_point_vector);
301 for (
const auto& point_index : decoded_point_vector) {
303 const PointT& candidate_point = this->getPointByIndex(point_index);
306 float squared_dist = pointSquaredDist(candidate_point, point);
309 if (squared_dist < smallest_squared_dist) {
310 prioPointQueueEntry point_entry;
312 point_entry.point_distance_ = squared_dist;
313 point_entry.point_idx_ = point_index;
314 point_candidates.push_back(point_entry);
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);
333 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
337 const double radiusSquared,
342 std::vector<float>& k_sqr_distances,
346 double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
349 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
350 if (!this->branchHasChild(*node, child_idx))
354 child_node = this->getBranchChildPtr(*node, child_idx);
361 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
362 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
363 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
366 this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
369 squared_dist = pointSquaredDist(
static_cast<const PointT&
>(voxel_center), point);
372 if (squared_dist + this->epsilon_ <=
373 voxel_squared_diameter / 4.0 + radiusSquared +
374 sqrt(voxel_squared_diameter * radiusSquared)) {
378 getNeighborsWithinRadiusRecursive(point,
386 if (max_nn != 0 && k_indices.size() == max_nn)
391 const auto* child_leaf =
static_cast<const LeafNode*
>(child_node);
395 (*child_leaf)->getPointIndices(decoded_point_vector);
398 for (
const auto& index : decoded_point_vector) {
399 const PointT& candidate_point = this->getPointByIndex(index);
402 squared_dist = pointSquaredDist(candidate_point, point);
405 if (squared_dist > radiusSquared)
409 k_indices.push_back(index);
410 k_sqr_distances.push_back(squared_dist);
412 if (max_nn != 0 && k_indices.size() == max_nn)
420 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
436 double min_voxel_center_distance = std::numeric_limits<double>::max();
438 unsigned char min_child_idx = 0xFF;
441 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
442 if (!this->branchHasChild(*node, child_idx))
446 double voxelPointDist;
448 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
449 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
450 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
453 this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
455 voxelPointDist = pointSquaredDist(voxel_center, point);
458 if (voxelPointDist >= min_voxel_center_distance)
461 min_voxel_center_distance = voxelPointDist;
462 min_child_idx = child_idx;
463 minChildKey = new_key;
467 assert(min_child_idx < 8);
469 child_node = this->getBranchChildPtr(*node, min_child_idx);
473 approxNearestSearchRecursive(point,
474 static_cast<const BranchNode*
>(child_node),
484 const auto* child_leaf =
static_cast<const LeafNode*
>(child_node);
486 float smallest_squared_dist = std::numeric_limits<float>::max();
489 (**child_leaf).getPointIndices(decoded_point_vector);
492 for (
const auto& index : decoded_point_vector) {
493 const PointT& candidate_point = this->getPointByIndex(index);
496 float squared_dist = pointSquaredDist(candidate_point, point);
499 if (squared_dist >= smallest_squared_dist)
502 result_index = index;
503 smallest_squared_dist = squared_dist;
504 sqr_distance = squared_dist;
509 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
514 return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
517 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
520 const Eigen::Vector3f& min_pt,
521 const Eigen::Vector3f& max_pt,
528 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
531 child_node = this->getBranchChildPtr(*node, child_idx);
538 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
539 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
540 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
543 Eigen::Vector3f lower_voxel_corner;
544 Eigen::Vector3f upper_voxel_corner;
546 this->genVoxelBoundsFromOctreeKey(
547 new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
551 if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) ||
552 (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
553 (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
557 boxSearchRecursive(min_pt,
568 const auto* child_leaf =
static_cast<const LeafNode*
>(child_node);
571 (**child_leaf).getPointIndices(decoded_point_vector);
574 for (
const auto& index : decoded_point_vector) {
575 const PointT& candidate_point = this->getPointByIndex(index);
579 ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
580 (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
581 (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
585 k_indices.push_back(index);
592 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
596 Eigen::Vector3f direction,
601 key.
x = key.
y = key.
z = 0;
603 voxel_center_list.clear();
608 double min_x, min_y, min_z, max_x, max_y, max_z;
610 initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
612 if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
613 return getIntersectedVoxelCentersRecursive(min_x,
628 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
632 Eigen::Vector3f direction,
637 key.
x = key.
y = key.
z = 0;
643 double min_x, min_y, min_z, max_x, max_y, max_z;
645 initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
647 if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
648 return getIntersectedVoxelIndicesRecursive(min_x,
662 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
677 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
684 this->genLeafNodeCenterFromOctreeKey(key, newPoint);
686 voxel_center_list.push_back(newPoint);
695 double mid_x = 0.5 * (min_x + max_x);
696 double mid_y = 0.5 * (min_y + max_y);
697 double mid_z = 0.5 * (min_z + max_z);
700 auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
703 unsigned char child_idx;
708 child_idx =
static_cast<unsigned char>(curr_node ^ a);
714 this->getBranchChildPtr(
static_cast<const BranchNode&
>(*node), child_idx);
717 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
718 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
719 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
728 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
739 curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
744 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
755 curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
760 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
771 curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
776 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
787 curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
792 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
803 curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
808 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
819 curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
824 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
835 curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
840 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
854 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
855 return (voxel_count);
858 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
873 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
878 const auto* leaf =
static_cast<const LeafNode*
>(node);
881 (*leaf)->getPointIndices(k_indices);
890 double mid_x = 0.5 * (min_x + max_x);
891 double mid_y = 0.5 * (min_y + max_y);
892 double mid_z = 0.5 * (min_z + max_z);
895 auto curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
898 unsigned char child_idx;
902 child_idx =
static_cast<unsigned char>(curr_node ^ a);
908 this->getBranchChildPtr(
static_cast<const BranchNode&
>(*node), child_idx);
910 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
911 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
912 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
920 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
931 curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
936 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
947 curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
952 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
963 curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
968 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
979 curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
984 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
995 curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
1000 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1011 curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
1016 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1027 curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
1032 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1046 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
1048 return (voxel_count);
1054 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
1055 template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
Abstract octree branch class
Abstract octree leaf class
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving 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.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.