1. 理論基礎

KdTree（k-dimensional樹的簡稱），是一種對k維空間中的例項點進行儲存以便對其進行快速檢索的樹形資料結構。 主要應用於多維空間關鍵資料的搜尋（如：範圍搜尋和最近鄰搜尋）。K-D樹是二進位制空間分割樹的特殊的情況。

k-d樹是每個節點都為k維點的二叉樹。所有非葉子節點可以視作用一個超平面把空間分割成兩個半空間。節點左邊的子樹代表在超平面左邊的點，節點右邊的子樹代表在超平面右邊的點。(比如右節點大於父節點，左節點小於父節點，很明顯使用一次快速排序就能實現該功能。)

選擇超平面的方法如下：每個節點都與k維中垂直於超平面的那一維有關。因此，如果選擇按照x軸劃分，所有x值小於指定值的節點都會出現在左子樹，所有x值大於指定值的節點都會出現在右子樹。有很多種方法可以選擇軸垂直分割面（ axis-aligned splitting planes ），所以有很多種建立k-d樹的方法。 最典型的方法如下：

• 隨著樹的深度輪流選擇軸當作分割面。（例如：在三維空間中根節點是 x 軸垂直分割面，其子節點皆為 y 軸垂直分割面，其孫節點皆為 z軸垂直分割面，其曾孫節點則皆為 x 軸垂直分割面，依此類推。）
• 點由垂直分割面之軸座標的中位數區分並放入子樹

下圖演示了以A、B和C為超平面進行劃分的二叉樹構建，以及kdtree的搜尋功能。

2. KdTree搜尋程式碼舉例

pcl中的KdTree有兩種搜尋方式：

1. k近鄰域搜尋：搜尋某個點附近的k個點。
2. R半徑搜尋：搜尋某個點半徑為R內的所有點。

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
 


#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char **argv)
{
    srand(time(NULL)); //隨機種子

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // Generate pointcloud data
    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); //設定搜尋空間

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

    // K nearest neighbor search
    /***********************k近領域搜尋*************************/
    int K = 10;

    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;

    if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) //開始搜尋
    {
        for (std::size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
            std::cout << "    " << (*cloud)[pointIdxNKNSearch[i]].x
                      << " " << (*cloud)[pointIdxNKNSearch[i]].y
                      << " " << (*cloud)[pointIdxNKNSearch[i]].z
                      << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }

    // Neighbors within radius search
    /***********************半徑R搜尋************************/
    std::vector<int> pointIdxRadiusSearch;         //索引
    std::vector<float> pointRadiusSquaredDistance; //距離

    float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

    std::cout << "Neighbors within radius search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with radius=" << radius << std::endl;

    if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) //開始搜尋
    {
        for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
            std::cout << "    " << (*cloud)[pointIdxRadiusSearch[i]].x
                      << " " << (*cloud)[pointIdxRadiusSearch[i]].y
                      << " " << (*cloud)[pointIdxRadiusSearch[i]].z
                      << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }

    return 0;
}