pcl kdtree radiusSearch 半径搜索

knn搜索在这里pcl kdtree knn搜索-CSDN博客

建树跟knn搜索一样,搜索函数变了而已

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

搜索是radiusSearch

参数:第一个cloudIn->points[i]是搜索点,radius是搜索半径,out_ids是搜索半径内的点的索引,out_dists_square是距离平方

    for (int i = 0; i < cloud_size; i++) {
        vector<int> out_ids;
        vector<float> out_dists_square;
        if (kdtree.radiusSearch(cloudIn->points[i], radius, out_ids, out_dists_square) > 0) {
            //neighbors_dist_sqr.push_back(out_dists_square);
            //neighbors_ids.push_back(out_ids);
        }
    }

 下面是整个代码,因为要对比flann和nanoflann的建树,搜索的速度,我加了时间,可以删掉

    vector<vector<float> > neighbors_dist_sqr;记录所有点搜索半径内的邻居的距离平方
    vector<vector<int> > neighbors_ids;记录所有点搜索半径内的邻居的索引

只输出了前十个点的邻居的索引,值,距离的平方

void flann_radius_allneighbor(CP cloudIn, float radius) {
    pcl::console::TicToc time; time.tic();
    size_t cloud_size = cloudIn->size();


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

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

    vector<vector<float> > neighbors_dist_sqr;
    vector<vector<int> > neighbors_ids;
    for (int i = 0; i < cloud_size; i++) {
        vector<int> out_ids;
        vector<float> out_dists_square;
        if (kdtree.radiusSearch(cloudIn->points[i], radius, out_ids, out_dists_square) > 0) {
            neighbors_dist_sqr.push_back(out_dists_square);
            neighbors_ids.push_back(out_ids);
        }
    }

    for (int i = 0; i < 10; i++) {
        cout << cloudIn->points[i] << "有" << neighbors_dist_sqr[i].size() << "个符合条件的邻居:";
        for (int j = 0; j < neighbors_ids[i].size(); ++j) {
            cout << neighbors_ids[i][j] << " ";
            cout << cloudIn->points[neighbors_ids[i][j]] << " " << neighbors_dist_sqr[i][j] << endl;
        }
        cout << endl;
    }
    auto t2 = time.toc();
    //Myprint(cloudIn,all_neighbors, k);
    cout << "flann_radius :\t" << t2 << "ms" << endl;
    cout << "flann_radius 总时间:\t" << t1 + t2 << "ms" << endl;
    return;
}