39 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
45 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
51 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
53 bool b_success =
false;
56 this->genOctreeKeyforPoint(point, key);
58 LeafContainerT* leaf = this->findLeaf(key);
61 (*leaf).getPointIndices(point_idx_data);
69 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
72 voxelSearch(
const int index, std::vector<int>& point_idx_data)
74 const PointT search_point = this->getPointByIndex(index);
75 return (this->voxelSearch(search_point, point_idx_data));
79 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
84 std::vector<int>& k_indices,
85 std::vector<float>& k_sqr_distances)
87 assert(this->leaf_count_ > 0);
89 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
92 k_sqr_distances.clear();
97 prioPointQueueEntry point_entry;
98 std::vector<prioPointQueueEntry> point_candidates;
101 key.
x = key.
y = key.
z = 0;
104 double smallest_dist = std::numeric_limits<double>::max();
106 getKNearestNeighborRecursive(
107 p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
109 unsigned int result_count = static_cast<unsigned int>(point_candidates.size());
111 k_indices.resize(result_count);
112 k_sqr_distances.resize(result_count);
114 for (
unsigned int i = 0; i < result_count; ++i) {
115 k_indices[i] = point_candidates[i].point_idx_;
116 k_sqr_distances[i] = point_candidates[i].point_distance_;
119 return static_cast<int>(k_indices.size());
123 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
128 std::vector<int>& k_indices,
129 std::vector<float>& k_sqr_distances)
131 const PointT search_point = this->getPointByIndex(index);
132 return (nearestKSearch(search_point, k, k_indices, k_sqr_distances));
136 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
141 assert(this->leaf_count_ > 0);
143 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
146 key.
x = key.
y = key.
z = 0;
148 approxNearestSearchRecursive(
149 p_q, this->root_node_, key, 1, result_index, sqr_distance);
155 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
160 const PointT search_point = this->getPointByIndex(query_index);
162 return (approxNearestSearch(search_point, result_index, sqr_distance));
166 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
171 std::vector<int>& k_indices,
172 std::vector<float>& k_sqr_distances,
173 unsigned int max_nn)
const
176 "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
178 key.
x = key.
y = key.
z = 0;
181 k_sqr_distances.clear();
183 getNeighborsWithinRadiusRecursive(p_q,
192 return (static_cast<int>(k_indices.size()));
196 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
201 std::vector<int>& k_indices,
202 std::vector<float>& k_sqr_distances,
203 unsigned int max_nn)
const
205 const PointT search_point = this->getPointByIndex(index);
207 return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn));
211 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
215 const Eigen::Vector3f& max_pt,
216 std::vector<int>& k_indices)
const
220 key.
x = key.
y = key.
z = 0;
224 boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
226 return (static_cast<int>(k_indices.size()));
230 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
238 unsigned int tree_depth,
239 const double squared_search_radius,
240 std::vector<prioPointQueueEntry>& point_candidates)
const
242 std::vector<prioBranchQueueEntry> search_heap;
243 search_heap.resize(8);
247 double smallest_squared_dist = squared_search_radius;
250 double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth);
253 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
254 if (this->branchHasChild(*node, child_idx)) {
257 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
258 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
259 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
262 this->genVoxelCenterFromOctreeKey(
263 search_heap[child_idx].key, tree_depth, voxel_center);
266 search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx);
267 search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point);
270 search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity();
274 std::sort(search_heap.begin(), search_heap.end());
279 while ((!search_heap.empty()) &&
280 (search_heap.back().point_distance <
281 smallest_squared_dist + voxelSquaredDiameter / 4.0 +
282 sqrt(smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) {
286 child_node = search_heap.back().node;
287 new_key = search_heap.back().key;
289 if (tree_depth < this->octree_depth_) {
291 smallest_squared_dist =
292 getKNearestNeighborRecursive(point,
294 static_cast<const BranchNode*>(child_node),
297 smallest_squared_dist,
302 std::vector<int> decoded_point_vector;
304 const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
307 (*child_leaf)->getPointIndices(decoded_point_vector);
310 for (
const int& point_index : decoded_point_vector) {
312 const PointT& candidate_point = this->getPointByIndex(point_index);
315 float squared_dist = pointSquaredDist(candidate_point, point);
318 if (squared_dist < smallest_squared_dist) {
319 prioPointQueueEntry point_entry;
321 point_entry.point_distance_ = squared_dist;
322 point_entry.point_idx_ = point_index;
323 point_candidates.push_back(point_entry);
327 std::sort(point_candidates.begin(), point_candidates.end());
329 if (point_candidates.size() >
K)
330 point_candidates.resize(
K);
332 if (point_candidates.size() ==
K)
333 smallest_squared_dist = point_candidates.back().point_distance_;
336 search_heap.pop_back();
339 return (smallest_squared_dist);
343 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
347 const double radiusSquared,
350 unsigned int tree_depth,
351 std::vector<int>& k_indices,
352 std::vector<float>& k_sqr_distances,
353 unsigned int max_nn)
const
356 double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
359 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
360 if (!this->branchHasChild(*node, child_idx))
364 child_node = this->getBranchChildPtr(*node, child_idx);
371 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
372 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
373 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
376 this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
379 squared_dist = pointSquaredDist(static_cast<const PointT&>(voxel_center), point);
382 if (squared_dist + this->epsilon_ <=
383 voxel_squared_diameter / 4.0 + radiusSquared +
384 sqrt(voxel_squared_diameter * radiusSquared)) {
386 if (tree_depth < this->octree_depth_) {
390 static_cast<const BranchNode*>(child_node),
396 if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
401 const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
402 std::vector<int> decoded_point_vector;
405 (*child_leaf)->getPointIndices(decoded_point_vector);
408 for (
const int& index : decoded_point_vector) {
409 const PointT& candidate_point = this->getPointByIndex(index);
412 squared_dist = pointSquaredDist(candidate_point, point);
415 if (squared_dist > radiusSquared)
419 k_indices.push_back(index);
420 k_sqr_distances.push_back(squared_dist);
422 if (max_nn != 0 && k_indices.size() == static_cast<unsigned int>(max_nn))
431 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
437 unsigned int tree_depth,
447 double min_voxel_center_distance = std::numeric_limits<double>::max();
449 unsigned char min_child_idx = 0xFF;
452 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
453 if (!this->branchHasChild(*node, child_idx))
457 double voxelPointDist;
459 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
460 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
461 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
464 this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
466 voxelPointDist = pointSquaredDist(voxel_center, point);
469 if (voxelPointDist >= min_voxel_center_distance)
472 min_voxel_center_distance = voxelPointDist;
473 min_child_idx = child_idx;
474 minChildKey = new_key;
478 assert(min_child_idx < 8);
480 child_node = this->getBranchChildPtr(*node, min_child_idx);
482 if (tree_depth < this->octree_depth_) {
484 approxNearestSearchRecursive(point,
485 static_cast<const BranchNode*>(child_node),
493 std::vector<int> decoded_point_vector;
495 const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
497 double smallest_squared_dist = std::numeric_limits<double>::max();
500 (**child_leaf).getPointIndices(decoded_point_vector);
503 for (
const int& index : decoded_point_vector) {
504 const PointT& candidate_point = this->getPointByIndex(index);
507 double squared_dist = pointSquaredDist(candidate_point, point);
510 if (squared_dist >= smallest_squared_dist)
513 result_index = index;
514 smallest_squared_dist = squared_dist;
515 sqr_distance = static_cast<float>(squared_dist);
521 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
526 return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm();
530 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
534 const Eigen::Vector3f& max_pt,
537 unsigned int tree_depth,
538 std::vector<int>& k_indices)
const
541 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
544 child_node = this->getBranchChildPtr(*node, child_idx);
551 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
552 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
553 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
556 Eigen::Vector3f lower_voxel_corner;
557 Eigen::Vector3f upper_voxel_corner;
559 this->genVoxelBoundsFromOctreeKey(
560 new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
564 if (!((lower_voxel_corner(0) > max_pt(0)) || (min_pt(0) > upper_voxel_corner(0)) ||
565 (lower_voxel_corner(1) > max_pt(1)) || (min_pt(1) > upper_voxel_corner(1)) ||
566 (lower_voxel_corner(2) > max_pt(2)) || (min_pt(2) > upper_voxel_corner(2)))) {
568 if (tree_depth < this->octree_depth_) {
570 boxSearchRecursive(min_pt,
572 static_cast<const BranchNode*>(child_node),
579 std::vector<int> decoded_point_vector;
581 const LeafNode* child_leaf = static_cast<const LeafNode*>(child_node);
584 (**child_leaf).getPointIndices(decoded_point_vector);
587 for (
const int& index : decoded_point_vector) {
588 const PointT& candidate_point = this->getPointByIndex(index);
592 ((candidate_point.x >= min_pt(0)) && (candidate_point.x <= max_pt(0)) &&
593 (candidate_point.y >= min_pt(1)) && (candidate_point.y <= max_pt(1)) &&
594 (candidate_point.z >= min_pt(2)) && (candidate_point.z <= max_pt(2)));
598 k_indices.push_back(index);
606 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
610 Eigen::Vector3f direction,
612 int max_voxel_count)
const
615 key.
x = key.
y = key.
z = 0;
617 voxel_center_list.clear();
622 double min_x, min_y, min_z, max_x, max_y, max_z;
624 initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
626 if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
627 return getIntersectedVoxelCentersRecursive(min_x,
643 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
647 Eigen::Vector3f direction,
648 std::vector<int>& k_indices,
649 int max_voxel_count)
const
652 key.
x = key.
y = key.
z = 0;
658 double min_x, min_y, min_z, max_x, max_y, max_z;
660 initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
662 if (std::max(std::max(min_x, min_y), min_z) < std::min(std::min(max_x, max_y), max_z))
663 return getIntersectedVoxelIndicesRecursive(min_x,
678 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
691 int max_voxel_count)
const
693 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
700 this->genLeafNodeCenterFromOctreeKey(key, newPoint);
702 voxel_center_list.push_back(newPoint);
711 double mid_x = 0.5 * (min_x + max_x);
712 double mid_y = 0.5 * (min_y + max_y);
713 double mid_z = 0.5 * (min_z + max_z);
716 int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
719 unsigned char child_idx;
724 child_idx = static_cast<unsigned char>(curr_node ^ a);
730 this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
733 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
734 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
735 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
744 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
755 curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
760 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
771 curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
776 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
787 curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
792 voxel_count += getIntersectedVoxelCentersRecursive(min_x,
803 curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
808 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
819 curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
824 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
835 curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
840 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
851 curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
856 voxel_count += getIntersectedVoxelCentersRecursive(mid_x,
870 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
871 return (voxel_count);
875 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
887 std::vector<int>& k_indices,
888 int max_voxel_count)
const
890 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
895 const LeafNode* leaf = static_cast<const LeafNode*>(node);
898 (*leaf)->getPointIndices(k_indices);
907 double mid_x = 0.5 * (min_x + max_x);
908 double mid_y = 0.5 * (min_y + max_y);
909 double mid_z = 0.5 * (min_z + max_z);
912 int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z);
915 unsigned char child_idx;
919 child_idx = static_cast<unsigned char>(curr_node ^ a);
925 this->getBranchChildPtr(static_cast<const BranchNode&>(*node), child_idx);
927 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
928 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
929 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
937 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
948 curr_node = getNextIntersectedNode(mid_x, mid_y, mid_z, 4, 2, 1);
953 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
964 curr_node = getNextIntersectedNode(mid_x, mid_y, max_z, 5, 3, 8);
969 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
980 curr_node = getNextIntersectedNode(mid_x, max_y, mid_z, 6, 8, 3);
985 voxel_count += getIntersectedVoxelIndicesRecursive(min_x,
996 curr_node = getNextIntersectedNode(mid_x, max_y, max_z, 7, 8, 8);
1001 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1012 curr_node = getNextIntersectedNode(max_x, mid_y, mid_z, 8, 6, 5);
1017 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1028 curr_node = getNextIntersectedNode(max_x, mid_y, max_z, 8, 7, 8);
1033 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1044 curr_node = getNextIntersectedNode(max_x, max_y, mid_z, 8, 8, 7);
1049 voxel_count += getIntersectedVoxelIndicesRecursive(mid_x,
1063 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
1065 return (voxel_count);
1068 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) \
1069 template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>;
1071 #endif // PCL_OCTREE_SEARCH_IMPL_H_