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 ();
92 unsigned int result_count;
95 std::vector<prioPointQueueEntry> point_candidates;
98 key.
x = key.
y = key.
z = 0;
101 double smallest_dist = std::numeric_limits<double>::max ();
103 getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates);
105 result_count = static_cast<unsigned int> (point_candidates.size ());
107 k_indices.resize (result_count);
108 k_sqr_distances.resize (result_count);
110 for (i = 0; i < result_count; ++i)
112 k_indices [i] = point_candidates [i].point_idx_;
113 k_sqr_distances [i] = point_candidates [i].point_distance_;
116 return static_cast<int> (k_indices.size ());
120 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 122 std::vector<int> &k_indices,
123 std::vector<float> &k_sqr_distances)
125 const PointT search_point = this->getPointByIndex (index);
126 return (nearestKSearch (search_point, k, k_indices, k_sqr_distances));
130 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 135 assert(this->leaf_count_>0);
136 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
139 key.
x = key.
y = key.
z = 0;
141 approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance);
147 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 151 const PointT search_point = this->getPointByIndex (query_index);
153 return (approxNearestSearch (search_point, result_index, sqr_distance));
157 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 159 std::vector<int> &k_indices,
160 std::vector<float> &k_sqr_distances,
161 unsigned int max_nn)
const 163 assert (
isFinite (p_q) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
165 key.
x = key.
y = key.
z = 0;
168 k_sqr_distances.clear ();
170 getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances,
173 return (static_cast<int> (k_indices.size ()));
177 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 179 std::vector<int> &k_indices,
180 std::vector<float> &k_sqr_distances,
181 unsigned int max_nn)
const 183 const PointT search_point = this->getPointByIndex (index);
185 return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn));
189 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 191 const Eigen::Vector3f &max_pt,
192 std::vector<int> &k_indices)
const 196 key.
x = key.
y = key.
z = 0;
200 boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices);
202 return (static_cast<int> (k_indices.size ()));
207 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
double 210 const double squared_search_radius, std::vector<prioPointQueueEntry>& point_candidates)
const 212 std::vector<prioBranchQueueEntry> search_heap;
213 search_heap.resize (8);
215 unsigned char child_idx;
219 double smallest_squared_dist = squared_search_radius;
222 double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth);
225 for (child_idx = 0; child_idx < 8; child_idx++)
227 if (this->branchHasChild (*node, child_idx))
231 search_heap[child_idx].key.x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
232 search_heap[child_idx].key.y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
233 search_heap[child_idx].key.z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
236 this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center);
239 search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx);
240 search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point);
244 search_heap[child_idx].point_distance = std::numeric_limits<float>::infinity ();
248 std::sort (search_heap.begin (), search_heap.end ());
252 while ((!search_heap.empty ()) && (search_heap.back ().point_distance <
253 smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_))
258 child_node = search_heap.back ().node;
259 new_key = search_heap.back ().key;
261 if (tree_depth < this->octree_depth_)
264 smallest_squared_dist = getKNearestNeighborRecursive (point,
K, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
265 smallest_squared_dist, point_candidates);
273 std::vector<int> decoded_point_vector;
275 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
278 (*child_leaf)->getPointIndices (decoded_point_vector);
281 for (i = 0; i < decoded_point_vector.size (); i++)
284 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
287 squared_dist = pointSquaredDist (candidate_point, point);
290 if (squared_dist < smallest_squared_dist)
295 point_entry.
point_idx_ = decoded_point_vector[i];
296 point_candidates.push_back (point_entry);
300 std::sort (point_candidates.begin (), point_candidates.end ());
302 if (point_candidates.size () >
K)
303 point_candidates.resize (
K);
305 if (point_candidates.size () ==
K)
306 smallest_squared_dist = point_candidates.back ().point_distance_;
309 search_heap.pop_back ();
312 return (smallest_squared_dist);
316 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 319 unsigned int tree_depth, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances,
320 unsigned int max_nn)
const 323 unsigned char child_idx;
326 double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth);
329 for (child_idx = 0; child_idx < 8; child_idx++)
331 if (!this->branchHasChild (*node, child_idx))
335 child_node = this->getBranchChildPtr (*node, child_idx);
342 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
343 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
344 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
347 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
350 squared_dist = pointSquaredDist (static_cast<const PointT&> (voxel_center), point);
353 if (squared_dist + this->epsilon_
354 <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared))
357 if (tree_depth < this->octree_depth_)
360 getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1,
361 k_indices, k_sqr_distances, max_nn);
362 if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
370 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
371 std::vector<int> decoded_point_vector;
374 (*child_leaf)->getPointIndices (decoded_point_vector);
377 for (i = 0; i < decoded_point_vector.size (); i++)
379 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
382 squared_dist = pointSquaredDist (candidate_point, point);
385 if (squared_dist > radiusSquared)
389 k_indices.push_back (decoded_point_vector[i]);
390 k_sqr_distances.push_back (squared_dist);
392 if (max_nn != 0 && k_indices.size () == static_cast<unsigned int> (max_nn))
401 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 405 unsigned int tree_depth,
409 unsigned char child_idx;
410 unsigned char min_child_idx;
411 double min_voxel_center_distance;
419 min_voxel_center_distance = std::numeric_limits<double>::max ();
421 min_child_idx = 0xFF;
424 for (child_idx = 0; child_idx < 8; child_idx++)
426 if (!this->branchHasChild (*node, child_idx))
430 double voxelPointDist;
432 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
433 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
434 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
437 this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center);
439 voxelPointDist = pointSquaredDist (voxel_center, point);
442 if (voxelPointDist >= min_voxel_center_distance)
445 min_voxel_center_distance = voxelPointDist;
446 min_child_idx = child_idx;
447 minChildKey = new_key;
451 assert(min_child_idx<8);
453 child_node = this->getBranchChildPtr (*node, min_child_idx);
455 if (tree_depth < this->octree_depth_)
458 approxNearestSearchRecursive (point, static_cast<const BranchNode*> (child_node), minChildKey, tree_depth + 1, result_index,
466 double smallest_squared_dist;
468 std::vector<int> decoded_point_vector;
470 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
472 smallest_squared_dist = std::numeric_limits<double>::max ();
475 (**child_leaf).getPointIndices (decoded_point_vector);
478 for (i = 0; i < decoded_point_vector.size (); i++)
480 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
483 squared_dist = pointSquaredDist (candidate_point, point);
486 if (squared_dist >= smallest_squared_dist)
489 result_index = decoded_point_vector[i];
490 smallest_squared_dist = squared_dist;
491 sqr_distance = static_cast<float> (squared_dist);
497 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
float 499 const PointT & point_b)
const 501 return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm ();
505 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
void 507 const Eigen::Vector3f &max_pt,
510 unsigned int tree_depth,
511 std::vector<int>& k_indices)
const 514 unsigned char child_idx;
517 for (child_idx = 0; child_idx < 8; child_idx++)
521 child_node = this->getBranchChildPtr (*node, child_idx);
528 new_key.
x = (key.
x << 1) + (!!(child_idx & (1 << 2)));
529 new_key.
y = (key.
y << 1) + (!!(child_idx & (1 << 1)));
530 new_key.
z = (key.
z << 1) + (!!(child_idx & (1 << 0)));
533 Eigen::Vector3f lower_voxel_corner;
534 Eigen::Vector3f upper_voxel_corner;
536 this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner);
540 if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) ||
541 (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) ||
542 (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) )
545 if (tree_depth < this->octree_depth_)
548 boxSearchRecursive (min_pt, max_pt, static_cast<const BranchNode*> (child_node), new_key, tree_depth + 1, k_indices);
554 std::vector<int> decoded_point_vector;
557 const LeafNode* child_leaf = static_cast<const LeafNode*> (child_node);
560 (**child_leaf).getPointIndices (decoded_point_vector);
563 for (i = 0; i < decoded_point_vector.size (); i++)
565 const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]);
568 bInBox = ( (candidate_point.x >= min_pt (0)) && (candidate_point.x <= max_pt (0)) &&
569 (candidate_point.y >= min_pt (1)) && (candidate_point.y <= max_pt (1)) &&
570 (candidate_point.z >= min_pt (2)) && (candidate_point.z <= max_pt (2)) );
574 k_indices.push_back (decoded_point_vector[i]);
582 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 585 int max_voxel_count)
const 588 key.
x = key.
y = key.
z = 0;
590 voxel_center_list.clear ();
595 double min_x, min_y, min_z, max_x, max_y, max_z;
597 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
599 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
600 return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
601 voxel_center_list, max_voxel_count);
607 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 609 Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector<int> &k_indices,
610 int max_voxel_count)
const 613 key.
x = key.
y = key.
z = 0;
619 double min_x, min_y, min_z, max_x, max_y, max_z;
621 initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a);
623 if (std::max (std::max (min_x, min_y), min_z) < std::min (std::min (max_x, max_y), max_z))
624 return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key,
625 k_indices, max_voxel_count);
630 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 632 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
635 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
643 this->genLeafNodeCenterFromOctreeKey (key, newPoint);
645 voxel_center_list.push_back (newPoint);
654 double mid_x = 0.5 * (min_x + max_x);
655 double mid_y = 0.5 * (min_y + max_y);
656 double mid_z = 0.5 * (min_z + max_z);
659 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
662 unsigned char child_idx;
669 child_idx = static_cast<unsigned char> (curr_node ^ a);
674 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
677 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
678 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
679 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
689 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
690 child_key, voxel_center_list, max_voxel_count);
691 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
696 voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
697 child_key, voxel_center_list, max_voxel_count);
698 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
703 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
704 child_key, voxel_center_list, max_voxel_count);
705 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
710 voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
711 child_key, voxel_center_list, max_voxel_count);
712 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
717 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
718 child_key, voxel_center_list, max_voxel_count);
719 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
724 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
725 child_key, voxel_center_list, max_voxel_count);
726 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
731 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
732 child_key, voxel_center_list, max_voxel_count);
733 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
738 voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
739 child_key, voxel_center_list, max_voxel_count);
743 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
744 return (voxel_count);
748 template<
typename Po
intT,
typename LeafContainerT,
typename BranchContainerT>
int 750 double min_x,
double min_y,
double min_z,
double max_x,
double max_y,
double max_z,
unsigned char a,
751 const OctreeNode* node,
const OctreeKey& key, std::vector<int> &k_indices,
int max_voxel_count)
const 753 if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0)
759 const LeafNode* leaf = static_cast<const LeafNode*> (node);
762 (*leaf)->getPointIndices (k_indices);
771 double mid_x = 0.5 * (min_x + max_x);
772 double mid_y = 0.5 * (min_y + max_y);
773 double mid_z = 0.5 * (min_z + max_z);
776 int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z);
779 unsigned char child_idx;
785 child_idx = static_cast<unsigned char> (curr_node ^ a);
790 child_node = this->getBranchChildPtr (static_cast<const BranchNode&> (*node), child_idx);
792 child_key.
x = (key.
x << 1) | (!!(child_idx & (1 << 2)));
793 child_key.
y = (key.
y << 1) | (!!(child_idx & (1 << 1)));
794 child_key.
z = (key.
z << 1) | (!!(child_idx & (1 << 0)));
803 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node,
804 child_key, k_indices, max_voxel_count);
805 curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1);
810 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node,
811 child_key, k_indices, max_voxel_count);
812 curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8);
817 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node,
818 child_key, k_indices, max_voxel_count);
819 curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3);
824 voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node,
825 child_key, k_indices, max_voxel_count);
826 curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8);
831 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node,
832 child_key, k_indices, max_voxel_count);
833 curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5);
838 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node,
839 child_key, k_indices, max_voxel_count);
840 curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8);
845 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node,
846 child_key, k_indices, max_voxel_count);
847 curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7);
852 voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node,
853 child_key, k_indices, max_voxel_count);
857 }
while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count));
859 return (voxel_count);
862 #define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch<T>; 864 #endif // PCL_OCTREE_SEARCH_IMPL_H_ 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.
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.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
float pointSquaredDist(const PointT &point_a, const PointT &point_b) const
Helper function to calculate the squared distance between two points.
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 approxNearestSearch(const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approx.
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).
int point_idx_
Index representing a point in the dataset given by setInputCloud.
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.
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).
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Abstract octree leaf class
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.
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.
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.
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
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 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.