Direct Sparse Odometry
Abstract
本文提出了一個較爲新穎的直接稀疏里程計(DSO)。整個系統包含了完整的直接法(最小光度誤差)模型,狀態包含相機的位姿,逆深度。爲了達到實時運算,算法去除了直接法添加先驗的做法,取而代之的是從整個圖像上均勻的採樣關鍵點。最後,整個系統添加了完整的光度標定,考慮了曝光時間,鏡頭漸暈以及非線性響應函數(這些基本上都是相機的參數)。
Introduce
這部分主要介紹Direct vs Indirect、Sparse vs Dense方法。
Direct VS Indirect
- Indirect方法主要分爲兩步:1) 預處理圖像,獲得一致性的表示(指代匹配步驟,不管是匹配法或者光流法);2) 使用觀測進行自變量的估計;
- Direct方法捨棄了第一步,直接使用像素值的光度誤差進行估計;
通常,Direct使用光度誤差進行優化,而Indirect方法則使用幾何誤差進行優化。
Dense VS Sparse
- Sparse方法:利用圖像中部分獨立點進行重建;
- Dense方法:利用圖像的全部像素進行重建;
- Semi-dense方法:避免重建整個平面,本質上還是使用部分子集進行重建,論文中描述子集的特點爲連接程度大且約束嚴格,個人的理解是梯度比較大,優化信息比較明顯;
個人感覺,Dense和Sparse再講如何使用圖像中的像素點。
關於兩種方法的組合方法,這裏不再贅述,詳細可以看論文中的部分。DSO使用的顯然就是Direct+Sparse方法,也就是優化方法是光度誤差優化,而在整個過程中,是選擇圖像中一部分像素參與到優化的。
關於爲什麼使用Direct方法
Indirect方法的優勢是不言而喻的,關鍵點對於光照變化、鏡頭畸變具有很好的魯棒性。但是針對相機的曝光時間、伽馬矯正等已知噪聲的影響,Indirect方法顯得無能爲力;但是對於Direct方法,由於使用光度誤差進行優化,可以很自然的將相機的一些特性引入到模型中。
直接法的最大的優勢就是不需要關鍵點的匹配,從而能得到一個更細粒度的幾何表示(這點暫時沒有感受到);同時,直接法可以在整個圖像上尋找有用的點,包括edge上的點和灰度比較低的點,這樣的好處是能在稀疏紋理的地方顯得更加的魯邦。
稀疏法最大的優勢在於保持了求解矩陣的稀疏性,沒有引入幾何先驗使得Hassan陣是稀疏的,如下圖:
文章的結構
這裏主要記錄一下文章的組織結構,方便之後對照。
- 第二章都在講優化的模型,包含
- 相機的內參矯正和光度矯正;
- 整個系統使用的誤差公式;
- 劃窗優化;
- 第三章講前端,包含
- 如何提取特徵點;
- 如何正確有效且準確的初始化;
- 第四章講解了系統的一些別的實驗,包含:
- 一些參數的選擇;
- 一些新引入的概念;
- 一些擾動實驗;
- 第五章做了總結;
這裏筆者打算先進行第三章,先了解一下系統的實際設計,再回來看第二章的理論部分。然後後面的章節就不做贅述了。
Visual Odometry Front-End
這部分算是對整個系統前端的算法描述,該前端的作用有三:
- 如何選擇幀與特徵點,如何構建誤差方程,涉及到外點剔除和遮擋檢測(what?);
- 爲後端提供一個良好的初始值,因爲圖像本身具有很高的非線性,在一個像素周圍僅有1-2的半徑範圍能保證線性;
- 決定什麼時候邊緣化;
Frame Management
step1–初始化幀的跟蹤
當新關鍵幀產生的時候,所有已經成熟的點就投影到當前的幀上而且稍微的膨脹一下(這塊需要看一下代碼是怎麼做的)形成一個半稠密的深度圖。之後的新幀(在該關鍵幀到下一個關鍵幀之間的幀)都用這一幀使用圖像對齊方式進行位姿的初始化。論文中指出,僅僅是多使用像素是沒有意義的,只會增加計算量,但是如果一個像素使用周圍像素的深度則會大大的有益於初始化。
如果最終的均方根誤差如果是上一幀的均方根誤差的兩倍,我們認爲這次的對齊是失敗的!此時並沒有結束,作者在金字塔的最頂層(此時感受野最大)找了27個不同方向再次進行圖像對齊(個人理解就是給迭代選擇一個比較好的初值)!如果依舊失敗應該就真的失敗了。
step2–關鍵幀的選擇
DSO關鍵幀的選擇有三個條件:
-
光流追不上了,根據公式:
-
相機的移動(translation)會造成遮擋(occulsion)問題,即便是上述的f值很小,也可能會有動態的物體遮擋住相機,此時算法需要更多的關鍵幀來做到魯邦,主要根據僅考慮平移的光流公式:
-
如果兩幀之間的曝光參數變化比較大,那麼也應該生成一個關鍵幀,主要根據公式:
這三個條件的數值在幀的追蹤時都可以算出來,因此使用公式來決定要不要產生一個新的關鍵幀;
step3–關鍵幀的邊緣化
DSO的邊緣化幀選取也比較簡單,設是窗口中的關鍵幀,其中1表示最新的,n表示最老的,那麼:
-
必須留下,即最新的兩個幀;
-
如果某個幀的觀測點在最新幀中所佔的比例不超過5%了,那麼邊緣化掉它;
-
把距離分數最大的幀邊緣化掉,距離分數公式爲:
可以看到該公式就是在找最離羣的幀,保證剩下的幀都和最新的幀距離很近; -
在邊緣化之後,會把以被邊緣化的幀爲host幀的點都給扔掉,這樣H矩陣會稍微稀疏一些。
Point Management
在DSO中,作者使用較多的圖像下采樣操作來到達實時的效果,相比較於Indirect方法,DSO能夠很有效的利用弱紋理、重複區域以及邊緣的信息;
在對於active點的控制上,DSO保證整個劃窗中只有個點,這些點要均勻的分佈在空間以及劃窗的關鍵幀中;整個activa過程如下:
- 在每個關鍵鎮上選擇Np個候選點,保證後面我們有足夠的點可以用於激活;
- 候選點不立刻進行優化,相反,他們會在後續的幀中用於track,生成一個初始的深度;
- 在優化點的時候,從劃窗中所有幀的觀測中選擇一部分進行active;
step1–候選點的選擇(這裏簡單寫一下,後面對照源碼會再詳細的說)
- 把整個圖像分作n個32*32的塊,對每個塊結合梯度直方圖取得自閾值(這個步驟之後其實還會對塊圖像進行一個均值濾波),然後取作爲最後的自適應閾值;
- 爲了得到均勻分佈的點集,我們把圖像分爲若干個的像素塊,選擇其中大於自適應閾值的點作爲關鍵點;
- 爲了更好的利用圖像信息,作者又對和提取關鍵點,這一步主要是希望一些弱紋理的地方也能夠提取出一些漸變邊緣的點來幫助track,一個很好的例子就是一面漸變的牆,分辨率看不出來漸變邊緣的點,但是分辨率再低一些就可以看出來一些;
- 如果上述過程拿到的點比期望的要多,那麼就增大d的值,如果少,就減小d的值,做到自適應,代碼中是個遞歸,只遞歸一次;
step2–候選點的跟蹤
候選點的跟蹤使用離散的極線搜索來最小化光度誤差。對於最好的匹配上的點,就用這個點計算深度和相關的協方差,用於下次的極線搜索。
step3–候選點的激活
當一些點被marge了,那麼就要添加一些新的點進去,也就是active。
爲了將關鍵點分散在圖像和空間中,DSO將所有關鍵幀的active點都投影到當前幀上,然後選擇與它們距離最大的點進行激活(requiring larger distance for candidates created during the second or third block-run,這裏的block-run沒看懂是什麼意思)。
異常值和遮擋的檢測
關於異常值,1)在候選點的追蹤過程中,如果在極線搜索的時候,光度誤差明顯不是那麼小,那麼這些點會被永久的丟棄掉,說明當前幀不用它了,這種做法能大幅減少重複區域的誤匹配;2)光度誤差明顯大於閾值的要被去掉。該閾值也是根據當前幀情況變化的(在代碼裏要關注一下)。
Direct Sparse Model
該章節主要分爲三個部分:1)標定;2)模型;3)優化。下面依次總結;
相機的幾何標定
DSO使用小孔相機+徑向失真的模型來表示相機,有如下公式,其中c表示相機的內參:
相機的光度標定
DSO中採用的光度響應模型如下公式所述:
其中
- B表示該點的輻照度(irradiance),表示該點的總能量(與方向無關);
- V表示鏡頭的漸暈,值域爲;
- 表示曝光時間;
- G表示光度響應函數,作用是將能量映射爲像素值;
整個過程大概類似於下面的情形
關於裏面詳細的標定,這裏不再贅述,可以參考reference中的鏈接或者文章。
現在,假設我們進行了相機的光度標定,那麼我們就可以消除鏡頭的漸暈以及非線性光度相應函數的影響,拿到某個點真正對應的能量,亦即公式:
簡單說,對於某個點x,首先逆推回光度值,然後消除漸暈的影響,就是sensor感受到真實的能量值,這個值相比於灰度值要更加的robust。
模型公式
在開頭就介紹過,DSO採用光度誤差的方法進行優化,自然,這裏光度誤差也是對一個像素塊的光度誤差求和,正常情況下的公式如下:
其中表示是一個像素塊區域,是每個像素的權重,分別表示當前幀和參考幀,表示huber函數。
DSO在這個常規的步驟上又更近一步,因爲前面已經對相機進行了光度校正,知道了整個非線性函數以及漸暈的模型,就可以用上面的轉換公式把灰度值轉移爲觀測點的能量與曝光時間的乘積,那麼相比於灰度值,觀測點的能量要顯得更加穩定和抗干擾。因此,如果我們知道了相機的光度校正模型,那麼就可以變爲下面的公式,這裏就是經過光度校正之後的觀測點的能量與曝光時間的乘積:
但是,如果我們並不知道光度校正模型呢?作者就也想用類似與的方式來反過來獲得觀測點的能量與曝光時間的乘積,但是光度響應函數本身是一個非線性的函數,漸暈模型也是一個非線性的模型,爲了簡單起見(可以簡單認爲稍微校正一下總比什麼都不做好得多),作者把這部分建模爲線性模型:,如果我們連也不知道,那麼模型就變爲,其中。
所以,綜上所述,把兩個考慮都加進來之後,光度誤差公式就變作如下形式,其中,作者爲了將k限制爲正,並且去除求導變得越來越小的現象,把處理爲的形式:
因此在使用的時候,會有如下幾種情況:
- 知道光度校正模型,那麼就把上述中的通過公式轉化爲觀測點的能量與曝光時間的乘積,此時作者加入了一個線性矯正係數的正則項,以此來迫使兩數是一個很小的數字;
- 如果不知道光度校正模型,那麼都用線性模型進行校正,即爲0;
最後就剩下一個權重係數了,作者給出的形式爲如下形式:
當該點的梯度特別大的時候,權重相反變得比較小,c爲常數。
滑動窗口優化方法
DSO系統採用滑動窗口的方法對位姿進行局部優化,上文中也講了DSO邊緣化一幀時候滿足的條件,這部分主要是講解當老幀被踢出去的時候,算法做了些什麼。
First Estimate Jacobian
DSO使用FEJ的方法來保證信息的不丟失,這裏對論文中的公式進行一些說明:
Notations:
- 使用來表示所有的優化變量;
- 使用來表示更新的量;
根據FEJ,新的優化參數一定是以被邊緣化時優化參數爲起點,所以殘差公式如下:
公式中的所有優化參數(位姿、光度參數)均是當前的優化參數,即,所以相應的Jacobian爲:
相應的,在計算Jacobian時要把設置爲0,也就是要用第一次邊緣化時候的線性化點,以此來保證系統的可觀性不被改變;例如在k時刻進行了第一次邊緣化,邊緣化了幀,關聯幀爲,無關幀爲,記此時的估計量爲整個系統的狀態量爲,那麼在之後的優化過程中,的線性化點是不能變的,要保持k時刻的參數(即Jacobian保持相同,亦即迭代的方向要保持一致,看下圖),而通過增量方程計算出來的增量用來更新除了被邊緣化之外的參數。
這裏記錄一個小問題,以後讀完代碼或者相應的文章再回來補充:
- 如上例子,因爲與是關聯的,因此使用固定線性化點沒毛病,那對於呢?線性化點要固定嗎?
- FEJ保持的線性化點全程保持不變嗎?如果是,那麼當第一次邊緣化的時候所有的關聯幀都被邊緣化之後,是不是FEJ的作用就沒有了?如果不是,這個的線性化點又是如何更新的?
Marginalization
因爲使用了劃窗的優化,爲了保證老的信息不丟失,使用了邊緣化的方法去除劃窗中較老的關鍵幀;邊緣化的基本思路和公式可以參考賀博的博客。這次看了DSO的邊緣化之後,自己也稍微有了多一些的理解,稍後會整理一下。這裏就先對DSO的邊緣化進行說明。
在k時刻,系統進行了邊緣化,那麼此時把與邊緣化有關的參數的誤差記作,此時的參數爲,那麼針對後面的過程,其實有:
其中:
對上述公式求導並等於0,就能得到公式:
注意,上面的不是增量方程,求解的不是迭代的量,而直接是參數值! 上面的方程依舊是增量方程,但是求解的增量並不是相對於上一次迭代的增量,而是相對於當時線性化點的增量。爲此我們使用Schur補進行邊緣化,永久刪除不再使用的幀,有如下公,其中爲要保留的值,爲被邊緣化的值:
執行schur補之後得到如下公式:
所以對於被保留的部分,我們之後都可以在所有的誤差上加入先驗項:
該先驗項告訴優化器迭代的方向在的方向上,且迭代出的值應該儘量使得該誤差項變小。
在之後計算光度誤差的時候,對於保留的部分,在線性化時一定要用固定點,保證迭代的方向都是當時的方向。這點和VINS-MONO中的邊緣化策略有着較大的不同。
Summary
這裏總結一下整個DSO的特點:
- DSO是直接稀疏法的視覺里程計系統,使用圖像中部分的關鍵點進行光度誤差模型的優化;
- 整個參數空間包含相機位姿,光度模型參數,觀測點的逆深度;
- 使用劃窗的方式進行局部的位姿優化,該優化使用FEJ的方法保證不破壞系統的可觀性,使用邊緣化的方法保證被舊的幀的信息不丟失;
整個論文到這裏就不再向下展開,下面的都是作者的一些實驗以及參數的調優;下一步就是根據代碼來逐步的反饋論文中的思想。
就整個總結而言,其中一定不乏一些不對的地方,還請得道的大佬們指出來,筆者會即使改進,提前感激不盡!
Reference
- Direct Sparse Odometry
- A Photometrically Calibrated Benchmark For Monocular Visual Odometry
- https://www.cnblogs.com/luyb/p/6077478.html
- https://blog.csdn.net/heyijia0327/article/details/52822104
- Notes on DSO