1.簡介

kd-tree簡稱k維樹，是一種空間劃分的數據結構。常被用于高維空間中的搜索，比如范圍搜索和最近鄰搜索。kd-tree是二進制空間劃分樹的一種特殊情況。（在激光雷達SLAM中，一般使用的是三維點云。所以，kd-tree的維度是3）

由于三維點云的數目一般都比較大，所以，使用kd-tree來進行檢索，可以減少很多的時間消耗，可以確保點云的關聯點尋找和配準處于實時的狀態。

2.原理

? ?? 2.1數據結構

? ? ?kd-tree，是k維的二叉樹。其中的每一個節點都是k維的數據，數據結構如下所示：

struct kdtree{Node-data - 數據矢量   數據集中某個數據點，是n維矢量（這里也就是k維）Range     - 空間矢量   該節點所代表的空間范圍split     - 整數       垂直于分割超平面的方向軸序號Left      - kd樹       由位于該節點分割超平面左子空間內所有數據點所構成的k-d樹Right     - kd樹       由位于該節點分割超平面右子空間內所有數據點所構成的k-d樹parent    - kd樹       父節點  
}

上面的數據在進行算法解析中，并不是全部都會用到。一般情況下，會用到的數據是{數據矢量，切割軸號，左支節點，右支節點}。這些數據就已經滿足kd-tree的構建和檢索了。

? ?? 2.2 構建K-d tree

? ? ?kd-tree的構建就是按照某種順序將無序化的點云進行有序化排列，方便進行快捷高效的檢索。構建算法如下：

Input:  無序化的點云，維度k
Output：點云對應的kd-tree
Algorithm：
1、初始化分割軸：對每個維度的數據進行方差的計算，取最大方差的維度作為分割軸，標記為r；
2、確定節點：對當前數據按分割軸維度進行檢索，找到中位數數據，并將其放入到當前節點上；
3、劃分雙支：劃分左支：在當前分割軸維度，所有小于中位數的值劃分到左支中；劃分右支：在當前分割軸維度，所有大于等于中位數的值劃分到右支中。
4、更新分割軸：r = (r + 1) % k;
5、確定子節點：確定左節點：在左支的數據中進行步驟2；確定右節點：在右支的數據中進行步驟2；

這樣的化，就可以構建出一顆完整的kd-tree了。（???更新分割軸r=(r+1)%k原理是啥？？？）

拿個例子說明為：二維樣例：{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}

構建步驟：

1、初始化分割軸：

發現x軸的方差較大，所以，最開始的分割軸為x軸。

2、確定當前節點：

對{2，5，9，4，8，7}找中位數，發現{5,7}都可以，這里我們選擇7，也就是(7,2);

3、劃分雙支數據：

在x軸維度上，比較和7的大小，進行劃分：

左支：{(2,3)，(5,4)，(4,7)}

右支：{(9,6)，(8,1)}

4、更新分割軸：

一共就兩個維度，所以，下一個維度是y軸。

5、確定子節點：

左節點：在左支中找到y軸的中位數(5,4)，左支數據更新為{(2,3)}，右支數據更新為{(4,7)}

右節點：在右支中找到y軸的中位數(9,6)，左支數據更新為{(8,1)}，右支數據為null。

6、更新分割軸：

下一個維度為x軸。

7、確定(5,4)的子節點：

左節點：由于只有一個數據，所以，左節點為(2,3)

右節點：由于只有一個數據，所以，右節點為(4,7)

8、確定(9,6)的子節點：

左節點：由于只有一個數據，所以，左節點為(8,1)

右節點：右節點為空。

最終，就可以構建整個的kd-tree了。

示意圖如下所示 ：

二維空間表示：(二維坐標系下的分割示意圖，如左圖) ? ? ? ? ? ? ? ?kd-tree表示：（如右圖）

??

? ? ?? 2.3 最近鄰搜索

? 在構建了完整的kd-tree之后，我們想要使用他來進行高維空間的檢索。所以，這里講解一下比較常用的最近鄰檢索，其中范圍檢索也是同樣的道理。

最近鄰搜索，其實和之前我們曾經學習過的KNN很像。不過，在激光點云章，如果使用常規的KNN算法的話，時間復雜度會空前高漲。因此，為了減少時間消耗，在工程上，一般使用kd-tree進行最近鄰檢索。

由于kd-tree已經按照維度進行劃分了，所以，我們在進行比較的時候，也可以通過維度進行比較，來快速定位到與其最接近的點。由于可能會進行多個最近鄰點的檢索，所以，算法也可能會發生變化。因此，我將從最簡單的一個最近鄰開始說起。

• 一個最近鄰

一個最近鄰其實很簡單，我們只需要定位到對應的分支上，找到最接近的點就可以了。

舉個例子：查找(2.1,3.1)的最近鄰。

1. 計算當前節點(7,2)的距離，為6.23，并且暫定為(7,2)，根據當前分割軸的維度（2.1 < 7），選取左支。
2. 計算當前節點(5,4)的距離，為3.03，由于3.03 > 6.23，暫定為(5,4)，根據當前分割軸維度（3.1 < 4），選取左支。
3. 計算當前節點(2,3)的距離，為0.14，由于0.14 < 3.03，暫定為(2,3)，根據當前分割軸維度（2.1 > 2），選取右支，而右支為空，回溯上一個節點。
4. 計算(2.1,3.1)與(5,4)的分割軸{y = 4}的距離，（為什么要算與分割軸的距離啊？？？）如果0.14小于距離值，說明就是最近值。如果大于距離值，說明，還有可能存在值與(2.1,3.1)最近，需要往右支檢索。

由于0.14 < 0.9，（此處為什么用與分割軸的距離作比較？？？？，因為如果與分割軸的距離大于現有距離，那么分割軸另一側分支的所有與它的距離更大，就不用比較了）我們找到了最近鄰的值為(2,3)，最近距離為0.14。

? ?? 多個最近鄰

? ? ?多個近鄰其實和一個最近鄰類似，不過是存儲區間變為了多個，判定方法還是完全一樣。

? ?? 參考文章：

? 3、常用函數

kd-tree在日常使用中，一般會在兩個方面使用：

• 最近鄰搜索
• 距離范圍搜索

距離范圍搜索的原理和最近鄰搜索的差不多，把滿足距離的全部放進去就可以了。

最近鄰搜索的函數在激光點云匹配中找最近點的時候用的比較多：

//頭文件
#include <pcl/kdtree/kdtree_flann.h>
//設定kd-tree的智能指針
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
//輸入三維點云，構建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
//在點云中尋找點searchPoint的k近鄰的值，返回下標pointSearchInd和距離pointSearchSqDis
kdtreeCornerLast->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);

其中，當k為1的時候，就是最近鄰搜索。當k大于1的時候，就是多個最近鄰搜索。

距離范圍搜索：

//在點云中尋找和點searchPoint滿足radius距離的點和距離，返回下標pointIdxRadiusSearch和距離pointRadiusSquaredDistance
kdtreeCornerLast->radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

完整代碼：

#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 datacloud->width = 1000;cloud->height = 1;cloud->points.resize (cloud->width * cloud->height);for (std::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);}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 searchint 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->points[ pointIdxNKNSearch[i] ].x << " " << cloud->points[ pointIdxNKNSearch[i] ].y << " " << cloud->points[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;}// Neighbors within radius searchstd::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->points[ pointIdxRadiusSearch[i] ].x << " " << cloud->points[ pointIdxRadiusSearch[i] ].y << " " << cloud->points[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;}return 0;
}

? ??

總結

以上是生活随笔為你收集整理的PCL：k-d tree 1 讲解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。