1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
#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)); // 用系统时间初始化
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 随机初始化点云
    cloud->width = 1000;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);

    // 循环填充点云
    for (size_t i=0; i < cloud->points.size(); ++i){
        cloud->points[i].x = 1024.0 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0 * rand() / (RAND_MAX + 1.0f);
    }

    // 创建KdTreeFLANN对象，并把创建的点云设置为输入，创建一个searchPoint变量作为查询点
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);

    // 设置查寻点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 1024.0 * rand() / (RAND_MAX + 1.0f);
    searchPoint.y = 1024.0 * rand() / (RAND_MAX + 1.0f);
    searchPoint.z = 1024.0 * rand() / (RAND_MAX + 1.0f);

    // -------------------------------------------------------------------------------------------
    // K 近邻搜索
    int k=10; // 查询10近邻
    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;

    if (kdtree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){
        std::cout << "idx\t" << "x\t" << "y\t" << "z\t" << " (squared distance)" << std::endl;
        for (size_t i=0; i < pointIdxNKNSearch.size(); ++i){
            std::cout << i << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        }
    }

    // -------------------------------------------------------------------------------------------
    // 半径内搜索
    std::vector<int> pointIdxRadiusSearch; // 存储近邻索引
    std::vector<float> pointRadiusSquaredDistance; // 存储近邻距离

    float radius = 256.0f * rand() / (RAND_MAX + 1.0f); // 随机生成半径
    std::cout<<"--------------------------------------------------------"<<std::endl;
    std::cout<<"nearest neighbor within radius search at ("<<searchPoint.x<<","<<searchPoint.y<<","<<searchPoint.z<<") with radius="<<radius<<std::endl;

    if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0){
        std::cout << "idx\t" << "x\t" << "y\t" << "z\t" << " (squared distance)" << std::endl;
        for (size_t i=0; i < pointIdxRadiusSearch.size(); ++i){
            std::cout << i << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;    
        }
    }


    return 0;
    
}

CMakeLists.txt

1
2
3
4
5
6
7
8
9
10
11
12
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(kdtree_search)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (kdtree_search main.cpp)
target_link_libraries (kdtree_search ${PCL_LIBRARIES})

20240725175751