KD tree has been described in detail before, with C + + implementation code attached. Detailed explanation of KD tree principle

the KD Tree Library in PCL also provides the data structure of KD tree, which performs fast nearest neighbor search based on FLANN. Nearest neighbor search is a very core operation in matching, feature description calculation and neighborhood feature extraction. And it depends on pcl_common module.

## Class description in KD tree

there are two main classes in KD tree module, namely class pcl::KdTree and class pcl::KdTreeFLANN; There is an inheritance relationship between them. KdTreeFLANN inherits from KdTree.

### 1. class pcl::KdTree< PointT >

- KdTree (bool sorted = true)

empty constructor to set the default value for member variables. - virtual void setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ())

set the input point cloud data and provide KD tree with a shared pointer to the input data. Indexes is the index corresponding to the points used in the KD tree. If it is not set, the KD tree is filled with the whole point cloud by default. - virtual ~KdTree ()

destructor. - virtual int nearestKSearch (const PointT &p_q, int k, std::vector &k_indices, std::vector &k_sqr_distances) const = 0

pure virtual function, specifically implemented in its subclass KdTreeFLANN, used for K-Neighborhood search, parameter p_q is the point to be queried, K is the number of K neighborhoods, K_ Indexes is the index corresponding to the searched neighborhood point, k_sqr_distances is the Euclidean distance between each neighborhood point searched and the query point. - virtual int nearestKSearch (const PointCloud &cloud, int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const

virtual function, parameter cloud is the point set to be queried, index is the index of the point to be queried in the point set, and other parameters are the same as above. - inline int nearestKSearchT (const PointTDiff &point, int k, std::vector &k_indices, std::vector &k_sqr_distances) const

&emsp； Inline function, template function, point is the point to be queried, and other parameters are the same as above. - virtual int nearestKSearch (int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const

virtual function, used for k-Neighborhood search. The parameter index is the index to be queried in the point cloud. Other parameters are the same as above. - virtual int radiusSearch (const PointT &p_q, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const = 0

&emsp； Pure virtual function, specifically implemented in the subclass KdTreeFLANN, which is used for neighborhood search within r radius, parameter p_q is the point to be queried, radius is the radius to be queried, k_Iindices is the index corresponding to the searched neighborhood, k_sqr_distances is the Euclidean distance between each neighborhood point searched and the query point, max_nn is the upper limit of the number of returned neighborhoods. If it is 0 or greater than the number of returned neighborhoods, it returns all query results. - virtual int radiusSearch (const PointCloud &cloud, int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const

&emsp； Virtual function, parameter cloud is the set of points to be queried, index is the index of the points to be queried in the set of points, and other parameters are the same as above - inline int radiusSearchT (const PointTDiff &point, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const

inline function, template function, point is the point to be queried, and other parameters are the same as above. - virtual int radiusSearch (int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const

virtual function, index is the index of the query point, and the other parameters are the same as above. - virtual inline void setEpsilon (float eps)

virtual function, set the error limit. - inline void setMinPts (int min_pts)

inline function, which sets the minimum number of feasible results in the k-nearest neighborhood.

### 2. pcl::KdTreeFLANN< PointT >

because KdTreeFLANN inherits from KdTree, its related function description can refer to its base class description.

## Case code

#include <pcl/point_cloud.h> #include <pcl/kdtree/kdtree_flann.h> #include <iostream> #include <vector> #include <ctime> void print_point(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &idxs, std::vector<float> &dis){ for (size_t i = 0; i < dis.size(); ++i){ std::cout << "point " << i + 1 << ", x:" << cloud->points[idxs[i] ].x << ", y:" << cloud->points[idxs[i] ].y << ", z:" << cloud->points[idxs[i] ].z << ", distance:" << dis.at(i) << ")" << std::endl; } } int main(int argc, char** argv){ srand(time(nullptr)); // create cloud and init the obj pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); cloud->width = 1000; cloud->height = 1; cloud->points.resize(cloud->width * cloud->height); // init xyz coors for(size_t i = 0; i < cloud->points.size(); ++i){ cloud->points[i].x = 1024.f * rand() / (RAND_MAX + 1.f); cloud->points[i].y = 1024.f * rand() / (RAND_MAX + 1.f); cloud->points[i].z = 1024.f * rand() / (RAND_MAX + 1.f); } // create KdTreeFLANN's obj pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree; // set kd_tree input cloud kd_tree.setInputCloud(cloud); // set search point pcl::PointXYZ search_point; search_point.x = 1024.f * rand() / (RAND_MAX + 1.f); search_point.y = 1024.f * rand() / (RAND_MAX + 1.f); search_point.z = 1024.f * rand() / (RAND_MAX + 1.f); // nearest point search int k = 10; std::vector<int> point_nkn_idx_search(k); std::vector<float> point_nkn_squared_distance(k); std::cout << "Nearest neighbor search at ( x:" << search_point.x << ", y:" << search_point.y << ", z:" << search_point.z << std::endl; if (0 < kd_tree.nearestKSearch(search_point, k, point_nkn_idx_search, point_nkn_squared_distance)){ print_point(cloud, point_nkn_idx_search, point_nkn_squared_distance); } // radius search std::vector<int> point_idx_radius_search; std::vector<float> point_radius_squared_distance; float radius = 256.f * rand() / (RAND_MAX + 1.f); std::cout << "Neighbors within radius search at ( x:" << search_point.x << ", y:" << search_point.y << ", z:" << search_point.z << ", radius = " << radius << std::endl; if(0 < kd_tree.radiusSearch(search_point, radius, point_idx_radius_search, point_radius_squared_distance)){ print_point(cloud, point_nkn_idx_search, point_radius_squared_distance); } return 0; } result: nearest neighbor search at ( x:997.516, y:318.435, z:346.642 point 1, x:977.089, y:278.464, z:416.04, distance:6831.11) point 2, x:1017.69, y:342.322, z:434.146, distance:8634.49) point 3, x:982.738, y:398.693, z:267.528, distance:12918.7) point 4, x:967.351, y:399.696, z:272.996, distance:12937) point 5, x:955.333, y:206.196, z:310.058, distance:15715.3) point 6, x:1019.2, y:425.554, z:424.412, distance:17992.8) point 7, x:868.145, y:270.722, z:349.877, distance:19023.9) point 8, x:945.958, y:397.525, z:448.071, distance:19201.3) point 9, x:997.9, y:175.836, z:326.142, distance:20755) point 10, x:932.754, y:447.417, z:404.144, distance:24136.9) Neighbors within radius search at ( x:997.516, y:318.435, z:346.642, radius = 139.389 point 1, x:977.089, y:278.464, z:416.04, distance:6831.11) point 2, x:1017.69, y:342.322, z:434.146, distance:8634.49) point 3, x:982.738, y:398.693, z:267.528, distance:12918.7) point 4, x:967.351, y:399.696, z:272.996, distance:12937) point 5, x:955.333, y:206.196, z:310.058, distance:15715.3) point 6, x:1019.2, y:425.554, z:424.412, distance:17992.8) point 7, x:868.145, y:270.722, z:349.877, distance:19023.9) point 8, x:945.958, y:397.525, z:448.071, distance:19201.3)