kd-tree搜索近邻点

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>

using namespace pcl;
using namespace std;
void main()
{

    srand(time(NULL));
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    cloud->width=1000;
    cloud->height=1;
    cloud->resize(cloud->width*cloud->height);
    for (size_t i=0;i<cloud->size();++i)
    {
        cloud->points[i].x=1024*rand()/(RAND_MAX+1.0f);
        cloud->points[i].y=1024*rand()/(RAND_MAX+1.0f);
        cloud->points[i].z=1024*rand()/(RAND_MAX+1.0f);
    }
    KdTreeFLANN <PointXYZ> kdtree;//创建kdtree对象
    kdtree.setInputCloud(cloud);//设置搜索空间
    PointXYZ searchPoint;//定义查询点并设随机值
    searchPoint.x=1024*rand()/(RAND_MAX+1.0f);
    searchPoint.y=1024*rand()/(RAND_MAX+1.0f);
    searchPoint.z=1024*rand()/(RAND_MAX+1.0f);

    cout<<"K近邻搜索：\n" ;
    int K=10;
    vector<int> pointIndexNKNSearch(K);//存储查询点近邻索引
    vector<float> pointNKNSquaredDistance(K);
    cout<<"K nearest neighbor search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<")with K"<<K<<endl;
    if (kdtree.nearestKSearch(searchPoint,K,pointIndexNKNSearch,pointNKNSquaredDistance)>0)
    {
        for(size_t i=0;i<pointIndexNKNSearch.size();++i)
        {
            cout<<" "<<cloud->points[pointIndexNKNSearch[i]].x<<" "<<cloud->points[pointIndexNKNSearch[i]].y<<" "<<cloud->points[pointIndexNKNSearch[i]].z
                <<"(squrared distance:"<<pointNKNSquaredDistance[i]<<")"<<endl;
        }
    }
    //半径r内近邻搜索
    cout<<"半径r内近邻搜索：\n" ;
    vector<int> pointIdxRadiusSearch;
    vector<float> pointRadiusSquaredDistance;//搜索近邻对应的距离平方
    float radius=256.0f*rand()/(RAND_MAX+1.0f);
    if (kdtree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointNKNSquaredDistance)>0)
    {
        for (size_t i=0;i<pointIndexNKNSearch.size();++i)
        {
            cout<<" "<<cloud->points[pointIndexNKNSearch[i]].x<<" "<<cloud->points[pointIndexNKNSearch[i]].y<<" "<<cloud->points[pointIndexNKNSearch[i]].z<<
                "(square distance:"<<pointIndexNKNSearch[i]<<")"<<endl;
        }
    }
    system("pause") ;

}