pcl小知识（十）——实例运行kd-tree

转载自：https://www.cnblogs.com/21207-iHome/p/6103354.html#undefined

下面代码给出了在实际编程中调用kd树的示例。

Kd树按空间划分生成叶子节点，各个叶子节点里存放点数据，其可以按半径搜索或邻区搜索。PCL中的Kd tree的基础数据结构使用了FLANN以便可以快速的进行邻区搜索。FLANN is a library for performing fast approximate nearest neighbor searches in high dimensional spaces。下面是一个最基本的例子，给定一个中心点searchPoint，然后从40万点中寻找一个最近点：

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>


	int main(int argc, char** argv)
	{
		srand(time(NULL));  //seeds rand() with the system time 

		time_t begin, end;
		begin = clock();  //开始计时
		//-------------------------------------------------------------------------------
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

		// Generate pointcloud data，新建指针cloud存放40万点云，下面是设置cloud的长度
		cloud->width = 400000;
		cloud->height = 1;
		cloud->points.resize(cloud->width * cloud->height);

		// fills a PointCloud with random data，利用随机数填充点云指针cloud
		for (size_t i = 0; i < cloud->points.size(); ++i)
		{
			cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
			cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
			cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
		}

		// creates kdtree object，新建一个搜索对象kdtree，就像StatisticalOutlierRemoval中的sor.
		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

		// sets our randomly created cloud as the input，输入点云cloud
		kdtree.setInputCloud(cloud);

		//create a “searchPoint” which is assigned random coordinates，创建一个指定的随机坐标点searchPoint作为中心点
		pcl::PointXYZ searchPoint;

		searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
		searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);


		// K nearest neighbor search，进行k最近邻搜索
		int K = 1;                               //设搜索一个点，即：K为1.
		std::vector<int> pointIdxNKNSearch(K);   //设置一个类似数组的东西，（我对于C++中vector的理解直接把它当成了数组），pointIdxNKNSearch（K）的长度为K，用于存放点索引
		std::vector<float> pointNKNSquaredDistance(K);//同样一个长度为K的数组pointNKNSquaredDistance，用于存放距离的平方

		std::cout << "K nearest neighbor search at (" << searchPoint.x
			<< " " << searchPoint.y
			<< " " << searchPoint.z
			<< ") with K=" << K << std::endl;          //输出：点searchPoint周围K=1的最近点

		/***********************************************************************************************
		template<typename PointT>
		virtual int pcl::KdTree< PointT >::nearestKSearch  ( const PointT &  p_q,
		int  k,
		std::vector< int > &  k_indices,
		std::vector< float > &  k_sqr_distances
		)  const [pure virtual]

		Search for k-nearest neighbors for the given query point.
		Parameters:
		[in] the given query point
		[in] k the number of neighbors to search for
		[out] the resultant indices of the neighboring points
		[out] the resultant squared distances to the neighboring points
		Returns:
		number of neighbors found
		********************************************************************************************/
		if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)//前面定义了kdtree作为搜索对象，然后就可以用它来调用系统的nearestKSearch函数了，注意后面的参数，分别是上面已经定义好的。
		{
			for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
				std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
				<< " " << cloud->points[pointIdxNKNSearch[i]].y
				<< " " << cloud->points[pointIdxNKNSearch[i]].z
				<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
		}
		//--------------------------------------------------------------------------------------------
		end = clock();  //结束计时
		double Times = double(end - begin) / CLOCKS_PER_SEC; //将clock()函数的结果转化为以秒为单位的量

		std::cout << "time: " << Times << "s" << std::endl;

		return 0;
	}

　生成四十万个随机点，release版本下测试0.3s左右找到最近点，这比之前自己写的Kd树（这是我转载的那位原作者的链接，他自己写的kd树代码）不知快到哪里去了。当然自己写只是为了更好的理解其中的原理，真要用的时候还得靠别人的轮子...