DSO初始化流程
嗨，各位讀者朋友們好！今天我們接着講DSO全家桶中的第三講——初始化。
這一講的內容相對來說會簡單一些，因為筆者會將初始化階段的運動估計移到下一講：DSO全家桶（四）——前端：前端跟蹤中去講，因為本質上這兩個跟蹤的方法比較接近，就沒有必要贅述了。
在第二講中，我們已經介紹了DSO提取梯度點的策略，那么這一講就接着后續的操作，講講這些梯度點會怎么用。對於單目SLAM來說，我們通常至少需要兩個幀才可以完成初始化，DSO也不例外。
那么，我們先看看DSO初始化階段的主要流程：
圖1. DSO初始化流程
1. 對第一幀的圖像金字塔提取梯度點，並將每個點的逆深度值初始化為1，並設置每個點的外點閾值；
2. 對步驟1中的每一個梯度點構建最近鄰（makeNN）索引，在同一層中找到10個與其距離最接近的點構建鄰域關系。通過在下一層的NN索引中，找到與當前點距離最小的點，設置為parent點，目的是關聯當前層與下一層，便於光流金字塔和逆深度值的傳播；
3. 輸入第二幀圖像，將第一幀初始化的梯度點投影到當前幀中構建殘差，並采用光流金字塔對初始相對運動進行估計，這部分內容我們會在下一講詳細介紹；
4. 將第一幀設置為關鍵幀，加入滑動窗口中；
5. 統計第一幀所有點的逆深度值，並計算歸一化尺度因子，用於將相對位移歸一化；
6. 遍歷所有點，對所有點創建未成熟點，根據閾值條件判斷可否進行激活；
7. 為所有可激活的點設置逆深度值，值得注意的是，DSO設置了兩個逆深度值，一個是有尺度因子的，一個是沒有尺度的；
8. 設置兩幀各自的初始位姿；
9. 將第二幀加入滑動窗口中進行聯合優化；
10. 聯合優化后會根據滑動窗口內關鍵幀數量設置不同的閾值條件，通過判斷均方根誤差（RMSE）與閾值的大小，確定是否初始化成功。閾值設置規律為：滑窗內的關鍵幀越多，閾值越小。若連續4個關鍵幀的聯合均方根誤差低於閾值條件，則初始化成功！
KD-Tree + FLANN（Fast Library for Approximate Nearest Neighbors）
筆者仔細研究了一下上述10個步驟，由於我們計划將相對姿態初值的估計移到下一講介紹，導致這一講內容比較單薄。筆者一時之間不知道該詳細介紹哪個。認真研究了一波，感覺這個近似最近鄰好像有點可以講，筆者就現學現賣，給大家整理一下這個玩意兒是怎么操作的。參考的資料都附引用，以表示對原作者的尊重。
OK，我們開始吧。
KD-Tree
KD-Tree又稱 K 維樹是計算機科學中使用的一種數據結構，用來組織表示 K 維空間中點集合，它是一種帶有其他約束條件的二分查找樹。
實際上，我們知道二叉搜索樹有這樣的特性：
對於根結點，其左子樹上所有結點的值均小於根結點的值，其右子樹上所有結點的值均大於根結點的值。
圖2. 二叉搜索樹（引用自百度百科）
那么，在KD-Tree中，也是保有這樣的特性。對於一組無序的輸入，假設我們常用到的激光雷達的點、或者是RGB-D圖像得到的點、或者是我們DSO中估計得到的三維點，都是一系列無序的三維點。對於每一個輸入點，為了能夠快速地搜索到與其相鄰的點（假設是特征匹配，或者叫最近鄰匹配），我們需要對現有的數據進行重組，建立離散點間的拓撲關系，使其能夠快速查找。這件事情與BOW構建詞袋模型類似，筆者在介紹OBR-SLAM2中的回環檢測時有提到過，不過當時討論的是具備描述子的特征，感興趣的讀者可以看看。
首先確定一下，我們的輸入是無序三維點，通常我們只在三個維度中進行處理，因此所有的KD-Tree都將是三維KD-Tree。
由二叉搜索樹的思路我們很容易想到，每次指定某個坐標軸作為分割線，假設第一次選擇X軸，理想情況下，處於中位數的結點應該被設置為根結點，隨后X值小於根結點的點全部被掛在左子樹，反之則掛在右子樹上。由圖3所示，可以非常直觀的看到具體的操作。
圖3. KD-Tree（引用自參考資料1）
於是，這里就有兩個問題需要解決：
1. 怎么樣確定理想的中位數？
2. 直接針對原始數據進行處理會導致頻繁調用拷貝構造函數（swap），那么怎么避免計算中位數時出現的這個問題？
在這里，我們就借鑒快速排序的思路：
第一個問題，其實我們並不需要對整個數據進行排序，因此只需要找到橫向或者縱向的中位數即可，采用 nth_element(...) 就可以非常容易得找到目標值；
第二個問題，參考FLANN中的方案，KD-Tree構建時，會同時引入一個與點雲同等大小的index索引數組。在划分中位數的時候，改變的只是index，不需要拷貝復制任何點。
於是，通過上述的方案，我們就可以將無序三維點（點雲）構建成一個KD-Tree了，構建KD-Tree的方法可以參考淺談kdtree。
FLANN
圖4. 最近鄰搜索（引用自參考資料3）
這里我們需要依次介紹一下1-NN、K-NN和A-NN。
1-NN：在KD-Tree的基礎上，給定一個查詢點，需要在樹中找到與其最近似的點。
在數據規模較小的情況下，我們可能會考慮采用暴力法一頓擼，這是有效的。但是數據規模非常大時，暴力法的低效是令人發指的。那么有了KD-Tree，要咋利用呢？
首先，我們可以知道，數據已經呈現某個規律排布了。那么我們就將從根結點開始計算查詢點與根結點的距離作為初始的最小值，隨后依次計算左子樹和右子樹結點與查詢點的距離，若其中一個距離小於根結點，則更新最小距離。根據左右子樹結點的大小，確定是先遍歷左子樹還是右子樹。后續就是不斷遞歸，找到最小的空間距離即可。
到這里，大家肯定會問，那這樣和暴力法有啥區別？emmmm....區別在於這里有剪枝的功能，假設我們計算得到的左子樹結點與搜索點的距離比根結點與搜索點的距離還大，那么左子樹就整根被砍掉了，所以很顯然比暴力法要高效多了。
值得注意的是，1-NN並不是在左子樹找到了最近鄰點就不遍歷右子樹了，而是左子樹結束后再遍歷右子樹，直到確定當前篩選到的點確實是最近鄰點為止。如圖4所示，即在左子樹中可以找到中間最底下的點，但是實際上右子樹上有一個離搜索點更近的點，利用1-NN可以准確無誤的找到它。
K-NN：思路與1-NN類似，不同的地方在於此處需要維護一個K近鄰列表。
A-NN：在介紹A-NN之前，我們可以參考一下近似最近鄰搜索ANN(Approximate Nearest Neighbor)中的最后舉例的Kd-Tree的最近鄰查找，在終止條件上，通常是利用搜索點和最近點的距離為半徑構建一個圓，若上一層的結點與該圓相交或者是在該圓內才更新最近鄰點信息，否則進行裁剪。而ANN的判斷方式與該方法有一點點區別，那便是將上述的半徑除去某個大於1的數，使得搜索半徑變得更小，過濾掉了更多的點。emmm....據參考文獻所述，這樣的操作可以使得查詢速度提高10-100倍。
DSO中的用法：
在這里，筆者也簡單提一下，KD-Tree和FLANN在DSO中的用法，主要是在makeNN()這個函數中：
1. 對金字塔圖像所有層提取到的的點，逐層構建樹並保存對應的索引信息；
2. 遍歷金字塔圖像每一層：
2.1 對金字塔圖像每一層的每個點查找10個鄰域點；
2.2 統計每個鄰域點與當前點的距離；
2.3 將當前點進行縮放，即橫縱坐標*0.5，退到下一層金字塔圖像中，搜索最近鄰點，並構建父子鏈接，目的是用於后續在跟蹤時的深度傳播。
總結
到了總結的階段了，這一講我們主要講的是DSO中的初始化模塊，實際上最重要的是兩幀的跟蹤，完成初始地圖和位姿的估計，但跟蹤模塊跟我們大綱的第四講有重合，於是我們就留到下一次再講。這一講的內容主要是講構建KD-Tree和FLANN，並關聯前后金字塔圖像的點。
由於筆者也是現學現賣，有講的不對的地方，煩請各位讀者指正。本講給出的參考資料對於本講的內容有非常大的豐富，感興趣的可以閱讀參考資料。
參考文獻
[1] PCL 點雲索引方法K維樹（KD-tree）和八叉樹（octree）介紹
[2] 淺談kdtree
[5] 近似最近鄰搜索ANN(Approximate Nearest Neighbor)
版權聲明
如需轉載，請聯系本人郵箱peichu.ye at mail2.gdut.edu.cn。未經允許，不可轉載。