PCL KD树的使用

KD树的全称是k维树（k-Dimensional Tree），它是一种二叉树，用于对k维空间中的点进行组织，以便于快速地进行点的查询操作（如最近邻搜索）。KD树的基本思想是通过递归地将数据空间划分为两个部分，从而形成一棵二叉树。每个节点代表一个k维超矩形，划分过程直到节点包含的数据点数目低于某个阈值或不能再划分为止。

1.1.1 数据拆分过程

1.选择维度：
   - 在构建KD树时，选择一个维度来划分数据空间。通常，使用循环选择的方法，即在每一层递归中，依次选择数据的不同维度。例如，在三维空间（k=3）中，第一层选择x维度，第二层选择y维度，第三层选择z维度，第四层再回到x维度，以此类推。
2.选择分割点：
   - 在选定的维度上，通过找到数据在该维度上的中位数，将数据空间划分为两个部分。中位数的选择保证了数据点被尽量均匀地分割开。
3.递归构建：
   - 将数据空间划分后，左子树包含小于或等于中位数的数据点，右子树包含大于中位数的数据点。递归地对每个子空间构建KD树，直到满足终止条件（如节点数据点数目小于某个阈值）。

1.1.2 树的构建示例

假设我们有一组二维点 (x, y)，要将其构建为一棵KD树：

第一层（根节点）：选择x维度进行划分，找到x维度的中位数，作为根节点的分割点。左子树包含所有x值小于或等于中位数的点，右子树包含所有x值大于中位数的点。
第二层：对左子树和右子树，选择y维度进行划分，找到y维度的中位数，分别作为左右子树的分割点。继续递归划分。
第三层：再回到x维度进行划分，依次类推，直到不能再划分为止。

1.2实现步骤

        KD树的搜索过程通常是以递归的方式进行的，主要分为最近邻搜索和范围搜索两种常见的操作。
1.最近邻搜索：
   - 从根节点开始，沿树向下递归搜索，比较查询点与节点的分割点，决定搜索左子树还是右子树。
   - 在找到一个叶节点后，记录下该叶节点的点，并计算它与查询点的距离。
   - 回溯检查其他可能包含更近点的子树。
2.范围搜索：
   - 同样从根节点开始，根据分割维度的值，判断是否需要搜索左右子树。
   - 判断当前节点的分割点是否在查询范围内，如果是，则将其纳入结果集。
   - 继续递归地检查子树，直到所有符合条件的节点都被搜索到。
通过这种分割和搜索方法，KD树能够有效地减少高维空间中的搜索范围，从而加快查询速度。

1.3应用场景

KD树广泛应用于以下场景：

最近邻搜索：用于查找点云中距离某点最近的邻居点，常用于点云配准。
k近邻搜索：用于查找点云中某点的k个最近邻点，适用于点云平滑、特征计算等。
范围搜索：用于查找在一定范围内的所有邻居点，适用于聚类、边界检测等。

二、代码实现

2.1关键函数

2.1.1KD树构建与查询：

pcl::KdTreeFLANN：用于创建KD树对象，支持最近邻、k近邻、范围搜索。
setInputCloud：将点云数据输入到KD树中进行构建。
nearestKSearch：执行k近邻搜索，找到指定点的k个最近邻点。
radiusSearch：执行范围搜索，找到指定点在给定半径内的所有邻居点。

2.1.2 k近邻搜索

int pcl::KdTreeFLANN< PointT, Dist >::nearestKSearch  ( const PointT &  point,  
  unsigned int  k,  
  Indices &  k_indices,  
  std::vector< float > &  k_sqr_distances  
 )  const

2.1.3半径搜索

int pcl::KdTreeFLANN< PointT, Dist >::radiusSearch  ( const PointT &  point,  
  double  radius,  
  Indices &  k_indices,  
  std::vector< float > &  k_sqr_distances,  
  unsigned int  max_nn = 0  
 )  const

2.2完整代码

#include <pcl/io/pcd_io.h>  // 用于加载PCD文件
#include <pcl/point_types.h>  // PCL点类型定义
#include <pcl/kdtree/kdtree_flann.h>  // PCL中的KD树实现
#include <pcl/visualization/pcl_visualizer.h>  // 用于可视化点云
#include <vector>
#include <iostream>

// 将搜索到的结果渲染为红色，并在可视化窗口中显示
void visualizeResult(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::vector<int>& indices)
{
    // 创建一个带颜色信息的点云对象，用于存储带有颜色信息的点云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    // 遍历原始点云中的每个点
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        // 创建一个带有RGB颜色信息的点对象
        pcl::PointXYZRGB point;
        point.x = cloud->points[i].x;  // 复制点的X坐标
        point.y = cloud->points[i].y;  // 复制点的Y坐标
        point.z = cloud->points[i].z;  // 复制点的Z坐标

        // 判断当前点是否在搜索到的索引列表中
        if (std::find(indices.begin(), indices.end(), i) != indices.end())
        {
            // 如果是搜索到的点，将颜色设置为红色
            point.r = 255;
            point.g = 0;
            point.b = 0;
        }
        else
        {
            // 如果不是搜索到的点，将颜色设置为白色
            point.r = 255;
            point.g = 255;
            point.b = 255;
        }

        // 将带颜色的点添加到新的点云对象中
        coloredCloud->points.push_back(point);
    }

    // 设置点云的基本属性
    coloredCloud->width = coloredCloud->points.size();  // 设置点云宽度为点的数量
    coloredCloud->height = 1;  // 设置点云高度为1（无序点云）
    coloredCloud->is_dense = true;  // 设置点云为稠密点云

    // 创建一个PCLVisualizer对象，用于显示带颜色的点云
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色
    viewer->addPointCloud<pcl::PointXYZRGB>(coloredCloud, "result");  // 将带颜色的点云添加到可视化窗口
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "result");  // 设置点的大小

    // 启动可视化主循环，保持窗口打开直到用户关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);  // 刷新窗口，间隔100ms
    }
}

int main(int argc, char** argv)
{
    // 1. 创建PointCloud对象，用于存储点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 2. 读取点云数据，加载PCD文件
    if (pcl::io::loadPCDFile("bunny.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file input.pcd \n");  // 如果文件加载失败，输出错误信息
        return -1;  // 返回错误代码并退出程序
    }

    // 3. 创建KD树对象，并将点云数据输入到KD树中进行构建
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  // 创建KD树对象
    kdtree.setInputCloud(cloud);  // 将点云数据设置为KD树的输入

    // 4. 定义一个查询点，用于进行搜索
    pcl::PointXYZ searchPoint(-0.0208042, 0.125974, 0.0209606);  // 查询点的坐标

    // 5. k近邻搜索（K Nearest Neighbor Search）
    int k = 1000;  // 设置k值，表示要查找的最近邻点的数量
    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;

    // 使用KD树进行k近邻搜索，找到距离查询点最近的k个点
    if (kdtree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        // 如果搜索成功，将搜索结果进行可视化
        visualizeResult(cloud, pointIdxNKNSearch);
    }

    // 6. 半径搜索（Radius Search）
    float radius = 0.03;  // 设置搜索半径
    std::vector<int> pointIdxRadiusSearch;  // 用于存储在指定半径范围内的点的索引
    std::vector<float> pointRadiusSquaredDistance;  // 用于存储在指定半径范围内的点的距离平方值

    std::cout << "Neighbors within radius " << radius << " around point ("
        << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl;

    // 使用KD树进行半径搜索，找到查询点在给定半径范围内的所有点
    if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
        // 如果搜索成功，将搜索结果进行可视化
        visualizeResult(cloud, pointIdxRadiusSearch);
    }

    // 7. 混合搜索（k近邻搜索 + 半径搜索）
    std::vector<int> hybridSearchIndices;  // 存储混合搜索结果的索引
    for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
    {
        // 判断k近邻搜索的点是否也在指定半径范围内
        if (pointNKNSquaredDistance[i] <= radius * radius)
        {
            hybridSearchIndices.push_back(pointIdxNKNSearch[i]);  // 如果满足条件，将索引添加到结果中
        }
    }

    std::cout << "Hybrid search results within K=" << k << " and radius " << radius << std::endl;

    // 如果混合搜索结果不为空，将结果进行可视化
    if (!hybridSearchIndices.empty())
    {
        visualizeResult(cloud, hybridSearchIndices);
    }

    return 0;
}

三、实现效果

3.1处理后点云

3.2数据显示

K nearest neighbor search at (-0.0208042 0.125974 0.0209606) with K=1000
Neighbors within radius 0.03 around point (-0.0208042 0.125974 0.0209606)
Hybrid search results within K=1000 and radius 0.03

原文地址：https://blog.csdn.net/qq_47947920/article/details/142383221

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：Java面试篇基础部分-Java语言中的锁有哪些？
下一篇：企业搭建VR虚拟展厅，如何选择搭建平台?

计算机网络笔记001
通信网络研究的是通信终端（如电话）和内部通信问题，而计算机网络则更关注计算机之间的联网和通信，比如我们的Wi-Fi网络。分布式系统是一种建立在计算机网络之上的、具有高度内聚性 ( Cohesiven
阅读更多2024-09-23
第七章监听器
监听器的作用是被观察的对象发生某些情况时，自动触发代码的执行。监听器时GOF设计模式中，观察者模式的典型案例。观察者模式: 当被观察的对象发生某些改变时, 观察者自动采取对应的行动的一种设计模式。在J
阅读更多2024-09-23
Qemu开发ARM篇-3、qemu运行uboot演示
在上一篇文章中，我们搭建了交叉编译工具链，并成功进行了uboot的交叉编译，在该篇中，我们将演示如何利用qemu运行上一篇中交叉编译的uboot程序。
阅读更多2024-09-23
MATLAB基于传统方法的车道线检测实现
在Canny算法中，非极大值抑制是进行边缘检测的重要步骤，简单说就是寻找像素点局部最大值，将非极大值点所对应的灰度值置为0，这样可以剔除掉许多非边缘的点，我们在霍夫空间中定义交点的最小阈值来检测线，霍
阅读更多2024-09-23
Qt信号和槽
参考：【Qt】信号和槽
阅读更多2024-09-23
docker build前耗时太长，不明所以
在构建镜像时发现明明没基础镜像没那么大，却一直在加载什么东西，直至到了一个点，搜索发现是因为Docker Client会默认发送Dockerfile同级目录下的所有文件到Dockerdaemon中，是
阅读更多2024-09-23
【C#生态园】从容面对.NET性能挑战：全面解析多种性能监控工具
本文将围绕六个.NET性能监控工具展开讨论，分别是MiniProfiler、Glimpse、MiniProfiler扩展库：MiniProfiler.EF、Glimpse扩展库：Glimpse.Ado
阅读更多2024-09-23
机器学习查漏补缺(4)
[M] What happens if we accidentally duplicate every data point in your train set or in your test set
阅读更多2024-09-23
软媒市场新探索：软文媒体自助发布，开启自助发稿新篇章
这种全新的营销方式,不仅极大地提高了企业的营销效率,还降低了企业的营销成本,让数字营销变得更加简单和高效。无论企业的目标受众是谁,无论企业的营销需求是什么,都能在这个平台上找到合适的媒体进行投放,实现
阅读更多2024-09-23
【AI创作组】Matlab简介
MATLAB是一种高性能的科学计算语言，广泛应用于工程计算、控制设计、信号处理和图形显示等领域。
阅读更多2024-09-23

PCL KD树的使用

一、概述

1.1原理

1.1.1 数据拆分过程

1.1.2 树的构建示例

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1KD树构建与查询：

2.1.2 k近邻搜索

2.1.3半径搜索

2.2完整代码

三、实现效果

3.1处理后点云

3.2数据显示

相关文章