39 #ifndef PCL_OCTREE_SEARCH_IMPL_H_
40 #define PCL_OCTREE_SEARCH_IMPL_H_
42 #include <pcl/point_cloud.h>
43 #include <pcl/point_types.h>
45 #include <pcl/common/common.h>
51 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
53 std::vector<int>& point_idx_data)
55 assert (
isFinite (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
57 bool b_success =
false;
60 this->genOctreeKeyforPoint (point, key);
62 LeafContainerT* leaf = this->findLeaf (key);
66 (*leaf).getPointIndices (point_idx_data);
74 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool
76 std::vector<int>& point_idx_data)
78 const PointT search_point = this->getPointByIndex (index);
79 return (this->voxelSearch (search_point, point_idx_data));
83 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
85 std::vector<int> &k_indices,
86 std::vector<float> &k_sqr_distances)
88 assert(this->leaf_count_>0);
89 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
92 k_sqr_distances.clear ();
98 unsigned int result_count;
101 std::vector<prioPointQueueEntry> point_candidates;
104 key.
x = key.
y = key.
z = 0;
107 double smallest_dist = numeric_limits<double>::max ();
109 getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
111 result_count =
static_cast<unsigned int> (point_candidates.size ());
113 k_indices.resize (result_count);
114 k_sqr_distances.resize (result_count);
116 for (i = 0; i < result_count; ++i)
118 k_indices [i] = point_candidates [i].point_idx_;
119 k_sqr_distances [i] = point_candidates [i].point_distance_;
122 return static_cast<int> (k_indices.size ());
126 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
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>
void
141 assert(this->leaf_count_>0);
142 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
145 key.
x = key.
y = key.
z = 0;
147 approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
153 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
157 const PointT search_point = this->getPointByIndex (query_index);
159 return (approxNearestSearch (search_point, result_index, sqr_distance));
163 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
165 std::vector<int> &k_indices,
166 std::vector<float> &k_sqr_distances,
167 unsigned int max_nn)
const
169 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
171 key.
x = key.
y = key.
z = 0;
174 k_sqr_distances.clear ();
176 getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
179 return (static_cast<int> (k_indices.size ()));
183 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
185 std::vector<int> &k_indices,
186 std::vector<float> &k_sqr_distances,
187 unsigned int max_nn)
const
189 const PointT search_point = this->getPointByIndex (index);
191 return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
195 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
197 const Eigen::Vector3f &max_pt,
198 std::vector<int> &k_indices)
const
202 key.
x = key.
y = key.
z = 0;
206 boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
208 return (static_cast<int> (k_indices.size ()));
213 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
double
216 const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates)
const
218 std::vector<prioBranchQueueEntry> search_heap;
219 search_heap.resize (8);
221 unsigned char child_idx;
225 double smallest_squared_dist = squared_search_radius;
228 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
231 for (child_idx = 0; child_idx < 8; child_idx++)
233 if (this->branchHasChild (*node, child_idx))
237 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
238 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
239 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
242 this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
245 search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
246 search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
250 search_heap[child_idx].point_distance = numeric_limits<float>::infinity ();
254 std::sort (search_heap.begin (), search_heap.end ());
258 while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
259 smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
264 child_node = search_heap.back ().node;
265 new_key = search_heap.back ().key;
267 if (tree_depth < this->octree_depth_)
270 smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
271 smallest_squared_dist, point_candidates);
279 vector<int> decoded_point_vector;
284 (*child_leaf)->getPointIndices (decoded_point_vector);
287 for (i = 0; i < decoded_point_vector.size (); i++)
290 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
293 squared_dist = pointSquaredDist (candidate_point, point);
296 if (squared_dist < smallest_squared_dist)
301 point_entry.
point_idx_ = decoded_point_vector[i];
302 point_candidates.push_back (point_entry);
306 std::sort (point_candidates.begin (), point_candidates.end ());
308 if (point_candidates.size () >
K)
309 point_candidates.resize (K);
311 if (point_candidates.size () ==
K)
312 smallest_squared_dist = point_candidates.back ().point_distance_;
315 search_heap.pop_back ();
318 return (smallest_squared_dist);
322 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
325 unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
326 unsigned int max_nn)
const
329 unsigned char child_idx;
332 double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
335 for (child_idx = 0; child_idx < 8; child_idx++)
337 if (!this->branchHasChild (*node, child_idx))
341 child_node = this->getBranchChildPtr (*node, child_idx);
348 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
349 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
350 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
353 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
356 squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
359 if (squared_dist + this->epsilon_
360 <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
363 if (tree_depth < this->octree_depth_)
367 k_indices, k_sqr_distances, max_nn);
368 if (max_nn != 0 && k_indices.size () ==
static_cast<unsigned int> (max_nn))
377 vector<int> decoded_point_vector;
380 (*child_leaf)->getPointIndices (decoded_point_vector);
383 for (i = 0; i < decoded_point_vector.size (); i++)
385 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
388 squared_dist = pointSquaredDist (candidate_point, point);
391 if (squared_dist > radiusSquared)
395 k_indices.push_back (decoded_point_vector[i]);
396 k_sqr_distances.push_back (squared_dist);
398 if (max_nn != 0 && k_indices.size () ==
static_cast<unsigned int> (max_nn))
407 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
411 unsigned int tree_depth,
415 unsigned char child_idx;
416 unsigned char min_child_idx;
417 double min_voxel_center_distance;
425 min_voxel_center_distance = numeric_limits<double>::max ();
427 min_child_idx = 0xFF;
430 for (child_idx = 0; child_idx < 8; child_idx++)
432 if (!this->branchHasChild (*node, child_idx))
436 double voxelPointDist;
438 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
439 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
440 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
443 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
445 voxelPointDist = pointSquaredDist (voxel_center, point);
448 if (voxelPointDist >= min_voxel_center_distance)
451 min_voxel_center_distance = voxelPointDist;
452 min_child_idx = child_idx;
453 minChildKey = new_key;
457 assert(min_child_idx<8);
459 child_node = this->getBranchChildPtr (*node, min_child_idx);
461 if (tree_depth < this->octree_depth_)
464 approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
472 double smallest_squared_dist;
474 vector<int> decoded_point_vector;
478 smallest_squared_dist = numeric_limits<double>::max ();
481 (**child_leaf).getPointIndices (decoded_point_vector);
484 for (i = 0; i < decoded_point_vector.size (); i++)
486 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
489 squared_dist = pointSquaredDist (candidate_point, point);
492 if (squared_dist >= smallest_squared_dist)
495 result_index = decoded_point_vector[i];
496 smallest_squared_dist = squared_dist;
497 sqr_distance =
static_cast<float> (squared_dist);
503 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
float
505 const PointT & point_b)
const
507 return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
511 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void
513 const Eigen::Vector3f &max_pt,
516 unsigned int tree_depth,
517 std::vector<int>& k_indices)
const
520 unsigned char child_idx;
523 for (child_idx = 0; child_idx < 8; child_idx++)
527 child_node = this->getBranchChildPtr (*node, child_idx);
534 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
535 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
536 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
539 Eigen::Vector3f lower_voxel_corner;
540 Eigen::Vector3f upper_voxel_corner;
542 this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
546 if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
547 (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
548 (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
551 if (tree_depth < this->octree_depth_)
554 boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
560 vector<int> decoded_point_vector;
566 (**child_leaf).getPointIndices (decoded_point_vector);
569 for (i = 0; i < decoded_point_vector.size (); i++)
571 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
574 bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
575 (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
576 (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
580 k_indices.push_back (decoded_point_vector[i]);
588 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
591 int max_voxel_count)
const
594 key.
x = key.
y = key.
z = 0;
596 voxel_center_list.clear ();
601 double min_x, min_y, min_z, max_x, max_y, max_z;
603 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
605 if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
606 return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
607 voxel_center_list, max_voxel_count);
613 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
615 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
616 int max_voxel_count)
const
619 key.
x = key.
y = key.
z = 0;
625 double min_x, min_y, min_z, max_x, max_y, max_z;
627 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
629 if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z))
630 return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
631 k_indices, max_voxel_count);
636 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
638 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
641 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
649 this->genLeafNodeCenterFromOctreeKey (key, newPoint);
651 voxel_center_list.push_back (newPoint);
660 double mid_x = 0.5 * (min_x + max_x);
661 double mid_y = 0.5 * (min_y + max_y);
662 double mid_z = 0.5 * (min_z + max_z);
665 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
668 unsigned char child_idx;
675 child_idx =
static_cast<unsigned char> (curr_node ^ a);
680 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
683 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
684 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
685 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
695 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
696 child_key, voxel_center_list, max_voxel_count);
697 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
702 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
703 child_key, voxel_center_list, max_voxel_count);
704 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
709 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
710 child_key, voxel_center_list, max_voxel_count);
711 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
716 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
717 child_key, voxel_center_list, max_voxel_count);
718 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
723 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
724 child_key, voxel_center_list, max_voxel_count);
725 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
730 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
731 child_key, voxel_center_list, max_voxel_count);
732 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
737 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
738 child_key, voxel_center_list, max_voxel_count);
739 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
744 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
745 child_key, voxel_center_list, max_voxel_count);
749 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
750 return (voxel_count);
754 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int
756 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
757 const OctreeNode* node,
const OctreeKey& key, std::vector<int> &k_indices,
int max_voxel_count)
const
759 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
768 (*leaf)->getPointIndices (k_indices);
777 double mid_x = 0.5 * (min_x + max_x);
778 double mid_y = 0.5 * (min_y + max_y);
779 double mid_z = 0.5 * (min_z + max_z);
782 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
785 unsigned char child_idx;
791 child_idx =
static_cast<unsigned char> (curr_node ^ a);
796 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
798 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
799 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
800 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
809 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
810 child_key, k_indices, max_voxel_count);
811 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
816 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
817 child_key, k_indices, max_voxel_count);
818 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
823 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
824 child_key, k_indices, max_voxel_count);
825 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
830 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
831 child_key, k_indices, max_voxel_count);
832 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
837 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
838 child_key, k_indices, max_voxel_count);
839 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
844 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
845 child_key, k_indices, max_voxel_count);
846 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
851 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
852 child_key, k_indices, max_voxel_count);
853 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
858 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
859 child_key, k_indices, max_voxel_count);
863 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
865 return (voxel_count);
868 #endif // PCL_OCTREE_SEARCH_IMPL_H_