PCL学习笔记——kd树原理以及pcl中的使用

kd树介绍

顾名思义，kd树其实就是多维二叉树（空间二叉树的一种特殊情况）， 里面储存着k维的点的信息，是对k维空间进行划分的一种数据结构。一般用来解决二维空间和三维空间的信息检索问题。
本文主要讨论kd树在三维点云pcl库中的使用。

kd树理论

kd树的根部，所有子节点在第一个指定维度上被分开，第一维坐标小于根节点的点被分到左边，大于根节点的被分在右边的子树，树的每一级都在下一个维度上被分开，所有其他维度用完之后就会回到第一维度。

kd树建立

KD树的构建有两种方法（split域的选择）：一种利用方差（可以选择方差较大的那个维度为第一个指定维度，这样划分说明该维度不确定性更大，数据分散，这样作为第一个轴则其数据分辨率更高），一种根据维度来划分。

以三维空间数据来看，首先，边框为红色的竖直平面将整个空间划分为两部分，此两部分又分别被边框为绿色的水平平面划分为上下两部分。最后此4个子空间又分别被边框为蓝色的竖直平面分割为两部分，变为8个子空间，此8个子空间即为叶子节点。以此方式在各个选定维度上不断的递归，即得到了最终的叶子节点和根节点。

输入:  无序点云，维度3
构建步骤
步骤1、初始化分割轴：对每个维度的数据进行方差的计算，取最大方差的维度作为分割超平面，标记为r；
步骤2、确定节点：对当前数据按分割超平面维度进行检索，找到中位数数据，并将其放入到当前节点上；
步骤3、划分双支：
    划分左支：在当前分割超平面维度，所有小于中位数的值划分到左支中；
    划分右支：在当前分割超平面维度，所有大于等于中位数的值划分到右支中。
步骤4、更新分割超平面：r = (r + 1) % 3;（保证一直沿此3个轴进行分割）
步骤5、确定子节点：
    确定左节点：在左支的数据中进行步骤2；
    确定右节点：在右支的数据中进行步骤2；
输出：点云对应的kd-tree

kd树在最近邻搜索中的使用（二维为例）

给定点p，查询点云数据中与其距离最近点的过程即为最近邻搜索。
如在上文构建好的k-d tree上搜索(3,5)的最近邻时，结合如下图对二维空间的最近邻搜索过程进行分析。
a）首先从根节点(7,2)出发，将当前最近邻设为(7,2)，对该k-d tree作深度优先遍历。以(3,5)为圆心，其到(7,2)的距离为半径画圆（多维空间为超球面），可以看出(8,1)右侧的区域与该圆不相交，所以(8,1)的右子树全部忽略。
b）接着走到(7,2)左子树根节点(5,4)，与原最近邻对比距离后，更新当前最近邻为(5,4)。以(3,5)为圆心，其到(5,4)的距离为半径画圆，发现(7,2)右侧的区域与该圆不相交，忽略该侧所有节点，这样(7,2)的整个右子树被标记为已忽略。
c）遍历完(5,4)的左右叶子节点，发现与当前最优距离相等，不更新最近邻。所以(3,5)的最近邻为(5,4)。

pcl库中使用kd树进行k近邻搜索

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>  //kd_tree
#include <vector>
int main()
{
    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 (std::size_t i = 0; i < cloud->size(); ++i)
    {
        (*cloud)[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
        (*cloud)[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        (*cloud)[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud); // 输入点云

    int K = 10;
    std::vector<int> pointIdxKNNSearch(K); //保存最近邻的索引
    std::vector<float> pointKNNSquaredDistance(K); //保存其最近邻的平方根距离

    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 << "K nearest neighbor search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with K=" << K << std::endl;

    // 打印出“搜索点”的所有10个最近邻居的位置
    if (kdtree.nearestKSearch(searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0) // > 0表示能够找到近邻点， = 0表示找不到近邻点
    {
        for (std::size_t i = 0; i < pointIdxKNNSearch.size(); ++i)
            std::cout << "    " << (*cloud)[pointIdxKNNSearch[i]].x
                      << " " << (*cloud)[pointIdxKNNSearch[i]].y
                      << " " << (*cloud)[pointIdxKNNSearch[i]].z
                      << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
    }

    //半径搜索
    float radius = 200;
    std::vector<int> pointIdxRadiusSearch; //半径搜索的邻居点索引
    std::vector<float> pointRadiusSquaredDistance;  //半径平方根距离
    if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
    {
        for (std::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;
}