PCL中Kd tree的实现

 之前已经对kd-tree进行详细说明，并附有C++实现代码。 kd-tree原理详解

 PCL中kd-tree库也提供了kd-tree的数据结构，它是基于FLANN进行快速最近邻查找。最近邻查找在匹配、特征描述计算、邻域特征提取中是非常核心的操作。且它依赖与pcl_common模块。

kd-tree中的类说明

 kd-tree模块中主要有两个类，即class pcl::KdTree和class pcl::KdTreeFLANN；它们之间是继承关系，KdTreeFLANN继承于KdTree。

1. class pcl::KdTree< PointT >

1. KdTree (bool sorted = true)
    空构造函数，为成员变量设置默认值。
2. virtual void setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ())
    设置输入点云数据，为kd-tree提供输入数据的共享指针。indices为kd-tree中使用的点对应的索引，如果不设置，则默认使用整个点云填充kd-tree。
3. virtual ~KdTree ()
    析构函数。
4. virtual int nearestKSearch (const PointT &p_q, int k, std::vector &k_indices, std::vector &k_sqr_distances) const = 0
    纯虚函数，具体实现在其子类KdTreeFLANN中，用于进行K邻域搜索，参数p_q为需要查询的点，k为K邻域个数，k_indices为搜索完的邻域点对应的索引，k_sqr_distances为搜索完的每个邻域点与查询点之间的欧式距离。
5. virtual int nearestKSearch (const PointCloud &cloud, int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const
    虚函数，参数cloud为需要查询的点集合，index为点集合中需要查询点的索引，其他参数同上。
6. inline int nearestKSearchT (const PointTDiff &point, int k, std::vector &k_indices, std::vector &k_sqr_distances) const
   &emsp；内联函数，模板函数，point为需要查询的点，其余参数同上。
7. virtual int nearestKSearch (int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const
    虚函数，用来进行k邻域搜索，参数index为点云中需要查询的索引，其他参数同上。
8. 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；纯虚函数，具体实现在子类KdTreeFLANN中，其用来进行r半径内的邻域搜索，参数p_q为需要查询的点，radius为需要查询的半径大小，k_Iindices为搜索完的邻域对应的索引，k_sqr_distances为搜索完的每个邻域点与查询点之间的欧式距离，max_nn为设置返回的邻域个数上限，如果为0或者大于返回的邻域个数，其返回全部查询结果。
9. 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；虚函数，参数cloud为需要查询的点集合，index为点集合中需要查询点的索引，其他参数同上
10. inline int radiusSearchT (const PointTDiff &point, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const
     内联函数，模板函数，point为需要查询的点，其余参数同上。
11. virtual int radiusSearch (int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const
     虚函数，index为需要查询点的索引，其余参数同上。
12. virtual inline void setEpsilon (float eps)
     虚函数，设置误差限。
13. inline void setMinPts (int min_pts)
     内联函数，设置k近邻域中可行结果的最小数目。

2. pcl::KdTreeFLANN< PointT >

 因为KdTreeFLANN继承于KdTree，所以和其相关的函数说明均可参考其基类说明。

事例代码

#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)