PCL學(xué)習(xí)三：KD-Tree & Octree

這篇具有很好參考價值的文章主要介紹了PCL學(xué)習(xí)三：KD-Tree & Octree。希望對大家有所幫助。如果存在錯誤或未考慮完全的地方，請大家不吝賜教，您也可以點擊"舉報違法"按鈕提交疑問。

參考引用

Point Cloud Library

黑馬機(jī)器人 | PCL-3D點云

【量化課堂】KD-Tree系列

KD-Tree原理詳解

PCL點云庫學(xué)習(xí)筆記（文章鏈接匯總）

1. 引言

通過激光雷達(dá)或雙目相機(jī)獲取到的點云，一般數(shù)據(jù)量較大且分布不均勻，數(shù)據(jù)主要表征了目標(biāo)物表面的大量點的集合，這些離散的點如果希望實現(xiàn)基于鄰域關(guān)系的快速查找比對功能，就必須對這些離散的點之間建立拓?fù)潢P(guān)系
常見的空間索引一般是自上而下逐級劃分空間的各種索引結(jié)構(gòu)，包括 BSP 樹，KD tree、KDB tree、R tree、CELL tree、Octrees（八叉樹）等，有了這些關(guān)系，我們就可以實現(xiàn)點云的降采樣、計算特征向量、點云匹配和點云拆分等功能

2. BST（Binary Search Tree，二叉搜索樹）

二叉搜索樹定義
- 二叉搜索樹，也稱：二叉查找樹，二叉排序樹。它要么是一棵空樹，要么是具有下列性質(zhì)的二叉樹：
  - 若它的左子樹不為空，則左子樹上所有結(jié)點的值均小于它的根結(jié)點的值
  - 若它的右子樹不空，則右子樹上所有結(jié)點的值均大于它的根結(jié)點的值
  - 它的左、右子樹也分別為二叉搜索樹
復(fù)雜度
- 不論哪一種操作，所花的時間都和樹的高度成正比：若有 n 個元素，則平均每次操作需要 $O (l o g n)$ 的時間
最近鄰（1NN）搜索：設(shè)查詢點為 11
1. 從 8 開始：worst distance = 11 - 8 = 3（其中 worst distance 是在查詢點周圍搜索到的最大距離），因此 8 的左支離查詢點至少距離為 3 ，故從右支開始往下查詢，并且查詢范圍是（8，14）
2. 查詢到 10：worst distance = 11 - 10 = 1，因此更新最近鄰為 10，并且更新查詢范圍是（10，12）
3. 由于 10 只有右支，故往下查詢到 14，worst distance = 14 - 10 = 4，不更新
4. 再次往下查詢到 13，不符合查詢范圍（10，12），發(fā)現(xiàn)到底了故往上返回到 14
5. 繼續(xù)往上返回到 10
6. 最后到達(dá)頂點 8，由于 8 的左支離查詢點至少距離為 3，并且最新的最近鄰為 10，查詢范圍是（10，12），因此不考慮左支的查詢，查詢結(jié)束，最近鄰為 10

PCL學(xué)習(xí)三：KD-Tree & Octree

3. kNN（k-Nearest-Neighbours，k最近鄰）

在判定一個未知事物時，可以觀察離它最近的幾個樣本，這就是 kNN（k最近鄰）的方法

kNN（k-Nearest Neighbours）是機(jī)器學(xué)習(xí)中最簡單易懂的算法，它的適用面很廣，并且在樣本量足夠大的情況下準(zhǔn)確度很高，kNN 可以用來進(jìn)行分類或者回歸

3.1 一只兔子幫你理解 kNN

有一種兔子叫悲傷（Grief），它們的平均身高是 50 厘米，平均體重 5 公斤。我們拿來 100 只悲傷，分別測量它們的身高和體重，畫在坐標(biāo)圖上，用綠色方塊表示

PCL學(xué)習(xí)三：KD-Tree & Octree

還有一種兔子叫痛苦（Agony）。它們體型比較小，平均身高是 30 厘米，平均體重是 4 公斤。我們將 100 只痛苦的身高和體重畫在同一個坐標(biāo)圖上，用藍(lán)色三角表示

PCL學(xué)習(xí)三：KD-Tree & Octree

最后一種兔子叫絕望（Despair）。它們的平均身高 45 厘米，平均體重 2.5 公斤。100 只絕望的數(shù)據(jù)用黃色圓圈表示

在上面這些數(shù)據(jù)中，(身高，體重) 的二元組叫做特征（features），兔子的品種則是分類標(biāo)簽（class label）。我們想解決的問題是，給定一個未知分類的新樣本的所有特征，通過已知數(shù)據(jù)來判斷它的類別。某機(jī)器人只有測量兔子的身高和體重的能力，怎么讓它判斷一未知兔子的類別？我們給機(jī)器人預(yù)設(shè)一個整數(shù) k，讓它去尋找距離最近的 k 個數(shù)據(jù)樣本進(jìn)行分析：

機(jī)器人測量出這只兔子身長 40 厘米，體重 2.7 公斤，就是下面圖中那顆閃閃發(fā)亮的紅星

PCL學(xué)習(xí)三：KD-Tree & Octree

kNN 算法如何對這次觀測進(jìn)行分類要取決于 k 的大小。直覺告訴我們這只兔子像是一只絕望，因為除了最近的藍(lán)色三角外，附近其他都是黃色圓圈；如果設(shè) k = 15，算法會判斷這只兔子是一只絕望。但是如果設(shè) k = 1，那么由于距離最近的是藍(lán)色三角，會判斷迷之兔子是一只痛苦
如果按照 15NN 和 1NN 的方法對這個二維空間上的每一個點進(jìn)行分類，會形成以下的分割
- 在兩組分類中，1NN 的分類邊界明顯更 “崎嶇”，但是對歷史樣本沒有誤判
- 而 15NN 的分類邊界更平滑，但是對歷史樣本有誤判現(xiàn)象
- 選擇 k 的大小取決于對偏差和方差之間的權(quán)衡

PCL學(xué)習(xí)三：KD-Tree & Octree

3.2 距離函數(shù)

在選擇一個數(shù)量 k 還只是小問題，更重要的是距離的計算方法，畢竟，當(dāng)我們說 “最近的k個點” 時，這個 “近” 是怎么衡量的？

PCL學(xué)習(xí)三：KD-Tree & Octree

使用 kNN 時需要根據(jù)特征數(shù)據(jù)的取值區(qū)間來調(diào)整坐標(biāo)軸的比例，這個做法叫作標(biāo)準(zhǔn)化或者歸一化。為什么要這么做呢？拿上面的例子來說，一只兔子的身長（cm）數(shù)值平均是它的體重（kg）的 10 倍左右，如果我們在這組數(shù)值上直接使用 $L_2$ 距離函數(shù)的話就會導(dǎo)致橫軸的距離比重明顯放大，分類結(jié)果也不合理，如下圖所示：

PCL學(xué)習(xí)三：KD-Tree & Octree

如果把坐標(biāo)軸成其他的單位，比如毫米和噸，并用相應(yīng)的新數(shù)值來計算距離，又會得到完全不同的分類標(biāo)準(zhǔn)。甚至，在極端情況下，如果身高用納米并且體重用噸計量，那么相比之下身高的數(shù)值會奇高無比，以至于兩點之間的距離是完全由身高決定的，體重則沒有任何權(quán)重

為了解決這個問題，我們應(yīng)該在計算距離時把所有坐標(biāo)軸進(jìn)行歸一化
- 在之前的例子中，由于橫軸數(shù)值大約是豎軸的 10 倍左右，所以我們將橫軸（身高）的數(shù)值壓縮 10 倍，即計算距離時使用下式就可以得出合理的 kNN 分類

PCL學(xué)習(xí)三：KD-Tree & Octree

3.3 概率 kNN

上面的 kNN 算法返回的是對一組特征的絕對分類，告訴我們這只兔子被判斷為哪一個類別。可有時我們并不想知道一個確切地分類，而想知道它屬于某個分類的概率是多大。比如我們發(fā)現(xiàn)一只身長 37 體重 4.8 的兔子，在下圖五角星的位置

PCL學(xué)習(xí)三：KD-Tree & Octree

這只兔子的特征數(shù)據(jù)在悲傷和痛苦的分界處，機(jī)器不論判斷它屬于哪個類別都很有可能是錯的。這時，類似“它有一半可能性是痛苦，一半可能性是悲傷”的反饋會更有意義
- 為了這個目的，我們同樣找出距離問題特征最近的 k 個樣本，但與其尋找數(shù)量最多的分類，我們統(tǒng)計其中每個類別的分別有多少個，再除以 k 得到一個屬于每一個類別概率值
- 比如在上圖里，距離五角星最近的 15 個樣本中，有 8 只悲傷和 7 只痛苦，由此判斷：它有 53% 的可能性是悲傷，47% 的可能性是痛苦，0% 的可能性是絕望
- 在整個二維空間中的每一個點上進(jìn)行概率 kNN 算法，可以得到每個特征點是屬于某個類別的概率熱力圖，圖中顏色越深代表概率越大
- 相比于絕對的分類，這些概率的計算會給我們更有效的表述以及更多的應(yīng)用空間

PCL學(xué)習(xí)三：KD-Tree & Octree

kNN 雖然思路簡單，但實現(xiàn)起來有一個問題那就是計算量很大，當(dāng)數(shù)據(jù)量很多時，拿一組特征來和所有樣本依次計算距離并選取最近的 k 個，是非常耗費(fèi)時間的，其中 kNN 的一個高效算法為：KD-Tree

4. KD-Tree 算法

4.1 KD-Tree 理論基礎(chǔ)

kd-tree 簡稱 k 維樹，是計算機(jī)中用于在 k 維空間中一些點建立關(guān)系的數(shù)據(jù)結(jié)構(gòu)，它是一個包含特定約束的二叉搜索樹，常被用于高維空間中的搜索，比如范圍搜索和最近鄰搜索，kd-tree 是二進(jìn)制空間劃分樹的一種特殊情況

如果特征的維度是 D，樣本的數(shù)量是 N，那么一般來講 kd 樹算法的復(fù)雜度是 O(DlogN)，相比于窮算的 O(DN) 省去了非常多的計算量

通常只處理三維空間的點云（激光雷達(dá)），因此所有的 kd 樹都是三維空間的，由于三維點云的數(shù)目一般都比較大，所以使用 kd-tree 來進(jìn)行檢索，可以減少很多的時間消耗，可以確保點云的關(guān)聯(lián)點尋找和配準(zhǔn)處于實時的狀態(tài)

下面的數(shù)據(jù)在進(jìn)行算法解析中，并不是全部都會用到，一般情況會用到的數(shù)據(jù)是：數(shù)據(jù)矢量，切割軸號，左支節(jié)點，右支節(jié)點，這些數(shù)據(jù)就已經(jīng)滿足 kd-tree 的構(gòu)建和檢索

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

PCL學(xué)習(xí)三：KD-Tree & Octree

4.2 構(gòu)建 KD-Tree

kd-tree 的構(gòu)建就是按照某種順序將無序化的點云進(jìn)行有序化排列，方便進(jìn)行快捷高效的檢索，構(gòu)建算法如下：

Input:    無序化的點云，維度 k
Output：  點云對應(yīng)的 kd-tree
Algorithm：
1、初始化切分軸：對每個維度的數(shù)據(jù)進(jìn)行方差的計算，取最大方差的維度作為切分軸，標(biāo)記為 r
2、確定節(jié)點：對當(dāng)前數(shù)據(jù)按切分軸維度進(jìn)行檢索，找到中位數(shù)數(shù)據(jù)（如果一共有偶數(shù)個元素，則選擇
   中位左邊或右邊的元素，左或右并無影響），并將其放入到當(dāng)前節(jié)點上
3、劃分雙支：
    劃分左支：在當(dāng)前切分軸維度，所有小于中位數(shù)的值劃分到左支中
    劃分右支：在當(dāng)前切分軸維度，所有大于等于中位數(shù)的值劃分到右支中
4、更新切分軸：r = (r + 1) % k;
5、確定子節(jié)點：
    確定左節(jié)點：在左支的數(shù)據(jù)中進(jìn)行步驟 2
    確定右節(jié)點：在右支的數(shù)據(jù)中進(jìn)行步驟 2

構(gòu)造 kd-tree 實例

首先隨機(jī)在 $\mathbb{R}^2$ 中隨機(jī)生成 13 個點作為數(shù)據(jù)集。起始的切分軸 r = 0：這里 r = 0 對應(yīng) x 軸，而 r = 1 對應(yīng) y 軸

PCL學(xué)習(xí)三：KD-Tree & Octree

首先先沿 x 坐標(biāo)進(jìn)行切分選出 x 坐標(biāo)的中位點，獲取最根部節(jié)點的坐標(biāo)

PCL學(xué)習(xí)三：KD-Tree & Octree

并且按照該點的 x 坐標(biāo)將空間進(jìn)行切分，所有 x 坐標(biāo)小于 6.27 的數(shù)據(jù)用于構(gòu)建左枝，x 坐標(biāo)大于 6.27 的點用于構(gòu)建右枝
在下一步中 r = 0 + 1 = 1 mod 2 對應(yīng) y 軸，左右兩邊再按照 y 軸的排序進(jìn)行切分，。得到下面的樹，左邊的 x 是指這該層的節(jié)點都是沿 x 軸進(jìn)行分割的（空間的切分如下圖二）

PCL學(xué)習(xí)三：KD-Tree & Octree

下一步中 r ≡ 1 + 1 ≡ 0 mod 2，對應(yīng) x 軸，所以下面再按照 x 坐標(biāo)進(jìn)行排序和切分

PCL學(xué)習(xí)三：KD-Tree & Octree

最后每一部分都只剩一個點，將他們記在最底部的節(jié)點中，因為不再有未被記錄的點，所以不再進(jìn)行切分（切分同上），就此完成了 kd-tree 的構(gòu)造

PCL學(xué)習(xí)三：KD-Tree & Octree

4.3 k-近鄰搜索

在構(gòu)建了完整的 kd-tree 之后，想要使用它來進(jìn)行高維空間的搜索。所以，這里講解一下比較常用的最近鄰搜索（最近鄰搜索是 k-近鄰的特例，也就是 1 近鄰），其中范圍搜索也是同樣的道理
在激光點云中，常規(guī)的 kNN 算法時間復(fù)雜度會空前高漲。為減少時間消耗，工程上一般使用 kd-tree 進(jìn)行 k-近鄰搜索

使用 kd-tree 進(jìn)行 k-近鄰搜索實例

設(shè)想查詢的點為 p = (?1,?5)，設(shè)距離函數(shù)是 L2 距離，想找距離 p 最近的 k = 3 個點

（一）根據(jù) p 的坐標(biāo)值和每個節(jié)點的切分向下搜索（也就是說，如果樹的節(jié)點是按照 $x_r$ = a 進(jìn)行切分，并且 p 的 r 坐標(biāo)小于 a，則向左枝進(jìn)行搜索；反之則走右枝）
1. 首先，從頂部開始
2. 和這個節(jié)點的 x 軸比較一下，p 的 x 值更小，因此向左枝進(jìn)行搜索
3. 這次對比 y 軸，p 的 y 值更小，因此向左枝進(jìn)行搜索
4. 這個節(jié)點只有一個子枝，就不需要對比了，由此找到了最底部的節(jié)點 (?4.6,?10.55)

PCL學(xué)習(xí)三：KD-Tree & Octree

（二）當(dāng)達(dá)到一個底部節(jié)點時，將其標(biāo)記為訪問過。如果 L 里不足 k 個點，則將當(dāng)前節(jié)點的特征坐標(biāo)加入 L ；如果 L 不為空并且當(dāng)前節(jié)點的特征與 p 的距離小于 L 里最長的距離，則用當(dāng)前特征替換掉 L 中離 p 最遠(yuǎn)的點
1. 將當(dāng)前結(jié)點標(biāo)記為訪問過，并記錄下 L = [(?4.6,?10.55)]
（三）如果當(dāng)前節(jié)點不是整棵樹最頂端節(jié)點，則向上爬一個節(jié)點；反之輸出 L，算法完成。如果當(dāng)前（向上爬之后的）節(jié)點未曾被訪問過，將其標(biāo)記為被訪問過，然后執(zhí)行（1）和（2）；如果當(dāng)前節(jié)點被訪問過，則再次向上爬一個節(jié)點

（1）如果此時 L 里不足 k 個點，則將節(jié)點特征加入 L；如果 L 中已滿 k 個點，且當(dāng)前節(jié)點與 p 的距離小于 L 里最長的距離，則用節(jié)點特征替換掉 L 中離最遠(yuǎn)的點
（2）計算 p 和當(dāng)前節(jié)點切分線的距離。如果該距離大于等于 L 中距離 p 最遠(yuǎn)的距離并且 L 中已有 k 個點，則在切分線另一邊不會有更近的點，執(zhí)行（三）；如果該距離小于 L 中最遠(yuǎn)的距離或者 L 中不足 k 個點，則切分線另一邊可能有更近的點，因此在當(dāng)前節(jié)點的另一個枝從（一）開始執(zhí)行
1. 不是最頂端節(jié)點，向上爬一個節(jié)點到達(dá) (?6.88,?5.4)
1. 執(zhí)行（1），因為記錄下的點只有一個，小于 k = 3，所以也將當(dāng)前節(jié)點記錄下，有 L = [(?4.6,?10.55),(?6.88,?5.4)]。再執(zhí)行（2），因為當(dāng)前節(jié)點的左枝是空的，所以直接跳過，回到步驟（三），由于不是頂部，繼續(xù)往上爬一個節(jié)點
1. 由于還是不夠三個點，于是將當(dāng)前點也記錄下，有 L = [(?4.6,?10.55),(?6.88,?5.4),(1.24,?2.86)]，并且當(dāng)前結(jié)點修改為被訪問過的。由于當(dāng)前節(jié)點有其他的分枝，并且經(jīng)計算得出 p 點和 L 中的三個點的距離分別是 6.62，5.89，3.10，但 p 和當(dāng)前節(jié)點的分割線的距離只有 2.14，小于與 L 的最大距離
1. 因此，在分割線的另一端可能有更近的點。于是在當(dāng)前結(jié)點的另一個分枝從頭執(zhí)行（一），在紅線這里
1. 要用 p 和這個節(jié)點比較 x 坐標(biāo)：p 的 x 坐標(biāo)更大，因此探索右枝 (1.75,12.26)，并發(fā)現(xiàn)右枝已經(jīng)是最底部節(jié)點，因此啟動（二）
1. 經(jīng)計算，(1.75,12.26) 與 p 的距離是 17.48，要大于 p 與 L 的最大距離，因此不將其放入記錄中
1. 然后（三）判斷出不是頂端節(jié)點，再往上爬一個節(jié)點
1. 通過（1）計算這個節(jié)點與 p 的距離是 4.91，要小于 p 與 L 的最大距離 6.62
1. 因此，用這個新的節(jié)點替代 L 中離 p 最遠(yuǎn)的 (?4.6,?10.55)
1. 然后再通過（2）比對 p 和當(dāng)前節(jié)點的分割線的距離
1. 這個距離小于 L 與 p 的最小距離，因此要到當(dāng)前節(jié)點的另一個枝執(zhí)行（一），但那個枝只有一個點，故直接到（二）
1. 計算距離發(fā)現(xiàn)這個點離 p 比 L 最大距離更遠(yuǎn)，因此不進(jìn)行替代
1. 通過（三）發(fā)現(xiàn)不是頂點，再往上爬一個節(jié)點
1. 這個是已經(jīng)訪問過的了，所以再往上爬一個節(jié)點
1. 同理這個是已經(jīng)訪問過的了，所以再往上爬一個節(jié)點
1. 到頂點結(jié)束了嗎？當(dāng)然不，還沒輪到（三）呢，現(xiàn)在是（1）的回合，計算比對發(fā)現(xiàn)頂端節(jié)點與 p 的距離比 L 最大距離還要更遠(yuǎn)，因此不進(jìn)行更新
1. 然后是（2），計算 p 和分割線的距離發(fā)現(xiàn)也是更遠(yuǎn)，因此也不需要檢查另一個分枝
1. 執(zhí)行（三），判斷當(dāng)前是頂點，計算完成！輸出距離 p 最近的三個點 L = [(?6.88,?5.4),(1.24,?2.86),(?2.96,?2.5)]

4.4 KD-Tree 近鄰搜索代碼示例

kd_tree.cpp

/* 
    方式一：指定搜索最近的 K 個鄰居
    方式二：通過指定半徑搜索鄰居
*/
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char **argv) {
    // 用系統(tǒng)時間初始化隨機(jī)種子
    srand(time(NULL));

    // 使用隨機(jī)數(shù)據(jù)創(chuàng)建并填充 PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 1000;
    cloud->height = 1;  // 1 表示點云為無序點云
    cloud->points.resize(cloud->width * cloud->height);    
        // 1. 使用了 C++ 的標(biāo)準(zhǔn)庫函數(shù) rand() 來生成一個 0 到 RAND_MAX 之間的隨機(jī)整數(shù)
        //    然后將其除以 (RAND_MAX + 1.0f) 得到一個 0 到 1 之間的隨機(jī)實數(shù)
        // 2. 對于輸入的點云 cloud，代碼使用一個循環(huán)對其中每個點進(jìn)行操作。在每次循環(huán)中
        //    代碼將當(dāng)前點的 x、y、z 坐標(biāo)分別設(shè)置為 [0, 1024] 范圍內(nèi)的隨機(jī)實數(shù)
    for (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);
    }

    // 創(chuàng)建 Kd-Tree 的實現(xiàn)類 KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    // 將待搜索的點云數(shù)據(jù)集 cloud 設(shè)置為 KdTreeFLANN 的輸入
    kdtree.setInputCloud(cloud);
    // 初始化一個隨機(jī)的點，作為目標(biāo)查詢點
    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-近鄰搜索
        // 創(chuàng)建 K 和兩個向量來保存搜索到的數(shù)據(jù)
        // K = 10 表示搜索 10 個臨近點
        // pointIdxKNNSearch        保存搜索到的近鄰點的索引
        // pointKNNSquaredDistance  保存對應(yīng)近鄰點的距離的平方
    int K = 10;
    std::vector<int> pointIdxKNNSearch(K);
    std::vector<float> pointKNNSquaredDistance(K);

    std::cout << "K nearest neighbor search at (" << searchPoint.x
                                           << " " << searchPoint.y
                                           << " " << searchPoint.z
                                           << ") with K=" << K << std::endl;
    /*
    1. nearestKSearch()的輸入?yún)?shù)包括：
       待搜索的點、要找到的最近鄰數(shù)目 K、存儲最近鄰索引的向量、存儲對應(yīng)平方距離的向量
    2. 如果成功找到至少一個最近鄰，則進(jìn)入 for 循環(huán)，依次輸出每個最近鄰點的 x、y、z 坐標(biāo)和到搜索點的平方距離
    */
    if (kdtree.nearestKSearch(searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0) {
        for (size_t i = 0; i < pointIdxKNNSearch.size(); ++i)
            std::cout << "    " << cloud->points[pointIdxKNNSearch[i]].x
                         << " " << cloud->points[pointIdxKNNSearch[i]].y
                         << " " << cloud->points[pointIdxKNNSearch[i]].z
                         << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
    }

    // 方式二：通過指定半徑 k-近鄰搜索
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;
    // 創(chuàng)建一個 [0,256) 的隨機(jī)半徑值
    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 (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;
    }

    // 可視化操作
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.5);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
    // 添加從原點到搜索點的線段
    viewer.addLine(originPoint, searchPoint);
    // 添加一個以搜索點為圓心，搜索半徑為半徑的球體
    viewer.addSphere(searchPoint, radius, "sphere", 0);
    // 添加一個放到 200 倍后的坐標(biāo)系
    viewer.addCoordinateSystem(200);

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }
    return 0;
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(kdtree_search)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})

編譯并執(zhí)行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./kdtree_search

// 輸出結(jié)果
K nearest neighbor search at (888.876 1009.54 308.168) with K=10
   827.477 959.101 295.293 (squared distance: 6479.63)
   987.269 1023.7 358.487 (squared distance: 12413.7)
   928.859 995.463 183.952 (squared distance: 17226.4)
   903.099 899.302 232.047 (squared distance: 18149)
   866.187 902.418 415.973 (squared distance: 23611.4)
   1022.08 944.851 384.353 (squared distance: 27732.5)
   917.167 845.161 327.023 (squared distance: 28176.2)
   1007.55 988.147 188.48 (squared distance: 28866.8)
   765.84 1007.7 190.899 (squared distance: 28893.3)
   870.826 871.352 208.202 (squared distance: 29414.6)
Neighbors within radius search at (888.876 1009.54 308.168) with radius=169.287
 827.477 959.101 295.293 (squared distance: 6479.63)
 987.269 1023.7 358.487 (squared distance: 12413.7)
 928.859 995.463 183.952 (squared distance: 17226.4)
 903.099 899.302 232.047 (squared distance: 18149)
 866.187 902.418 415.973 (squared distance: 23611.4)
 1022.08 944.851 384.353 (squared distance: 27732.5)
 917.167 845.161 327.023 (squared distance: 28176.2)

PCL學(xué)習(xí)三：KD-Tree & Octree

5. Octree（八叉樹）算法

八叉樹可以提前終止搜索，而 kd-tree 最終都會回到一開始的根節(jié)點，因為無法提前終止搜索，必須回到根節(jié)點才能確定是不是遍歷了空間的所有可能性

kd-tree 只有一個維度的信息不足以確定什么時候可以終止搜索；而八叉樹有 3 個維度信息，一旦以查詢點為中心、以最壞距離為半徑構(gòu)建的一個球完全落在了某個立方體里，就知道搜索范圍限定在這個立方體中，立方體外面的東西不再考慮（不需要考慮根節(jié)點位置）

5.1 Octree 理論基礎(chǔ)

八叉樹定義
- 八叉樹（Octree）是一種用于描述三維空間的樹狀數(shù)據(jù)結(jié)構(gòu)。八叉樹的每個節(jié)點表示一個正方體的體積元素，每個節(jié)點有八個子節(jié)點，這八個子節(jié)點所表示的體積元素加在一起就等于父節(jié)點的體積，一般中心點作為節(jié)點的分叉中心
  
  Voxel 譯為體積元素，簡稱體素，描述了一個預(yù)設(shè)的最小單位的正方體
- 八叉樹（Octree）若不為空樹的，樹中任一節(jié)點的子節(jié)點恰好只會有八個或零個，也就是子節(jié)點不會有 0 與 8 以外的數(shù)目
- 想象一個立方體，最少可以切成多少個相同等分的小立方體？答案就是 8 個。再想象有一個房間，房間里某個角落藏著一枚金幣，可以把房間當(dāng)成一個立方體，先切成八個小立方體，然后排除掉沒有放任何東西的小立方體，再把有可能藏金幣的小立方體繼續(xù)切八等份，如此下去，平均在 $log_8(n)$ （n 表示房間內(nèi)的所有物體數(shù)）的時間內(nèi)就可找到金幣
- 八叉樹就是用在 3D 空間中的場景管理，可以很快地知道物體在 3D 場景中的位置，或偵測與其它物體是否有碰撞以及是否在可視范圍內(nèi)

PCL學(xué)習(xí)三：KD-Tree & Octree

八叉樹原理
1. 設(shè)定最大遞歸深度
2. 找出場景的最大尺寸，并以此尺寸建立第一個立方體
3. 依次將單位元元素丟入能被包含且沒有子節(jié)點的立方體
4. 若沒達(dá)到最大遞歸深度，就進(jìn)行細(xì)分八等份，再將該立方體所裝的單位元元素全部分給八個子立方體
5. 若發(fā)現(xiàn)子立方體所分配到的單位元元素數(shù)量不為零且跟父立方體是一樣的，則該子立方體停止細(xì)分，因為根據(jù)空間分割理論，細(xì)分的空間所得到的分配必定較少，若是一樣數(shù)目，則再怎么切數(shù)目還是一樣，會造成無窮切割的情形
6. 重復(fù) 3，直到達(dá)到最大遞歸深度

5.2 構(gòu)建 Octree

PCL學(xué)習(xí)三：KD-Tree & Octree

5.3 k-近鄰搜索

PCL學(xué)習(xí)三：KD-Tree & Octree

5.4 Octree 近鄰搜索代碼示例

octree_search.cpp

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/cloud_viewer.h>

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

int main(int argc, char **argv) {
    srand((unsigned int) time(NULL));
    
    // 首先定義并實例化一個共享的 Point Cloud 結(jié)構(gòu)，并用隨機(jī)點填充
    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 (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);
    }

    // 創(chuàng)建一個 octree 實例，用設(shè)置分辨率進(jìn)行初始化
    float resolution = 128.0f; // 設(shè)置分辨率為 128
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // resolution 描述了 octree 葉子節(jié)點的最小體素尺寸
    // 這兩句是最關(guān)鍵的，建立 PointCloud 和 octree 之間的聯(lián)系
    octree.setInputCloud(cloud); // 設(shè)置輸入點云
    octree.addPointsFromInputCloud(); // 通過點云構(gòu)建 octree

    // 初始化一個隨機(jī)的點，作為目標(biāo)查詢點
    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);

    // 方式一：“體素近鄰搜索”,它把查詢點所在的體素中其他點的索引作為查詢結(jié)果返回,
        // 結(jié)果以點索引向量的形式保存,因此搜索點和搜索結(jié)果之間的距離取決于八叉樹的分辨率參數(shù)
    std::vector<int> pointIdxVec;

    if (octree.voxelSearch(searchPoint, pointIdxVec)) {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x
                                                   << " " << searchPoint.y
                                                   << " " << searchPoint.z << ")" << std::endl;
        for (size_t i = 0; i < pointIdxVec.size(); ++i) {
            std::cout << " " << cloud->points[pointIdxVec[i]].x
                      << " " << cloud->points[pointIdxVec[i]].y
                      << " " << cloud->points[pointIdxVec[i]].z << std::endl;
        }
    }

    // 方式二：K 近鄰搜索,本例中K被設(shè)置成10, "K 近鄰搜索”方法把搜索結(jié)果寫到兩個分開的向量中,
        // 第一個pointIdxNKNSearch 包含搜索結(jié)果〈結(jié)果點的索引的向量〉
        // 第二個pointNKNSquaredDistance 保存相應(yīng)的搜索點和近鄰之間的距離平方。
    int K = 10;
    std::vector<int> pointIdxNKNSearch;
    std::vector<float> pointNKNSquaredDistance;

    std::cout << "K nearest neighbor search at (" << searchPoint.x
                                           << " " << searchPoint.y
                                           << " " << searchPoint.z
                                           << ") with K=" << K << std::endl;

    if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
        for (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;
        }
    }

    // 方式三：半徑內(nèi)近鄰搜索
        // “半徑內(nèi)近鄰搜索”原理和“K 近鄰搜索”類似,它的搜索結(jié)果被寫入兩個分開的向量中,
        // 這兩個向量分別存儲結(jié)果點的索引和對應(yīng)的距離平方
    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 (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
        for (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;
        }
    }

    // 可視化操作
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.5);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
    // 添加從原點到搜索點的線段
    viewer.addLine(originPoint, searchPoint);
    // 添加一個以搜索點為圓心，搜索半徑為半徑的球體
    viewer.addSphere(searchPoint, radius, "sphere", 0);
    // 添加一個放到200倍后的坐標(biāo)系
    viewer.addCoordinateSystem(200);

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }
}

配置文件 CMakeLists.txt

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(octree_search)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(octree_search octree_search.cpp)
target_link_libraries(octree_search ${PCL_LIBRARIES})

編譯并執(zhí)行

$ mkdir build
$ cd build
$ cmake ..
$ make

$ ./octree_search

// 輸出結(jié)果
Neighbors within voxel search at (252.328 696.188 682.94)
 263.416 626.134 739.909
 227.4 715.465 716.466
K nearest neighbor search at (252.328 696.188 682.94) with K=10
 227.4 715.465 716.466 (squared distance: 2117.02)
 182.548 736.153 660.19 (squared distance: 6984.08)
 232.876 779.102 673.087 (squared distance: 7350.15)
 264.602 782.368 696.721 (squared distance: 7767.66)
 263.416 626.134 739.909 (squared distance: 8275.91)
 294.628 715.86 604.602 (squared distance: 8313.08)
 250.773 591.312 744.471 (squared distance: 14787.5)
 279.201 816.457 655.874 (squared distance: 15919.4)
 222.472 728.584 555.59 (squared distance: 18158.9)
 265.322 671.457 820.796 (squared distance: 19784.7)
Neighbors within radius search at (252.328 696.188 682.94) with radius=137.033
 250.773 591.312 744.471 (squared distance: 14787.5)
 182.548 736.153 660.19 (squared distance: 6984.08)
 294.628 715.86 604.602 (squared distance: 8313.08)
 263.416 626.134 739.909 (squared distance: 8275.91)
 227.4 715.465 716.466 (squared distance: 2117.02)
 222.472 728.584 555.59 (squared distance: 18158.9)
 279.201 816.457 655.874 (squared distance: 15919.4)
 264.602 782.368 696.721 (squared distance: 7767.66)
 232.876 779.102 673.087 (squared distance: 7350.15)