三种搜索方式：

  • 体素
  • K邻域
  • 搜索半径
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
70
71
72
73
74
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

#include <iostream>
#include <fstream>
#include <ctime>

int main(int argc, char** argv) { 
    
    srand((unsigned int)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 * 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);
    }

    // ---------------------------------------------------------------
    // 创建 octree
    float resolution = 128.0f; 
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud); // 设置输入点云
    octree.addPointsFromInputCloud(); // 添加点云数据到 octree

    pcl::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);

    // ----------------------------------------------------------------
    // 方法1： 体素搜索
    std::vector<int> pointIdxVec; // 存储搜索到的点的索引

    if (octree.voxelSearch(searchPoint,pointIdxVec)){
        std::cout << "Neightbors within voxel search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << "): " << pointIdxVec.size() << std::endl;
        for (size_t i = 0; i < pointIdxVec.size(); ++i){
            std::cout << "    " << cloud->points[pointIdxVec[i]].x << " " << cloud->points[pointIdxVec[i]].y << " " << cloud->points[pointIdxVec[i]].z << std::endl;
        }
    }

    // ----------------------------------------------------------------
    // 方法2： K近邻搜索
    std::vector<int> pointIdxNKNSearch; // 存储搜索到的点的索引
    std::vector<float> pointNKNSquaredDistance; // 存储搜索到的点的距离
    std::cout << "--------------------------------------------------------------------" << std::endl;
    std::cout<< "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << " ) with K=" << 10 << std::endl;
    if (octree.nearestKSearch(searchPoint,10,pointIdxNKNSearch,pointNKNSquaredDistance) > 0){
        for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){
            std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y << " " << cloud->points[pointIdxNKNSearch[i]].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        }
}

    // ----------------------------------------------------------------
    // 方法3： 半径内搜索
    std::vector<int> pointIdxRadiusSearch; // 存储搜索到的点的索引
    std::vector<float> pointRadiusSquaredDistance; // 存储搜索到的点的距离

    float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
    std::cout << "--------------------------------------------------------------------" << std::endl;
    std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with radius=" << radius << std::endl;
    if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0){
        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i){
            std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
        }
}
    return 0; 
    }