转载自: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树代码)不知快到哪里去了。当然自己写只是为了更好的理解其中的原理,真要用的时候还得靠别人的轮子...