Kd树按空间划分生成叶子节点，各个叶子节点里存放点数据，其可以按半径搜索或邻区搜索。PCL中的Kd tree的基础数据结构使用了FLANN以便可以快速的进行邻区搜索。FLANN is a library for performing fast approximate nearest neighbor searches in high dimensional spaces。下面是一个最基本的例子，只寻找一个最近点：
#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->width = 400000; cloud->height = 1; cloud->points.resize (cloud->width * cloud->height); // fills a PointCloud with random data 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 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // sets our randomly created cloud as the input kdtree.setInputCloud (cloud); //create a “searchPoint” which is assigned random coordinates 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 int K = 1; std::vector<int> pointIdxNKNSearch(K); std::vector<float> pointNKNSquaredDistance(K); std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; /*********************************************************************************************** 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 ) { 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树不知快到哪里去了。当然自己写只是为了更好的理解其中的原理，真要用的时候还得靠别人的轮子...
参考：