#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <vector>
#include <ctime>
using namespace std;

int main()
{
    srand(time(NULL));
    //随机生成一个点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 1000;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.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);
    }
    //定义kdtree，相当于一个容器
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);
    //随机生成一个中心点
    pcl::PointXYZ keypoint;
    keypoint.x = 1024 * rand() / (RAND_MAX + 1.0f);
    keypoint.y = 1024 * rand() / (RAND_MAX + 1.0f);
    keypoint.z = 1024 * rand() / (RAND_MAX + 1.0f);
    //K近邻搜索，用到两个vector,一个存下标，一个存距离中心点的长度
    vector<int> KNsearch_idx;
    vector<float> KNsearch_dis;
    int K = 10;
    cout << "the K_nearestSearch at" << keypoint.x << "  " << keypoint.y << "  " << keypoint.z << endl;
    if (kdtree.nearestKSearch(keypoint, K, KNsearch_idx, KNsearch_dis) > 0)
    {
        for (size_t i = 0; i < KNsearch_idx.size(); i++)
        {
            cout << cloud->points[KNsearch_idx[i]].x << "  " << cloud->points[KNsearch_idx[i]].y << "  " << cloud->points[KNsearch_idx[i]].z << endl;
            cout << KNsearch_dis[i] << endl;
        }
    }
    //搜索半径R范围内的所有近邻
    vector<int> Rsearch_idx;
    vector<float> Rsearch_dis;
    int R = 256 * rand() / (RAND_MAX + 1.0f);
    cout << "In " << R << " field at " << keypoint.x << "  " << keypoint.y << "  " << keypoint.z << endl;
    if (kdtree.radiusSearch(keypoint, R, Rsearch_idx, Rsearch_dis) > 0)
    {
        for (size_t i = 0; i < Rsearch_idx.size(); i++)
        {
            cout << cloud->points[Rsearch_idx[i]].x << "  " << cloud->points[Rsearch_idx[i]].y << "  " << cloud->points[Rsearch_idx[i]].z << endl;
            cout << Rsearch_dis[i] << endl;
        }
    }
    return 0;
}