PCL-kdTree搜索

前言

在前两篇文章中，我们搭建起了PCL的开发环境以及和QT5相结合的环境。在本篇博客将在此基础上介绍kd-tree的搜索算法。相关基础环境代码下载

1.准备

1.1 kd-tree介绍

k-d树是一种分割k维数据空间的数据结构，在PCL分为两种搜索一种是范围查询，另一种是K近邻查询
范围查询:给定查询点和查询距离的阈值，从数据集中找出所有与查询点距离小于阈值的数据，
K近邻查询:给定查询点及正整数K，从数据集中找到距离查询点最近的K个数据，当K=1时，就是最近邻查询。
具体的理论结论我们可以不用太过于清楚，只要明白PCL提供两个API接口让我们去搜索最近点的数据。

1.2 增加相关逻辑

在上文中的UI_PclViewer.h里面添加两个按钮，如下并且进行初始化:

QPushButton *kdTreeBtn;
QPushButton *kdTreeRBtn;

// 初始化kd tree 按钮
kdTreeBtn = new QPushButton(PCLView);
kdTreeBtn ->setObjectName(QString::fromUtf8("kd tree"));
kdTreeBtn ->setGeometry(QRect(50,60,120,40));

kdTreeRBtn = new QPushButton(PCLView);
kdTreeRBtn ->setObjectName(QString::fromUtf8("kd Rtree"));
kdTreeRBtn ->setGeometry(QRect(50,110,120,40));

// 添加到数据到UI界面上去
kdTreeBtn->setText(QCoreApplication::translate("PCLView","Kd Tree", nullptr));
kdTreeRBtn->setText(QCoreApplication::translate("PCLView","Kd RTree", nullptr));

添加随机生成随机点函数:

// 随机生成点云图
void UI::PclViewer::randomPoint() {
    this->pointCloud->resize(10000);
    for (int i = 0; i < pointCloud->size(); ++i) {
        this->pointCloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        this->pointCloud->points[i].y = 1024* rand() /(RAND_MAX + 1.0f);
        this->pointCloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
        pointCloud->points[i].r = 255 ;
        pointCloud->points[i].g = 0 ;
        pointCloud->points[i].b = 0 ;
    }
   updatePointCloud();
}

// 更新点云图
void UI::PclViewer::updatePointCloud() {
    auto result = this->visualizer->updatePointCloud(this->pointCloud);
    if (!result) this->visualizer->addPointCloud(pointCloud);
    this->visualizer->resetCamera();
    refreshView();
}

重新QT5的按键监听函数，使之点击空格可以重置点云图坐标系。

// 重写keyReleaseEvent 释放空格键的时候，重置坐标系
void UI::PclViewer::keyReleaseEvent(QKeyEvent *event) {
    if (event->key() == Qt::Key_Space){
        visualizer->initCameraParameters();
        updatePointCloud();
    }
}

运行代码如图:

2. Kd-Tree 最近K点搜索

在给点搜索点，以这个点位中心，搜索距离给定搜索点最近的K个点。给按钮添加相应的事件,如下:

void UI::PclViewer::kdTreePressed() {
    // 声明Kd搜索对象
    pcl::KdTreeFLANN<PointT> kdTree;
    // 设置点云
    kdTree.setInputCloud(pointCloud);
    // 设置搜索点信息
    PointT 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);
    searchPoint.r = 75 ;
    searchPoint.g = 0;
    searchPoint.b = 130;

    // 设置搜索点云数量的1/10
    int K = pointCloud->size() / 10;

    // 添加搜索点到点云里面去
    pointCloud->push_back(searchPoint);

    // 设置搜索点数组
    std::vector<int> pointIdxKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    
    // 进行最近临近搜索K个点
    if (kdTree.nearestKSearch (searchPoint, K, pointIdxKNSearch, pointNKNSquaredDistance) > 0){
        for (int i: pointIdxKNSearch) {
            // 修改搜索后点的颜色
            pointCloud->points[i].r = 255 ;
            pointCloud->points[i].g = 255 ;
            pointCloud->points[i].b = 255 ;
        }
    }
    // 更新点云图
    updatePointCloud();
}

在按钮点击函数之后添加上面的K-D逻辑处理。搜索K个最近点。如下图：

3 K-d tree R半径内搜索

在给点搜索点，以这个点位中心，搜索距离给定搜索点距离为R的所有点。给按钮添加相应的事件,如下:

void UI::PclViewer::kdTreeRPressed() {
    // 声明Kd搜索对象
    pcl::KdTreeFLANN<PointT> kdTree;
    // 设置点云
    kdTree.setInputCloud(pointCloud);
    // 设置搜索点信息
    PointT 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);
    searchPoint.r = 75 ;
    searchPoint.g = 0;
    searchPoint.b = 130;

    //存储近邻索引
    std::vector<int> pointIdxRadiusSearch;
    //存储近邻对应距离的平方
    std::vector<float> pointRadiusSquaredDistance;
    //半径
    float radius = .5f;
    LOG(INFO) << radius;
    if ( kdTree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
        for (int i: pointIdxRadiusSearch) {
            // 修改搜索后点的颜色
            pointCloud->points[i].r = 72 ;
            pointCloud->points[i].g = 61 ;
            pointCloud->points[i].b = 139 ;
        }
    }
    // 更新点云图
    updatePointCloud();
}

如上以某点为搜索点为起点，R为半径进行搜索。其结果如下:

4. 总结

本文以QT5为基础，结合相应的PCL库，进行对kd-tree的搜索，下文将继续基于此进行相关的PCL学习