1、第5章 kd-tree,通过雷达、激光扫描、立体摄像机等三维测量设备获取的点云数据，具有数据量大、分布不均匀等特点。作为三维领域中一个重要的数据来源，点云数据主要是表征目标表面的海量点集合，并不具备传统实体网格数据的几何拓扑信息。所以点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于邻域关系的快速查找。 本章首先对常用的点云空间索引方法kd-tree概念进行介绍，然后对PCL的kd-tree相关模块及类进行简单说明，最后通过应用实例来展示如何灵活应用PCL中kd-tree模块。,本章各小节目录,5.1 kd-tree概念简介 5.2 PCL中kd-tree模块及类介绍 5.3 应
2、用实例解析,5.1 kd-tree概念简介,Kd-tree或者k维树是计算机科学中使用的一种数据结构，用来组织表示k维空间中点集合。它是一种带有其他约束条件的二分查找树。kd-tree对于区间和近邻搜索十分有用。我们为了达到目的，通常只在三个维度中进行处理，因此所有的kd-tree都将是三维kd-tree。如图5-1所示，kd-tree的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开（也就是说，如果第一维坐标小于根节点的点它将分在左边的子树中，如果大于根节点的点它将分在右边的子树中）。,树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度
3、。建立kd-tree最高效的方法是，像快速分类一样使用分割法，把指定维度的值放在根上，在该维度上包含较小数值的在左子树，较大的在右子树。然后分别在左边和右边的子树上重复这个过程，直到用户准备分类的最后一个树仅仅由一个元素组成。,5.2 PCL中kd-tree模块及类介绍,PCL中kd-tree库提供了kd-tree数据结构，基于FLANN进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、邻域特征提取中是非常基础的核心操作。kd-tree模块利用三个类与两个函数实现了利用kd-tree数据结构对点云的高效管理和检索，其依赖于pcl_common模块。,5.1.1 kd-tree模块中类说明以
4、及全局函数说明kd-tree模块中目前共有3个类，以后有可能增加以下两个类。 1. class pcl:KdTree 类KdTree是kd-tree数据结构的实现，并且是具有近邻搜索实现的类KdTreeFLANN的基类，其继承关系如图5-2所示。,图5-2 kd-tree模块中类的继承关系,类KdTree关键成员函数 KdTree(bool sorted=true) 空的构造函数 virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr& indices=IndicesConstPtr()
5、 设置输入点云，参数cloud为输入点云的共享指针引用，indices为在kd-tree中使用的点对应的索引，如果不设置，则默认使用整个点云填充kd-tree。 virtualKdTree() 析构函数。 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为搜索完的邻域点对应的索引，
6、k_sqr_distances为搜索完的每个领域点与查询点之间的欧式距离。 virtual int nearestKSearch(const PointCloud& cloud, int index, int k, std:vector& k_indices, std:vector& k_sqr_distances) const 纯虚函数，具体实现在其子类KdTreeFLANN中，参数cloud为需要查询的点集合，index为点集合中需要查询点的索引，其他参数同上。 virtual int nearestKSearch(int index, int k, std:vector& k_indic
7、es, std:vector& k_sqr_distances) const 纯虚函数，具体实现在其子类KdTreeFLANN中，其用来进行k邻域搜索，参数index为点云中需要查询点的索引，其他参数同上。,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 纯虚函数，具体实现在其子类KdTreeFLANN中，其用来进行r半径内的邻域搜索，参数p_q为需要查询的
8、点，radius为需要查询的半径大小，k_indices为搜索完的邻域点对应的索引，k_sqr_distances为搜索完的每个领域点与查询点之间的欧式距离，max_nn为设置返回的邻域个数上限，如果为0或者大于返回的领域个数，其返回全部查询结果。 virtual int radiusSearch(int index, double radius, std:vector& k_indices, std:vector& k_sqr_distances, unsigned int max_nn=0) const 纯虚函数，具体实现在其子类KdTreeFLANN中，其用来进行r半径内的领域搜索，参数
9、index为需要查询的点,的索引，其他同上。 virtual void setEpsilon(float eps) 设置误差限。 void setMinPts(int min_pts) 设置k近邻搜索中可行结果的最小数目。 2. class pcl:KdTreeFLANN 类KdTreeFLANN是继承了kd-tree数据结构的具有3D空间检索功能实现的KdTree子类。 类KdTreeFLANN关键成员函数： KdTreeFLANN(bool sorted=true) KdTreeFLANN类的默认构造函数。 KdTreeFLANN(const KdTreeFLANN& k) 复制构造函数。
10、 KdTreeFLANN&operator=(const KdTreeFLANN &k) 复制运算符重载函数。,void setEpsilon(float eps) 设置搜索精度。 void setSortedResults(bool sorted) 设置是否对结果进行排序。 virtualKdTreeFLANN() KdTreeFLANN析构函数。 void setInputCloud(const PointCloudConstPtr& cloud, const IndicesConstPtr& indices=IndicesConstPtr() 设置输入点云，参数cloud为输入点云的共享
11、指针引用，indices为在kd-tree中使用的点对应的索引，如果不设置，则默认使用整个点云填充kd-tree。 int nearestKSearch(const PointT& point, int k, std:vector& k_indices, std:vector& k_sqr_distances) const 实现父类的纯虚函数k近邻搜索，参数说明见父类。 int radiusSearch(const PointT& point, double radius, std:vector,& k_indices, std:vector& k_sqr_distances, unsigne
12、d int max_nn=0)const 实现父类的纯虚函数k近邻搜索，参数说明见父类。 PointCloudConstPtrgetInputCloud() const 获取输入点云的共享指针。 virtual int nearestKSearch(const PointCloud& cloud, int index, int k, std:vector& k_indices, std:vector& k_sqr_distances) const 实现父类的纯虚函数k近邻搜索，参数说明见父类。 virtual int nearestKSearch(int index, int k, std:v
13、ector& k_indices, std:vector& k_sqr_distances) const 实现父类的纯虚函数k近邻搜索，参数说明见父类。 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,实现父类的纯虚函数k近邻搜索，参数说明见父类。,5.2.2 kd-tree模块其他关键成员说明template void pcl:g
14、etApproximateIndices(const typename pcl:PointCloud:Ptr& cloud_in, const typename pcl:PointCloud:Ptr& cloud_ref, std:vector& indices) 获取参数点云中查询点云的近似，参数cloud_in为需要查询的点云引用，cloud_ref为参考点云的引用，indices返回查询结果在cloud_ref中的索引向量。该函数实现在PCL命名空间下，为全局函数，其实是通过函nearestKSearch()搜索输入点云cloud_in每个点的k为1的近邻来实现的。,5.3 应用实例解析
15、,本节中一起学习如何用kd-tree树找到具体点或空间位置的k近邻，然后学习如何找到用户（本例中是随机的）指定的某一半径内的所有近邻。 在本书提供光盘的第5章例1文件夹中，打开名为kdtree_search.cpp的代码文件。 1. 代码解释说明 下面对源代码中的关键语句进行解析，代码首先用系统时间初始化rand()函数的种子，利用时间初始化，每次运行时所产生的随机数都是不同的，或,者用户可以用固定的数值或不进行初始化随机种子，运行程序产生的随机数不变，然后创建点云对象，并用随机数据填充点云对象。 srand(time(NULL); /用系统时间初始化随机种子 pcl:PointCloud:P
16、tr cloud(new pcl:PointCloud); /随机点云生成 cloud-width=1000; /此处为点云数量 cloud-height=1; /此处表示点云为无序点云 cloud-points.resize(cloud-width*cloud-height); for(size_t i=0; ipoints.size(); +i) /循环填充点云数据 cloud-pointsi.x=1024.0f*rand()/(RAND_MAX+1.0f); cloud-pointsi.y=1024.0f*rand()/(RAND_MAX+1.0f); cloud-pointsi.z=1
17、024.0f*rand()/(RAND_MAX+1.0f); ,下面的代码块创建了KdTreeFLANN对象，并把创建的点云设置成输入，然后创建一个searchPoint变量作为查询点，并且分配随机坐标值给它。 pcl:KdTreeFLANN kdtree; /创建kd-tree对象 kdtree.setInputCloud(cloud); /设置搜索空间 pcl:PointXYZ searchPoint; /定义查询点并赋随机值 searchPoint.x=1024.0f*rand()/(RAND_MAX+1.0f); searchPoint.y=1024.0f*rand()/(RAND_M
18、AX+1.0f); searchPoint.z=1024.0f*rand()/(RAND_MAX+1.0f); 现在创建一个整数（设置成10）和两个向量来存储搜索到的K近邻，两个向量中一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方。 /k近邻搜索 int K=10; std:vector pointIdxNKNSearch(K); /存储查询点近邻索引,std:vector pointNKNSquaredDistance(K); /存储近邻点对应距离平方 std:cout0) /执行k近邻搜索 for(size_t i=0; ipointspointIdxNKNSearchi.x
19、pointspointIdxNKNSearchi.ypointspointIdxNKNSearchi.z“(squared distance:”pointNKNSquaredDistancei“)”std:endl; ,下面代码展示查找到给定searchPoint的某一半径（随机产生）内的所有近邻，重新定义两个向量pointIdxRadiusSearch、pointRadiusSquaredDistance来存储关于近邻的信息。 /半径r内近邻搜索方式 std:vector pointIdxRadiusSearch; /存储近邻索引 std:vector pointRadiusSquaredD
20、istance; /存储近邻对应的距离平方 float radius=256.0f*rand()/(RAND_MAX+1.0f); 像之前一样，如果kd-tree对象在指定半径内返回多于0个近邻，它将打印输出向量中存储的点的坐标与距离。 if(kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)0) for(size_t i=0; ipointIdxRadiusSearch.size(); +i),Std:coutpointspointIdxRadiusSearchi.xpointspointIdxRadiusSearchi.ypointspointIdxRadiusSearchi.zkdtree_search.exe 运行之后将看到类似图5-3所示的结果，打印输出所有近邻的点坐标以及对应的距离平方值。,
