39 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
48 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
51 const PointT& point, std::vector<int>& point_idx_data)
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>
74 const int index, std::vector<int>& point_idx_data)
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>
85 std::vector<int>& k_indices,
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 unsigned int result_count =
static_cast<unsigned int>(point_candidates.size());
112 k_indices.resize(result_count);
113 k_sqr_distances.resize(result_count);
115 for (
unsigned int 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 static_cast<int>(k_indices.size());
123 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
126 int index,
int k, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances)
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>
135 const PointT& p_q,
int& result_index,
float& sqr_distance)
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>
153 int query_index,
int& result_index,
float& sqr_distance)
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>
165 std::vector<int>& k_indices,
166 std::vector<float>& k_sqr_distances,
167 unsigned int max_nn)
const
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 (
static_cast<int>(k_indices.size()));
189 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
194 std::vector<int>& k_indices,
195 std::vector<float>& k_sqr_distances,
196 unsigned int max_nn)
const
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,
208 std::vector<int>& k_indices)
const
212 key.
x = key.
y = key.
z = 0;
216 boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices);
218 return (
static_cast<int>(k_indices.size()));
221 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
229 unsigned int tree_depth,
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;
280 if (tree_depth < this->octree_depth_) {
282 smallest_squared_dist =
283 getKNearestNeighborRecursive(point,
288 smallest_squared_dist,
293 std::vector<int> decoded_point_vector;
298 (*child_leaf)->getPointIndices(decoded_point_vector);
301 for (
const int& 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,
340 unsigned int tree_depth,
341 std::vector<int>& k_indices,
342 std::vector<float>& k_sqr_distances,
343 unsigned int max_nn)
const
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)) {
376 if (tree_depth < this->octree_depth_) {
378 getNeighborsWithinRadiusRecursive(point,
386 if (max_nn != 0 && k_indices.size() ==
static_cast<unsigned int>(max_nn))
392 std::vector<int> decoded_point_vector;
395 (*child_leaf)->getPointIndices(decoded_point_vector);
398 for (
const int& 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() ==
static_cast<unsigned int>(max_nn))
420 template <
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
426 unsigned int tree_depth,
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);
471 if (tree_depth < this->octree_depth_) {
473 approxNearestSearchRecursive(point,
482 std::vector<int> decoded_point_vector;
486 double smallest_squared_dist = std::numeric_limits<double>::max();
489 (**child_leaf).getPointIndices(decoded_point_vector);
492 for (
const int& index : decoded_point_vector) {
493 const PointT& candidate_point = this->getPointByIndex(index);
496 double 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 =
static_cast<float>(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,
524 unsigned int tree_depth,
525 std::vector<int>& k_indices)
const
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)))) {
555 if (tree_depth < this->octree_depth_) {
557 boxSearchRecursive(min_pt,
566 std::vector<int> decoded_point_vector;
571 (**child_leaf).getPointIndices(decoded_point_vector);
574 for (
const int& 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,
598 int max_voxel_count)
const
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,
633 std::vector<int>& k_indices,
634 int max_voxel_count)
const
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>
675 int max_voxel_count)
const
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 int 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>
870 std::vector<int>& k_indices,
871 int max_voxel_count)
const
873 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
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 int 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>;
1057 #endif // PCL_OCTREE_SEARCH_IMPL_H_