PCL从理解到应用【04】Octree 原理分析 | 案例分析

前言

Octree 作为一种高效的空间分割数据结构，具有重要的应用价值。

本文将深入分析 Octree 的原理，通过多个实际案例帮助读者全面理解其功能和应用，包括最近邻搜索、半径搜索、盒子搜索以及点云压缩（体素化）。

特性	近邻搜索	半径搜索	盒子搜索	点云压缩（体素化）
描述	查找距离给定点最近的一个或多个点	查找给定点一定半径范围内的所有点	查找给定空间盒子内的所有点	将点云数据划分为均匀大小的立方体（体素）
输入参数	目标点，近邻数量	目标点，半径	盒子的最小点和最大点	分辨率
输出	最近的一个或多个点的索引及距离	半径范围内所有点的索引及距离	盒子内所有点的索引	每个体素的中心点
适用场景	最近点查询，碰撞检测	局部邻域分析，聚类	空间范围查询，目标检测	数据降采样，减少计算复杂度
优点	精确查找最近点，效率高	查找范围可调节，适用于局部分析	可以查找任意形状的空间范围内的点	大幅度减少数据量，保留数据空间结构
缺点	仅限于查找最近的点，无法指定查找范围	结果集大小随半径变化，计算量可能较大	需要预先定义盒子范围，结果集大小不确定	可能丢失部分细节信息
实现方法	`octree.nearestKSearch`	`octree.radiusSearch`	`octree.boxSearch`	`octree.getOccupiedVoxelCenters`

此外，本文还提供了详细的源代码示例，便于读者实践和应用。

希望通过本文的学习，读者能够掌握 Octree 的基本原理及其在点云数据处理中的具体实现方法。

看一下示例效果：

红色点表示搜索点，绿色点表示最近邻点，白色点表示原始点云数据。

一、Octree原理分析

Octree是一种用于分层分割三维空间的数据结构。它是将三维空间递归地划分成更小的立方体区域的树形结构。

每个节点代表一个空间区域，该区域可以进一步细分为八个子区域。

Octree的原理是基于递归地将三维空间，分割成更小的立方体区域，从而形成一棵树形结构。

这种结构能够有效地表示和操作三维空间中的数据，如下图所示：

特点：

分层结构：Octree的每个节点对应一个立方体区域，当需要更详细的信息时，这些区域可以进一步划分成八个子区域。
递归分割：通过递归地将空间划分为更小的部分，Octree可以有效地表示三维空间中的数据。
适应性强：Octree可以灵活地适应不同密度的数据区域，在数据稀疏的区域使用较少的节点，而在数据密集的区域使用更多的节点。

用途：

三维计算机图形学：在图形渲染中，Octree用于加速光线追踪算法，通过快速确定光线与物体的交点。
碰撞检测：在物理模拟和游戏开发中，Octree用于高效的碰撞检测，特别是当物体分布在三维空间中的时候。
空间索引：在三维空间数据管理中，Octree用于空间索引和快速查询，例如在地理信息系统(GIS)中。
点云处理：在处理和表示三维扫描数据时，Octree可以有效地存储和操作点云数据。

KDTree与Octree进行对比分析，如下图所示：

特点	KDTree	Octree
适用维度	k维	三维
划分方式	划分超平面	划分立方体
结构类型	二叉树	八叉树
构建时间	O(n log n)	O(n log n)
查询效率	平均O(log n)，最坏情况O(n)	平均O(log n)，最坏情况O(n)
优势	通用性强，高效查询任意维度点集	适用于三维空间，高效处理三维点云数据
缺点	高维效率下降，维度灾难	仅适用于三维空间，可能需要较多内存和计算资源

二、Octree常用方法

PCL中的Octree提供了对点云数据进行分割、索引和查询的功能。

通过Octree，可以高效地实现点云的邻域搜索、体素化、下采样等操作。

常用类：

pcl::octree::OctreePointCloud（管理和操作三维点云数据）
pcl::octree::OctreePointCloudSearch（点云中进行查询，搜索功能）
pcl::octree::OctreePointCloudVoxel（点云数据进行体素化处理）

1）pcl::octree::OctreePointCloud

它是Octree数据结构的基本实现，专用于管理和操作三维点云数据。

提供基本的Octree构建和管理功能。
支持添加、删除和更新点云数据。
可以进行点云数据的空间划分和组织。

常用方法：

setInputCloud：设置输入点云。
addPointsFromInputCloud：从输入点云中添加点到Octree。
deleteVoxelAtPoint：删除指定点所在的体素。

示例代码：

// 创建Octree对象，并指定分辨率为0.1
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.1);

// 设置输入点云
octree.setInputCloud(cloud);

// 构建Octree
octree.addPointsFromInputCloud();

2）pcl::octree::OctreePointCloudSearch

它继承自OctreePointCloud，增加了搜索功能，用于高效地在点云中进行查询操作。

在Octree的基础上，增加了搜索功能。
支持各种类型的查询，如最近邻搜索、半径搜索、盒子搜索。

常用方法：

nearestKSearch：查找最近的K个邻居。
radiusSearch：查找给定半径内的所有点。
boxSearch：查找指定盒子区域内的所有点。

示例代码：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.1f); // 创建Octree对象，分辨率为0.1

octree.setInputCloud(cloud); // 设置输入点云
octree.addPointsFromInputCloud(); // 构建Octree

// 定义要查找的点
pcl::PointXYZ searchPoint;
std::vector<int> pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;
int K = 10;

// 最近邻搜索
octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);

3）pcl::octree::OctreePointCloudVoxel

它继承自OctreePointCloud，增加了体素化功能，用于将点云数据进行体素化处理。

在Octree的基础上，增加了体素化功能。
能够计算每个体素的中心点，用于下采样和数据压缩。

常用方法：

addPointsFromInputCloud：从输入点云中添加点到Octree并进行体素化处理。
getVoxelCentroids：获取所有体素的中心点。

示例代码：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::octree::OctreePointCloudVoxel<pcl::PointXYZ> octree(0.1f); // 创建Octree对象，分辨率为0.1

octree.setInputCloud(cloud); // 设置输入点云
octree.addPointsFromInputCloud(); // 构建Octree并进行体素化

// 获取体素中心点
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> voxelCentroids;
octree.getVoxelCentroids(voxelCentroids);

for (const auto& centroid : voxelCentroids)
{
    std::cout << "体素中心点: " << centroid.transpose() << std::endl;
}

这些类提供了在PCL中使用Octree进行点云数据管理和查询的基本功能，并能够高效地处理三维点云数据。

三、Octree案例——最近邻搜索

编写C++代码，调用pcl库实现Octree的最近邻搜索，通过cmake方式编译程序，最后可视化结果。

目录结构：

OctreeNearestNeighbor
├── CMakeLists.txt
├── src
│   └── main.cpp
└── build

在项目根目录下创建CMakeLists.txt文件：

cmake_minimum_required(VERSION 3.10)
project(OctreeNearestNeighbor)

# 查找PCL库
find_package(PCL 1.9 REQUIRED)

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

# 编译可执行文件
add_executable(OctreeNearestNeighbor src/main.cpp)
target_link_libraries(OctreeNearestNeighbor ${PCL_LIBRARIES})

src/main.cpp, 在src目录下创建main.cpp文件，并添加以下代码：

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <cstdlib> 
#include <ctime>  

int main()
{
    // 初始化随机数生成器
    std::srand(static_cast<unsigned int>(std::time(0)));

    // 创建一个PointXYZ点云指针并填充点云数据
    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.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 定义Octree分辨率并创建Octree对象
    float resolution = 128.0f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

    // 设置输入点云并构建Octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    // 定义要查找的点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
    searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
    searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    std::cout << "要查找的点: (" << searchPoint.x << ", " << searchPoint.y << ", " << searchPoint.z << ")" << std::endl;

    // 查找最近的10个邻居
    int K = 10;
    std::vector<int> pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;

    std::cout << "搜索最近邻点..." << std::endl;
    if (octree.nearestKSearch(searchPoint, K, 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
                      << " (平方距离: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        }
    }

    // 创建PCLVisualizer对象进行可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");

    // 可视化搜索点
    pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
    searchPointCloud->points.push_back(searchPoint);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(searchPointCloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(searchPointCloud, red, "search point");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search point");

    // 可视化最近邻点
    pcl::PointCloud<pcl::PointXYZ>::Ptr neighborCloud(new pcl::PointCloud<pcl::PointXYZ>());
    for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
    {
        neighborCloud->points.push_back(cloud->points[pointIdxNKNSearch[i]]);
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green(neighborCloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(neighborCloud, green, "neighbor points");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "neighbor points");

    // 开始主循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(50); // 每50毫秒刷新一次显示，并处理用户事件
    }

    return 0;
}

上述代码首先生成一个随机的点云数据集。
使用OctreePointCloudSearch类创建Octree，并进行最近邻搜索。
通过PCLVisualizer将点云数据、搜索点以及最近邻点可视化。

编译和运行

1）创建一个build目录，并进入该目录：

mkdir build
cd build

2）运行CMake以生成Makefile：

cmake ..

3）编译项目：

make

4）运行可执行文件：

./OctreeNearestNeighbor

看看可视化结果，如下图所示：

红色点表示搜索点，绿色点表示最近邻点，白色点表示原始点云数据。

输出结果信息：

要查找的点: (267.189, 466.127, 142.194)
搜索最近邻点...
225.61 412.933 142.412 (平方距离: 4558.5)
257.808 410.918 183.467 (平方距离: 4839.53)
274 460.999 211.445 (平方距离: 4868.51)
243.09 540.606 131.491 (平方距离: 6242.47)
275.009 466.096 229.258 (平方距离: 7641.46)
240.711 511.06 217.932 (平方距离: 8456.47)
174.914 476.775 108.244 (平方距离: 9780.69)
178.43 475.309 188.146 (平方距离: 10074.1)
203.922 489.412 219.492 (平方距离: 10520)
343.789 480.705 208.93 (平方距离: 10533.8)

四、Octree案例——半径搜索

半径搜索的代码思路：

首先生成一个随机的点云数据集。
使用OctreePointCloudSearch类创建Octree，并进行半径搜索。
通过PCLVisualizer将点云数据、搜索点以及半径内的邻点可视化。
红色点表示搜索点，绿色点表示半径内的邻点，白色点表示原始点云数据。

核心的代码，如下所示：

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <cstdlib> 
#include <ctime>   

int main()
{
    // 初始化随机数生成器
    std::srand(static_cast<unsigned int>(std::time(0)));

    // 创建一个PointXYZ点云指针并填充点云数据
    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.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 定义Octree分辨率并创建Octree对象
    float resolution = 128.0f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

    // 设置输入点云并构建Octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    // 定义要查找的点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
    searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
    searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    // 打印 searchPoint 的值
    std::cout << "Search Point Coordinates: (" 
              << searchPoint.x << ", " 
              << searchPoint.y << ", " 
              << searchPoint.z << ")" << std::endl;

    // 查找给定半径内的所有点
    float radius = 256.0f;
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    std::cout << "搜索半径内的邻点..." << 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
                      << " (平方距离: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
        }
    }

    // 创建PCLVisualizer对象进行可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");

    // 可视化搜索点
    pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
    searchPointCloud->points.push_back(searchPoint);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(searchPointCloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(searchPointCloud, red, "search point");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search point");

    // 可视化半径内的邻点
    pcl::PointCloud<pcl::PointXYZ>::Ptr neighborCloud(new pcl::PointCloud<pcl::PointXYZ>());
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
    {
        neighborCloud->points.push_back(cloud->points[pointIdxRadiusSearch[i]]);
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green(neighborCloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(neighborCloud, green, "neighbor points");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "neighbor points");

    // 开始主循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 每100毫秒刷新一次显示，并处理用户事件
    }

    return 0;
}

看看可视化结果，如下图所示：

红色点表示搜索点，绿色点表示最近邻点，白色点表示原始点云数据。

输出结果信息：

Search Point Coordinates: (672.53, 848.881, 756.348)
搜索半径内的邻点...
    563.58 637.83 689.303 (平方距离: 60907.5)
    536.462 790.479 550.964 (平方距离: 64107.5)
    540.754 715.457 739.671 (平方距离: 35445.1)
    500.653 835.636 724.763 (平方距离: 30714.7)
    578.384 833.712 832.827 (平方距离: 14942.7)
    548.618 856.429 914.777 (平方距离: 40510.9)
    527.55 796.035 880.061 (平方距离: 39116.9)
    449.564 904.382 649.582 (平方距离: 64192.9)
    499.839 872.314 616.479 (平方距离: 49934.4)
    487.801 908.75 908.802 (平方距离: 60951.5)
    546.375 1016.81 859.397 (平方距离: 54735.8)
    636.812 604.273 751.585 (平方距离: 61131.5)
    659.817 739.358 680.051 (平方距离: 17978.1)
    652.143 649.493 668.577 (平方距离: 47874.9)
    659.318 757.01 681.943 (平方距离: 14150.9)
    834.803 730.708 613.067 (平方距离: 60826.7)
    777.22 699.299 659.459 (平方距离: 42722.2)
    781.315 696.036 643.011 (平方距离: 48040.7)
    598.679 665.372 752.981 (平方距离: 39140.7)
    620.251 672.501 777.007 (平方距离: 34269.7)
    642.869 713.274 783.507 (平方距离: 20006.7)
    672.395 668.634 835.468 (平方距离: 38748.9)
    671.215 861.606 710.117 (平方距离: 2300.95)
    622.279 757.322 709.815 (平方距离: 13073.6)
    658.69 820.696 730.139 (平方距离: 1672.82)
    604.35 823.723 742.655 (平方距离: 5468.91)
    661.581 819.162 861.857 (平方距离: 12135.4)
    768.492 663.652 782.472 (平方距离: 44201)
    773.832 660.906 725.373 (平方距离: 46555.8)
    723.146 708.955 721.807 (平方距离: 23334.2)
    808.975 731.02 721.804 (平方距离: 33701.7)
    745.785 703.49 901.843 (平方距离: 47673.5)
    778.82 710.248 922.674 (平方距离: 58181.1)
    798.036 736.621 857.316 (平方距离: 38548.7)
    740.366 712.4 891.645 (平方距离: 41534)
    782.45 746.209 816.893 (平方距离: 26289.7)
    764.26 821.114 801.76 (平方距离: 11247.8)
    601.229 914.836 544.503 (平方距离: 54312.4)
    648.524 962.854 701.196 (平方距离: 16607.9)
    653.201 956.799 618.907 (平方距离: 30909.9)
    734.031 892.564 565.114 (平方距离: 42260.7)
    720.409 943.268 534.037 (平方距离: 60623.3)
    758.06 870.037 644.098 (平方距离: 20362.8)
    800.157 963.6 689.941 (平方距离: 33858.9)
    656.089 916.227 956.117 (平方距离: 44713.5)
    674.688 983.219 959.94 (平方距离: 59501.3)
    665.463 919.025 857.996 (平方距离: 15302.6)
    774.945 993.878 800.679 (平方距离: 33478.2)
    789.017 945.61 886.762 (平方距离: 39933.5)
    792.737 1010.02 883.729 (平方距离: 56642.8)
    860.811 843.444 639.952 (平方距离: 49027.2)
    878.83 751.169 714.356 (平方距离: 53870.4)
    855.926 1006.99 694.897 (平方距离: 62407.9)
    920.233 897.31 750.461 (平方距离: 63736.7)
    903.325 913.274 828.116 (平方距离: 62563.3)
    618.377 802.345 964.152 (平方距离: 48280.7)
    705.442 855.34 963.439 (平方距离: 44011.6)
    718.328 888.42 1005 (平方距离: 65489.4)

五、Octree案例——盒子搜索

盒子搜索（Box Search）是一种在 Octree 中查找指定三维范围内所有点的方法。

该方法定义了一个盒子，指定其最小和最大点坐标，然后在这个盒子内查找所有落在该范围内的点。

思路逻辑：

生成了一个包含 1000 个随机点的点云。
使用 OctreePointCloudSearch 创建了一个八叉树，并将点云数据添加到其中。
定义了一个搜索盒子的最小和最大点（minPoint 和 maxPoint），并使用 boxSearch 方法在盒子内查找点。
将搜索到的点打印到控制台。
使用 PCLVisualizer 可视化整个点云和搜索到的点。

核心的代码，如下所示：

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <Eigen/Dense> // 包含 Eigen 库

int main()
{
    // 初始化随机数生成器
    std::srand(static_cast<unsigned int>(std::time(0)));

    // 创建一个PointXYZ点云指针并填充点云数据
    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.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 定义Octree分辨率并创建Octree对象
    float resolution = 128.0f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

    // 设置输入点云并构建Octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    // 定义搜索盒子的边界
    Eigen::Vector3f minPoint;
    Eigen::Vector3f maxPoint;
    minPoint[0] = 256.0f;
    minPoint[1] = 256.0f;
    minPoint[2] = 256.0f;
    maxPoint[0] = 768.0f;
    maxPoint[1] = 768.0f;
    maxPoint[2] = 768.0f;

    // 执行盒子搜索
    std::vector<int> pointIdxBoxSearch;
    if (octree.boxSearch(minPoint, maxPoint, pointIdxBoxSearch) > 0)
    {
        std::cout << "找到 " << pointIdxBoxSearch.size() << " 个点在盒子内:" << std::endl;
        for (size_t i = 0; i < pointIdxBoxSearch.size(); ++i)
        {
            std::cout << "    " << cloud->points[pointIdxBoxSearch[i]].x
                      << " " << cloud->points[pointIdxBoxSearch[i]].y
                      << " " << cloud->points[pointIdxBoxSearch[i]].z << std::endl;
        }
    }
    else
    {
        std::cout << "未找到任何点在盒子内。" << std::endl;
    }

    // 创建PCLVisualizer对象进行可视化
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");

    // 可视化盒子内的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr boxCloud(new pcl::PointCloud<pcl::PointXYZ>());
    for (size_t i = 0; i < pointIdxBoxSearch.size(); ++i)
    {
        boxCloud->points.push_back(cloud->points[pointIdxBoxSearch[i]]);
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green(boxCloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(boxCloud, green, "box points");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "box points");

    // 开始主循环
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 每100毫秒刷新一次显示，并处理用户事件
    }

    return 0;
}

看看可视化结果，如下图所示：

绿色点表示最近邻点，白色点表示原始点云数据。

输出结果信息：

找到 129 个点在盒子内:
305.738 283.948 366.022
390.078 263.586 358.944
498.589 369.01 290.586
418.491 412.725 313.189
435.588 318.347 467.745
404.428 418.288 374.571
485.747 368.239 418.217

............

六、Octree案例——点云压缩-体素化

基于 Octree 的体素化（Voxelization）是一种将点云数据划分成均匀大小的立方体（体素）的技术。

每个体素可以包含一个或多个点，通常只保留体素内的一个代表点来简化数据，从而实现降采样和加速后续处理。

思路流程：

生成点云数据：生成一个包含 1000 个随机点的点云数据。
构建 Octree：设置 Octree 的分辨率，并将点云数据添加到 Octree 中。
体素化处理：调用 octree.getOccupiedVoxelCenters 方法获取所有体素的中心点，并将这些点存储在 voxelizedCloud 中。这个方法会返回每个占据的体素的中心点，实现体素化效果。
可视化：使用 PCLVisualizer 同时可视化原始点云和体素化后的点云。原始点云用白色显示，体素化后的点云用绿色显示，以便进行对比。

核心的代码，如下所示：

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <cstdlib>
#include <ctime>

int main()
{
    // 初始化随机数生成器
    std::srand(static_cast<unsigned int>(std::time(0)));

    // 创建一个PointXYZ点云指针并填充点云数据
    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.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 定义Octree分辨率并创建Octree对象
    float resolution = 128.0f;
    pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(resolution);

    // 设置输入点云并构建Octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    // 体素化处理：获取所有体素的中心点
    pcl::PointCloud<pcl::PointXYZ>::Ptr voxelizedCloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::octree::OctreePointCloud<pcl::PointXYZ>::AlignedPointTVector voxelCenters;
    octree.getOccupiedVoxelCenters(voxelCenters);

    for (const auto& voxelCenter : voxelCenters)
    {
        voxelizedCloud->points.push_back(voxelCenter);
    }

    std::cout << "原始点云大小: " << cloud->points.size() << std::endl;
    std::cout << "体素化后点云大小: " << voxelizedCloud->points.size() << std::endl;

    // 创建PCLVisualizer对象进行可视化
    pcl::visualization::PCLVisualizer::Ptr viewerOriginal(new pcl::visualization::PCLVisualizer("Original Cloud Viewer"));
    viewerOriginal->setBackgroundColor(0, 0, 0);

    pcl::visualization::PCLVisualizer::Ptr viewerVoxelized(new pcl::visualization::PCLVisualizer("Voxelized Cloud Viewer"));
    viewerVoxelized->setBackgroundColor(0, 0, 0);

    // 可视化原始点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originalColor(cloud, 255, 255, 255);
    viewerOriginal->addPointCloud<pcl::PointXYZ>(cloud, originalColor, "original cloud");
    viewerOriginal->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original cloud");

    // 可视化体素化后的点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> voxelColor(voxelizedCloud, 0, 255, 0);
    viewerVoxelized->addPointCloud<pcl::PointXYZ>(voxelizedCloud, voxelColor, "voxelized cloud");
    viewerVoxelized->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "voxelized cloud");

    // 开始主循环
    while (!viewerOriginal->wasStopped() && !viewerVoxelized->wasStopped())
    {
        viewerOriginal->spinOnce(100);  // 每100毫秒刷新一次显示，并处理用户事件
        viewerVoxelized->spinOnce(100);  // 每100毫秒刷新一次显示，并处理用户事件
    }

    return 0;
}

看看可视化结果，如下图所示：