39 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
45 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
47 std::vector<int>& point_idx_data)
49 assert (
isFinite (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
51 bool b_success =
false;
54 this->genOctreeKeyforPoint (point, key);
56 LeafContainerT* leaf = this->findLeaf (key);
60 (*leaf).getPointIndices (point_idx_data);
68 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
70 std::vector<int>& point_idx_data)
72 const PointT search_point = this->getPointByIndex (index);
73 return (this->voxelSearch (search_point, point_idx_data));
77 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
79 std::vector<int> &k_indices,
80 std::vector<float> &k_sqr_distances)
82 assert(this->leaf_count_>0);
83 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
86 k_sqr_distances.clear ();
91 prioPointQueueEntry point_entry;
92 std::vector<prioPointQueueEntry> point_candidates;
95 key.
x = key.
y = key.
z = 0;
98 double smallest_dist = std::numeric_limits<double>::max ();
100 getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
102 unsigned int result_count = static_cast<unsigned int> (point_candidates.size ());
104 k_indices.resize (result_count);
105 k_sqr_distances.resize (result_count);
107 for (
unsigned int i = 0; i < result_count; ++i)
109 k_indices [i] = point_candidates [i].point_idx_;
110 k_sqr_distances [i] = point_candidates [i].point_distance_;
113 return static_cast<int> (k_indices.size ());
117 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
119 std::vector<int> &k_indices,
120 std::vector<float> &k_sqr_distances)
122 const PointT search_point = this->getPointByIndex (index);
123 return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
127 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
132 assert(this->leaf_count_>0);
133 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
136 key.
x = key.
y = key.
z = 0;
138 approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
144 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
148 const PointT search_point = this->getPointByIndex (query_index);
150 return (approxNearestSearch (search_point, result_index, sqr_distance));
154 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
156 std::vector<int> &k_indices,
157 std::vector<float> &k_sqr_distances,
158 unsigned int max_nn)
const
160 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
162 key.
x = key.
y = key.
z = 0;
165 k_sqr_distances.clear ();
167 getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
170 return (static_cast<int> (k_indices.size ()));
174 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
176 std::vector<int> &k_indices,
177 std::vector<float> &k_sqr_distances,
178 unsigned int max_nn)
const
180 const PointT search_point = this->getPointByIndex (index);
182 return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
186 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
188 const Eigen::Vector3f &max_pt,
189 std::vector<int> &k_indices)
const
193 key.
x = key.
y = key.
z = 0;
197 boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
199 return (static_cast<int> (k_indices.size ()));
204 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
double
207 const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates)
const
209 std::vector<prioBranchQueueEntry> search_heap;
210 search_heap.resize (8);
214 double smallest_squared_dist = squared_search_radius;
217 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
220 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
222 if (this->branchHasChild (*node, child_idx))
226 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
227 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
228 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
231 this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
234 search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
235 search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
239 search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
243 std::sort (search_heap.begin (), search_heap.end ());
247 while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
248 smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
253 child_node = search_heap.back ().node;
254 new_key = search_heap.back ().key;
256 if (tree_depth < this->octree_depth_)
259 smallest_squared_dist = getKNearestNeighborRecursive (point,
K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
260 smallest_squared_dist, point_candidates);
265 std::vector<int> decoded_point_vector;
267 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
270 (*child_leaf)->getPointIndices (decoded_point_vector);
273 for (
const int &point_index : decoded_point_vector)
276 const PointT& candidate_point = this->getPointByIndex (point_index);
279 float squared_dist = pointSquaredDist (candidate_point, point);
282 if (squared_dist < smallest_squared_dist)
284 prioPointQueueEntry point_entry;
286 point_entry.point_distance_ = squared_dist;
287 point_entry.point_idx_ = point_index;
288 point_candidates.push_back (point_entry);
292 std::sort (point_candidates.begin (), point_candidates.end ());
294 if (point_candidates.size () >
K)
295 point_candidates.resize (
K);
297 if (point_candidates.size () ==
K)
298 smallest_squared_dist = point_candidates.back ().point_distance_;
301 search_heap.pop_back ();
304 return (smallest_squared_dist);
308 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
311 unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
312 unsigned int max_nn)
const
315 double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
318 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
320 if (!this->branchHasChild (*node, child_idx))
324 child_node = this->getBranchChildPtr (*node, child_idx);
331 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
332 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
333 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
336 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
339 squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
342 if (squared_dist + this->epsilon_
343 <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
346 if (tree_depth < this->octree_depth_)
349 getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
350 k_indices, k_sqr_distances, max_nn);
351 if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
357 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
358 std::vector<int> decoded_point_vector;
361 (*child_leaf)->getPointIndices (decoded_point_vector);
364 for (
const int &index : decoded_point_vector)
366 const PointT& candidate_point = this->getPointByIndex (index);
369 squared_dist = pointSquaredDist (candidate_point, point);
372 if (squared_dist > radiusSquared)
376 k_indices.push_back (index);
377 k_sqr_distances.push_back (squared_dist);
379 if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
388 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
392 unsigned int tree_depth,
402 double min_voxel_center_distance = std::numeric_limits<double>::max ();
404 unsigned char min_child_idx = 0xFF;
407 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
409 if (!this->branchHasChild (*node, child_idx))
413 double voxelPointDist;
415 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
416 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
417 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
420 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
422 voxelPointDist = pointSquaredDist (voxel_center, point);
425 if (voxelPointDist >= min_voxel_center_distance)
428 min_voxel_center_distance = voxelPointDist;
429 min_child_idx = child_idx;
430 minChildKey = new_key;
434 assert(min_child_idx<8);
436 child_node = this->getBranchChildPtr (*node, min_child_idx);
438 if (tree_depth < this->octree_depth_)
441 approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
447 std::vector<int> decoded_point_vector;
449 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
451 double smallest_squared_dist = std::numeric_limits<double>::max ();
454 (**child_leaf).getPointIndices (decoded_point_vector);
457 for (
const int &index : decoded_point_vector)
459 const PointT& candidate_point = this->getPointByIndex (index);
462 double squared_dist = pointSquaredDist (candidate_point, point);
465 if (squared_dist >= smallest_squared_dist)
468 result_index = index;
469 smallest_squared_dist = squared_dist;
470 sqr_distance = static_cast<float> (squared_dist);
476 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
float
478 const PointT & point_b)
const
480 return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
484 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
486 const Eigen::Vector3f &max_pt,
489 unsigned int tree_depth,
490 std::vector<int>& k_indices)
const
493 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
497 child_node = this->getBranchChildPtr (*node, child_idx);
504 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
505 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
506 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
509 Eigen::Vector3f lower_voxel_corner;
510 Eigen::Vector3f upper_voxel_corner;
512 this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
516 if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
517 (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
518 (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
521 if (tree_depth < this->octree_depth_)
524 boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
529 std::vector<int> decoded_point_vector;
532 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
535 (**child_leaf).getPointIndices (decoded_point_vector);
538 for (
const int &index : decoded_point_vector)
540 const PointT& candidate_point = this->getPointByIndex (index);
543 bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
544 (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
545 (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
549 k_indices.push_back (index);
557 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
560 int max_voxel_count)
const
563 key.
x = key.
y = key.
z = 0;
565 voxel_center_list.clear ();
570 double min_x, min_y, min_z, max_x, max_y, max_z;
572 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
574 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
575 return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
576 voxel_center_list, max_voxel_count);
582 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
584 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
585 int max_voxel_count)
const
588 key.
x = key.
y = key.
z = 0;
594 double min_x, min_y, min_z, max_x, max_y, max_z;
596 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
598 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
599 return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
600 k_indices, max_voxel_count);
605 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
607 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
610 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
618 this->genLeafNodeCenterFromOctreeKey (key, newPoint);
620 voxel_center_list.push_back (newPoint);
629 double mid_x = 0.5 * (min_x + max_x);
630 double mid_y = 0.5 * (min_y + max_y);
631 double mid_z = 0.5 * (min_z + max_z);
634 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
637 unsigned char child_idx;
644 child_idx = static_cast<unsigned char> (curr_node ^ a);
649 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
652 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
653 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
654 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
664 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
665 child_key, voxel_center_list, max_voxel_count);
666 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
671 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
672 child_key, voxel_center_list, max_voxel_count);
673 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
678 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
679 child_key, voxel_center_list, max_voxel_count);
680 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
685 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
686 child_key, voxel_center_list, max_voxel_count);
687 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
692 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
693 child_key, voxel_center_list, max_voxel_count);
694 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
699 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
700 child_key, voxel_center_list, max_voxel_count);
701 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
706 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
707 child_key, voxel_center_list, max_voxel_count);
708 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
713 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
714 child_key, voxel_center_list, max_voxel_count);
718 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
719 return (voxel_count);
723 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
725 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
726 const OctreeNode* node,
const OctreeKey& key, std::vector<int> &k_indices,
int max_voxel_count)
const
728 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
734 const LeafNode* leaf = static_cast<const LeafNode*> (node);
737 (*leaf)->getPointIndices (k_indices);
746 double mid_x = 0.5 * (min_x + max_x);
747 double mid_y = 0.5 * (min_y + max_y);
748 double mid_z = 0.5 * (min_z + max_z);
751 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
754 unsigned char child_idx;
760 child_idx = static_cast<unsigned char> (curr_node ^ a);
765 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
767 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
768 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
769 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
778 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
779 child_key, k_indices, max_voxel_count);
780 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
785 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
786 child_key, k_indices, max_voxel_count);
787 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
792 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
793 child_key, k_indices, max_voxel_count);
794 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
799 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
800 child_key, k_indices, max_voxel_count);
801 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
806 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
807 child_key, k_indices, max_voxel_count);
808 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
813 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
814 child_key, k_indices, max_voxel_count);
815 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
820 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
821 child_key, k_indices, max_voxel_count);
822 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
827 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
828 child_key, k_indices, max_voxel_count);
832 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
834 return (voxel_count);
837 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
839 #endif // PCL_OCTREE_SEARCH_IMPL_H_