SVO深度解析(二)之跟蹤部分

本篇博客總結了SVO的前端跟蹤部分

轉載請說明出處：

http://blog.csdn.net/zhubaohua_bupt/article/details/74910568

2 跟蹤部分

跟蹤部分主要乾了兩件事

<1>經過圖像對齊，計算一個粗糙的位姿

<2>創建一個當前幀的局部地圖，根據地圖，對位姿進一步優化。

2.1圖像對齊

圖像對齊的目的是迭代計算幀間位姿。

用於圖像對齊的地圖點是上一幀所能看到的地圖點，按先驗知識來說，圖像幀間變換比較小，

咱們有理由相信上一幀和當前幀所能看到場景大部分相同。

其過程以下，

1找到前幀（K-1）看到的地圖點p1,p2,p3。

2投影至後幀（k）二維圖像上，

3而後最小化灰度偏差函數（這是一個最優化過程，又稱圖像對齊），獲得位姿，over。

 

2.2 經過局部地圖對當前幀位姿優化

雖然上一幀能看到的地圖點和當前幀已經重合大部分，但仍有一部分當前幀能看到的視野，

上一幀看不到（比圖下圖p1,p2,p3,p4）。咱們知道，當前幀和以前看到的視野重合率越高，

優化的位姿就越趨於準確，那麼，除了上一幀，有沒有其餘幀和當前幀視野重合呢？

確定是有的，那把這些幀(以下圖Ir1和Ir2)都扒出來

（一般是關鍵幀,由於這些幀既保證視野重複率不是過高，並且位姿相對與普通幀較準確），

用於優化當前幀位姿。這就是本步驟要乾的事情。

這些幀稱之爲局部關鍵幀，其看到的地圖點集合稱之爲局部地圖。

 

2.2.1 局部地圖的構建

2.2.1.1 五點的構建

SVO採用的5點法來構建局部地圖，思想是構建關鍵幀的時候，順便在關鍵幀圖像上找到5個特徵點，

分佈以下

 

上圖中，紅色點表明提取的普通特徵點，藍色點表明5點法提取的特徵點，

看到這裏，5點怎麼提取應該一目瞭然了吧，即提取左上、左下、右上、右下和中間的特徵點做爲5點。

其實如今Frame.cpp裏setKeyPoints()，checkKeyPoints（）函數有個錯誤，稍微看一下就能看到。

 

2.2.1.2 局部地圖的構建

當前幀以前的每個關鍵幀都有5點分佈，那麼選哪些關鍵幀做爲局部優化關鍵幀呢？

也就是5點在篩選局部關鍵幀時有啥用呢？

SVO這樣用：把每一個關鍵幀上的5點投影至當前幀，只要投影上任何一個，

就把對應關鍵幀以及關鍵幀與當前幀的距離記錄下來，

而後，按距離排序，取前n個關鍵幀，做爲局部關鍵幀，關鍵幀上的地圖點爲局部地圖。

實如今Reprojector.cpp裏，具體位置以下

2.2.2優化位姿

這裏先說一下一個概念：

重投影偏差：地圖點P在當前幀的投影點p1和P與當前幀匹配的像素點p2之間的幾何像素距離。

優化位姿的前提是找到局部地圖點與當前幀像素的匹配關係，

而後，利用匹配關係，經過縮小的地圖點的重投影，來優化位姿。

2.2.2.1 地圖點是什麼，與每一幀的關係是什麼？

地圖點一般又叫作地標點（landmark），它是在關鍵幀中提取的特徵點，被賦予深度後的三維點。

在RGBD和雙目的SLAM裏，關鍵幀中提取的特徵點伴隨着深度信息，通過篩選直接就能放入地圖。

可是，在單目SLAM裏，關鍵幀中剛提取的特徵點，並無深度信息，須要通過一段時間的不斷估計，

認爲深度比較準確後，才把該特徵點對應的三維點放入地圖。

也就是說，地圖點的來源是關鍵幀，

做用： 1 優化幀的位姿（縮小重投影偏差函數）

            2 創建稀疏地圖

那麼，地圖點與普通幀和關鍵幀有沒有其餘聯繫呢？

實際上，在VSLAM工做一段時間後，不管是雙目，RGBD仍是單目，

圖像幀的位姿通常都是經過PNP來估計的，PNP的過程可表述爲，

三維地圖點到當前幀的投影，而後創建偏差函數（直接法：灰度，特徵點法：距離）來優化位姿。

那麼，地圖點與幀（包括普通幀和關鍵幀）的關係就是，用地圖點估計並優化該幀的位姿。

2.2.2.2 地圖點和當前幀找匹配關係

對於局部地圖裏的每一個地圖點P，都被當前幀（CF）以前的多個關鍵幀KFS看到過，

也就是說，對於一個地圖點P,咱們能找到觀測到它的全部關鍵幀集合。

 

代碼裏，存儲在Point的obs_數據結構裏面。

 

<1>和誰匹配？

匹配不但須要P的座標，並且還須要P的描述，用來計算類似度（要否則怎麼知道是否匹配上了呢）。

對於每個地圖點P，都關聯着許多關鍵幀（或者說，許多關鍵幀都看到過它），

也就是說，點P的描述能夠來源於其中的任何關鍵幀。那麼，選擇哪一個關鍵幀上的描述和當前幀匹配呢？

SVO是這樣作的：選擇觀測角度（地圖點與不一樣時刻相機的光心連線）與當前幀比較小的那個關鍵幀，

做爲匹配描述。

這個作法也很好理解，畢竟夾角越小，兩幀圖像位姿差別越小，

地圖點在兩幅圖像上（一個是關鍵幀，另外一個是當前幀）的描述越類似，越容易匹配成功嘛。

<2>怎麼匹配？

匹配策略：網格法匹配。

爲何要網格法匹配？

咱們知道，理論上，三個不在一條線上的點能肯定一個平面。

實際上，在SLAM；裏，地圖點都是有偏差的，若是這三個點離的很近，

那麼肯定的平面精度沒有離得遠時肯定的平面準確，

 

在SLAM中，這個平面就是圖像平面。若是咱們能保證地圖點在當前幀圖像平面上投影的比較均勻，

那麼，用這些地圖點估計的位姿也就比較準確。

怎麼進行網格法匹配？

在當前幀上畫一個虛擬的網格，統計每一個網格里投影上的地圖點，而後把地圖點按質量排序。

對於某個網格，按地圖點質量從前日後匹配，只要匹配成功一個，

此網格就不接受其餘地圖點的匹配了，這樣就保證了地圖點在當前幀上的均勻匹配。

實如今reprojector.cpp裏的reprojectMap（）函數裏。

 

 

匹配方式：特徵對齊（feature alignment）

首先，找到選擇觀測角度最小的關鍵幀；

而後，計算兩幀之間的仿射矩陣；

其次，找到關鍵幀中最適合匹配的那一層（已經有圖像金字塔）

最後，用LK光流法完成地圖點與圖像像素點的匹配（實如今feature_alignment.cpp裏）。

匹配不是在極線上搜索的。

匹配的原理以下，

 

用圖像塊計算灰度偏差沒什麼好說的，光流嘛，注意這裏，

因爲兩幀（參與匹配的關鍵幀和當前幀）有位姿差別，兩個匹配窗口須要通過仿射變化（Ai）。

2.2.2.3 優化位姿

經過以上步驟，就找到了局部地圖點與當前幀的匹配關係。接下來，根據匹配關係，優化位姿。

 

優化位姿的思想是縮小重投影偏差函數，偏差函數如(14)。

其中ui是匹配點，π(T,)是通過不太精確的位姿，投影到當前幀的投影點。

當偏差函數最小時，認爲此時的位姿最精確。