pcl kdtree knn搜索

pcl kdtree radiusSearch半径搜索在这里 pcl kdtree radiusSearch 半径搜索-CSDN博客

kdtree原理:等以后闲了再说

knn,k=10,查找所有点的十个最近邻

建树

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloudIn);

搜索

out_ids记录当前搜索点cloudIn->points[i]的最近的十个点的索引

out_dists_square则记录着十个点到搜索点的距离平方,这里是平方!

for (int i = 0; i < cloud_size; i++) {
    vector<int> out_ids(k);
    vector<float> out_dists_square(k);
    if (kdtree.nearestKSearch(cloudIn->points[i], k, out_ids, out_dists_square) > 0) {
        /*for (size_t j = 0; j < k; j++) {
                all_neighbors[i][j] = out_ids[j];
         }*/
    }
}

void flann_knn_allneighbor_openmp(CP cloudIn, int k) {
    pcl::console::TicToc time; time.tic();
    size_t cloud_size = cloudIn->size();

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloudIn);
    //vector<vector<int>> all_neighbors(cloud_size, vector<int>(k));

    auto t1 = time.toc();
    cout << "flann+openmp建树的时间:" << t1 << "ms" << endl;;
    time.tic();


    #pragma omp parallel for
    for (int i = 0; i < cloud_size; i++) {

        vector<int> out_ids(k);
        vector<float> out_dists_square(k);
        if (kdtree.nearestKSearch(cloudIn->points[i], k, out_ids, out_dists_square) > 0) {
            /*for (size_t j = 0; j < k; j++) {
                all_neighbors[i][j] = out_ids[j];
            }*/
        }
    }
    //Myprint(cloudIn,all_neighbors, k);
    auto t2 = time.toc();
    cout << "flann+openmp KNN时间:" << t2 << "ms" << endl;
    cout << "flann+openmp 总时间:" << t2 + t1 << "ms" << endl;
    return;
}

把所有注释取消,输出一下.

void Myprint(CP cloudIn,vector<vector<int> > nums, int k) {
    for (int i = 0; i < nums.size(); i++) {
        cout << cloudIn->points[i] << " 的最近的" << k << "个点是:";
        for (int j = 0; j < k; j++) {
            cout << cloudIn->points[nums[i][j]] << " ";
        }cout<<endl;
    }
    return;
}

这里可以看到,最近的十个点里,第一个点事自己(自己离自己最近)