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> 50 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool 52 std::vector<int>& point_idx_data)
54 assert (
isFinite (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
56 bool b_success =
false;
59 this->genOctreeKeyforPoint (point, key);
61 LeafContainerT* leaf = this->findLeaf (key);
65 (*leaf).getPointIndices (point_idx_data);
73 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
bool 75 std::vector<int>& point_idx_data)
77 const PointT search_point = this->getPointByIndex (index);
78 return (this->voxelSearch (search_point, point_idx_data));
82 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 84 std::vector<int> &k_indices,
85 std::vector<float> &k_sqr_distances)
87 assert(this->leaf_count_>0);
88 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
91 k_sqr_distances.clear ();
97 unsigned int result_count;
100 std::vector<prioPointQueueEntry> point_candidates;
103 key.
x = key.
y = key.
z = 0;
106 double smallest_dist = std::numeric_limits<double>::max ();
108 getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
110 result_count =
static_cast<unsigned int> (point_candidates.size ());
112 k_indices.resize (result_count);
113 k_sqr_distances.resize (result_count);
115 for (i = 0; i < result_count; ++i)
117 k_indices [i] = point_candidates [i].point_idx_;
118 k_sqr_distances [i] = point_candidates [i].point_distance_;
121 return static_cast<int> (k_indices.size ());
125 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 127 std::vector<int> &k_indices,
128 std::vector<float> &k_sqr_distances)
130 const PointT search_point = this->getPointByIndex (index);
131 return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
135 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 140 assert(this->leaf_count_>0);
141 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
144 key.
x = key.
y = key.
z = 0;
146 approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
152 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 156 const PointT search_point = this->getPointByIndex (query_index);
158 return (approxNearestSearch (search_point, result_index, sqr_distance));
162 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 164 std::vector<int> &k_indices,
165 std::vector<float> &k_sqr_distances,
166 unsigned int max_nn)
const 168 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
170 key.
x = key.
y = key.
z = 0;
173 k_sqr_distances.clear ();
175 getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
178 return (static_cast<int> (k_indices.size ()));
182 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 184 std::vector<int> &k_indices,
185 std::vector<float> &k_sqr_distances,
186 unsigned int max_nn)
const 188 const PointT search_point = this->getPointByIndex (index);
190 return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
194 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 196 const Eigen::Vector3f &max_pt,
197 std::vector<int> &k_indices)
const 201 key.
x = key.
y = key.
z = 0;
205 boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
207 return (static_cast<int> (k_indices.size ()));
212 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
double 215 const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates)
const 217 std::vector<prioBranchQueueEntry> search_heap;
218 search_heap.resize (8);
220 unsigned char child_idx;
224 double smallest_squared_dist = squared_search_radius;
227 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
230 for (child_idx = 0; child_idx < 8; child_idx++)
232 if (this->branchHasChild (*node, child_idx))
236 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
237 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
238 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
241 this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
244 search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
245 search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
249 search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
253 std::sort (search_heap.begin (), search_heap.end ());
257 while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
258 smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
263 child_node = search_heap.back ().node;
264 new_key = search_heap.back ().key;
266 if (tree_depth < this->octree_depth_)
269 smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
270 smallest_squared_dist, point_candidates);
278 std::vector<int> decoded_point_vector;
283 (*child_leaf)->getPointIndices (decoded_point_vector);
286 for (i = 0; i < decoded_point_vector.size (); i++)
289 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
292 squared_dist = pointSquaredDist (candidate_point, point);
295 if (squared_dist < smallest_squared_dist)
300 point_entry.
point_idx_ = decoded_point_vector[i];
301 point_candidates.push_back (point_entry);
305 std::sort (point_candidates.begin (), point_candidates.end ());
307 if (point_candidates.size () >
K)
308 point_candidates.resize (K);
310 if (point_candidates.size () ==
K)
311 smallest_squared_dist = point_candidates.back ().point_distance_;
314 search_heap.pop_back ();
317 return (smallest_squared_dist);
321 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 324 unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
325 unsigned int max_nn)
const 328 unsigned char child_idx;
331 double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
334 for (child_idx = 0; child_idx < 8; child_idx++)
336 if (!this->branchHasChild (*node, child_idx))
340 child_node = this->getBranchChildPtr (*node, child_idx);
347 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
348 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
349 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
352 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
355 squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
358 if (squared_dist + this->epsilon_
359 <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
362 if (tree_depth < this->octree_depth_)
365 getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
366 k_indices, k_sqr_distances, max_nn);
367 if (max_nn != 0 && k_indices.size () ==
static_cast<unsigned int> (max_nn))
376 std::vector<int> decoded_point_vector;
379 (*child_leaf)->getPointIndices (decoded_point_vector);
382 for (i = 0; i < decoded_point_vector.size (); i++)
384 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
387 squared_dist = pointSquaredDist (candidate_point, point);
390 if (squared_dist > radiusSquared)
394 k_indices.push_back (decoded_point_vector[i]);
395 k_sqr_distances.push_back (squared_dist);
397 if (max_nn != 0 && k_indices.size () ==
static_cast<unsigned int> (max_nn))
406 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 410 unsigned int tree_depth,
414 unsigned char child_idx;
415 unsigned char min_child_idx;
416 double min_voxel_center_distance;
424 min_voxel_center_distance = std::numeric_limits<double>::max ();
426 min_child_idx = 0xFF;
429 for (child_idx = 0; child_idx < 8; child_idx++)
431 if (!this->branchHasChild (*node, child_idx))
435 double voxelPointDist;
437 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
438 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
439 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
442 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
444 voxelPointDist = pointSquaredDist (voxel_center, point);
447 if (voxelPointDist >= min_voxel_center_distance)
450 min_voxel_center_distance = voxelPointDist;
451 min_child_idx = child_idx;
452 minChildKey = new_key;
456 assert(min_child_idx<8);
458 child_node = this->getBranchChildPtr (*node, min_child_idx);
460 if (tree_depth < this->octree_depth_)
463 approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
471 double smallest_squared_dist;
473 std::vector<int> decoded_point_vector;
477 smallest_squared_dist = std::numeric_limits<double>::max ();
480 (**child_leaf).getPointIndices (decoded_point_vector);
483 for (i = 0; i < decoded_point_vector.size (); i++)
485 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
488 squared_dist = pointSquaredDist (candidate_point, point);
491 if (squared_dist >= smallest_squared_dist)
494 result_index = decoded_point_vector[i];
495 smallest_squared_dist = squared_dist;
496 sqr_distance =
static_cast<float> (squared_dist);
502 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
float 504 const PointT & point_b)
const 506 return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
510 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 512 const Eigen::Vector3f &max_pt,
515 unsigned int tree_depth,
516 std::vector<int>& k_indices)
const 519 unsigned char child_idx;
522 for (child_idx = 0; child_idx < 8; child_idx++)
526 child_node = this->getBranchChildPtr (*node, child_idx);
533 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
534 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
535 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
538 Eigen::Vector3f lower_voxel_corner;
539 Eigen::Vector3f upper_voxel_corner;
541 this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
545 if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
546 (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
547 (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
550 if (tree_depth < this->octree_depth_)
553 boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
559 std::vector<int> decoded_point_vector;
565 (**child_leaf).getPointIndices (decoded_point_vector);
568 for (i = 0; i < decoded_point_vector.size (); i++)
570 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
573 bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
574 (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
575 (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
579 k_indices.push_back (decoded_point_vector[i]);
587 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 590 int max_voxel_count)
const 593 key.
x = key.
y = key.
z = 0;
595 voxel_center_list.clear ();
600 double min_x, min_y, min_z, max_x, max_y, max_z;
602 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
604 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
605 return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
606 voxel_center_list, max_voxel_count);
612 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 614 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
615 int max_voxel_count)
const 618 key.
x = key.
y = key.
z = 0;
624 double min_x, min_y, min_z, max_x, max_y, max_z;
626 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
628 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
629 return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
630 k_indices, max_voxel_count);
635 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 637 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
640 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
648 this->genLeafNodeCenterFromOctreeKey (key, newPoint);
650 voxel_center_list.push_back (newPoint);
659 double mid_x = 0.5 * (min_x + max_x);
660 double mid_y = 0.5 * (min_y + max_y);
661 double mid_z = 0.5 * (min_z + max_z);
664 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
667 unsigned char child_idx;
674 child_idx =
static_cast<unsigned char> (curr_node ^ a);
679 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
682 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
683 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
684 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
694 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
695 child_key, voxel_center_list, max_voxel_count);
696 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
701 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
702 child_key, voxel_center_list, max_voxel_count);
703 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
708 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
709 child_key, voxel_center_list, max_voxel_count);
710 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
715 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
716 child_key, voxel_center_list, max_voxel_count);
717 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
722 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
723 child_key, voxel_center_list, max_voxel_count);
724 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
729 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
730 child_key, voxel_center_list, max_voxel_count);
731 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
736 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
737 child_key, voxel_center_list, max_voxel_count);
738 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
743 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
744 child_key, voxel_center_list, max_voxel_count);
748 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
749 return (voxel_count);
753 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 755 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
756 const OctreeNode* node,
const OctreeKey& key, std::vector<int> &k_indices,
int max_voxel_count)
const 758 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
767 (*leaf)->getPointIndices (k_indices);
776 double mid_x = 0.5 * (min_x + max_x);
777 double mid_y = 0.5 * (min_y + max_y);
778 double mid_z = 0.5 * (min_z + max_z);
781 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
784 unsigned char child_idx;
790 child_idx =
static_cast<unsigned char> (curr_node ^ a);
795 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
797 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
798 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
799 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
808 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
809 child_key, k_indices, max_voxel_count);
810 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
815 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
816 child_key, k_indices, max_voxel_count);
817 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
822 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
823 child_key, k_indices, max_voxel_count);
824 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
829 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
830 child_key, k_indices, max_voxel_count);
831 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
836 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
837 child_key, k_indices, max_voxel_count);
838 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
843 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
844 child_key, k_indices, max_voxel_count);
845 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
850 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
851 child_key, k_indices, max_voxel_count);
852 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
857 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
858 child_key, k_indices, max_voxel_count);
862 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
864 return (voxel_count);
867 #endif // PCL_OCTREE_SEARCH_IMPL_H_ int 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, std::vector< int > &k_indices, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of indices...
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...
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
void getNeighborsWithinRadiusRecursive(const PointT &point, const double radiusSquared, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn) const
Recursive search method that explores the octree and finds neighbors within a given radius...
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
int 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, int max_voxel_count) const
Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers...
Priority queue entry for point candidates
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &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, unsigned int tree_depth, std::vector< int > &k_indices) const
Recursive search method that explores the octree and finds points within a rectangular search area...
void approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
double getKNearestNeighborRecursive(const PointT &point, unsigned int K, const BranchNode *node, const OctreeKey &key, unsigned int 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.
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area.
int point_idx_
Index representing a point in the dataset given by setInputCloud.
void approxNearestSearchRecursive(const PointT &point, const BranchNode *node, const OctreeKey &key, unsigned int tree_depth, int &result_index, float &sqr_distance)
Recursive search method that explores the octree and finds the approximate nearest neighbor...
int getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, int max_voxel_count=0) const
Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction).
int getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector< int > &k_indices, int max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Abstract octree leaf class
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0)
Search for all neighbors of query point that are within a given radius.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
float point_distance_
Distance to query point.
Abstract octree node class