本文介紹一種單目視覺(jué)里程計(jì)的方法，重點(diǎn)說(shuō)明當(dāng)前幀的半稠密深度圖估計(jì)過(guò)程，估計(jì)到的半稠密深度圖反過(guò)來(lái)又可以用于跟蹤當(dāng)前相機(jī)位姿狀態(tài)。這里只考慮估計(jì)圖像中梯度足夠大的像素，對(duì)每一個(gè)像素估計(jì)的深度都用一個(gè)高斯概率分布來(lái)表示（基于一個(gè)先驗(yàn)假設(shè)，所有像素的逆深度服從高斯分布），當(dāng)新的圖像到來(lái)后，我們需要將估計(jì)到的信息傳遞到新的一幀，并且結(jié)合新的圖像更新這個(gè)估計(jì)值。
對(duì)應(yīng)的文章見參考文獻(xiàn)[1]。
本文分為三個(gè)部分
- 對(duì)本文視覺(jué)里程計(jì)總覽
- 半稠密深度圖的建圖方法（重點(diǎn)說(shuō)明）
- 對(duì)新幀的跟蹤
一、系統(tǒng)總覽
系統(tǒng)主要分為兩個(gè)線程：跟蹤（使用最近的深度圖跟蹤新幀的位姿狀態(tài)）和深度估計(jì)（連續(xù)地將逆深度值傳遞到下一幀，并使用立體視覺(jué)比較估計(jì)新幀的逆深度值，最后再使用正則化去除局外點(diǎn)）。
初始兩幀的位姿使用基于關(guān)鍵點(diǎn)的方法得到，這個(gè)位姿被當(dāng)作已知用于后續(xù)的深度估計(jì)。實(shí)驗(yàn)表明這個(gè)初始的位姿即使是非常不精確的，后續(xù)的方法也可以恢復(fù)出一個(gè)精確的深度圖。
二、半稠密深度圖估計(jì)
深度圖的估計(jì)分為三個(gè)主要的部分：
- 使用立體視覺(jué)的方法得到測(cè)量值，并且將其與先驗(yàn)深度相合并
- 深度圖在幀與幀之間的傳播
- 正則化和局外點(diǎn)的去除
2.1基于立體視覺(jué)的深度更新
整體的深度圖更新步驟如下：
- 提取視差搜索精度足夠大的像素
- 對(duì)每一個(gè)選取的像素，單獨(dú)為其選擇合適的參考幀，并執(zhí)行一個(gè)一維的視差搜索
- 傳遞的先驗(yàn)被用來(lái)減小視差搜索范圍、降低計(jì)算復(fù)雜度、消除錯(cuò)誤極小值
- 將獲得的深度估計(jì)融合到深度圖中
2.1.1參考幀的選取
理想情況下，參考幀的選取需要滿足最大化立體精度，同時(shí)保證視差搜索范圍和觀察視角足夠小。選擇參考幀的策略是：包含該像素的最早的一幀圖像，同時(shí)要滿足視差搜索范圍和觀察視角的條件，否則就向后延一幀。
2.1.2立體匹配的方法
在參考幀沿著極線方向窮盡搜索對(duì)應(yīng)的像素強(qiáng)度，并且對(duì)匹配視差進(jìn)行亞像素精度定位。如果先驗(yàn)的逆深度假設(shè)存在，則搜索的范圍就限制在d+-2sigma_d的范圍，這里的d和sigma_d表示先驗(yàn)假設(shè)中的均值和方差。如果沒(méi)有先驗(yàn)，則要搜索整條極線。
2.1.3不確定度的估計(jì)
本小節(jié)主要說(shuō)明如何推導(dǎo)逆深度值d的方差。
我們可以把最優(yōu)的逆深度值作為輸入?yún)?shù)的函數(shù)：
則其方差可以如下給定：
這里的Σ表示輸入?yún)?shù)的方差。
為了分析不確定度，我們將不確定度的計(jì)算分為三個(gè)步驟：
- 在參考幀上計(jì)算極線
- 確定最佳視差匹配位置λ
- 根據(jù)視差λ計(jì)算逆深度值
前兩個(gè)步驟涉及到兩個(gè)誤差：幾何誤差（第一步，來(lái)自于ξ和π）、光度誤差（第二步，來(lái)自于圖像噪聲）
2.1.3.1幾何誤差
我們將極線定義如下
這里的λ對(duì)應(yīng)的是視差搜索間隔，（lx，ly）是歸一化極線方向，l0是對(duì)應(yīng)無(wú)限深度的點(diǎn)。
我們現(xiàn)在考慮l0位置對(duì)視差誤差的影響，這個(gè)位置只受影響于各向同性的高斯噪聲εl，如圖1所示，圖中下方直線上的點(diǎn)表示l0的真值位置，由于高斯噪聲的存在，這個(gè)位置被確定為上方直線左邊點(diǎn)的位置，由此造成了極線的位置被確定為上方直線（下方直線才是其真值），在極線上搜索與下方黑點(diǎn)梯度相同的位置即為視差匹配點(diǎn)。由圖可知，當(dāng)極線方向越接近梯度方向時(shí)，誤差越小，當(dāng)極線方向越垂直梯度方向時(shí)，誤差越大。
我們假定等梯度線在局部是線性的，即局部的梯度方向是一個(gè)常量，可以給出下式：
其中g(shù)=（gx，gy）是梯度方向，g0表示等梯度線上的一點(diǎn)。將上式l0移到等式右邊，兩邊同乘g，最后將左邊系數(shù)移到右邊，可以得到下式：
根據(jù)方差傳遞公式，可以得到：
g表示圖像歸一化梯度方向，l表示歸一化極線方向，sigma_l是誤差epsilon_l的方差，由此可以看出，幾何誤差項(xiàng)只取決于ξ和π。
2.1.3.2光度誤差
我們同過(guò)最小化圖像亮度差的方式尋找視差匹配，如下式：
其中i_ref是參考幀圖像的強(qiáng)度，I_p是在極線上視差為λ對(duì)應(yīng)位置的強(qiáng)度，我們假定已經(jīng)有一個(gè)良好初始化的λ0存在，在I_p位置使用一階泰勒近似展開，得到下式：
其中g(shù)_p表示I_p沿著極線方向的梯度。使用方差傳遞公式可以得到光度誤差的不確定度：
sigma_i是圖像亮度噪聲的方差。
如圖2所示，圖像沿極線方向梯度越大，光度視差誤差越小，梯度越小，則光度誤差視差越大。
2.1.3.3像素到逆深度的轉(zhuǎn)換
對(duì)于微小旋轉(zhuǎn)的情況，逆深度值近似與視差成比例，逆深度的測(cè)量方差可以如下式計(jì)算：
這個(gè)比例系數(shù)α在圖像沒(méi)有對(duì)齊的情況下，每個(gè)像素的值通常是不同的，其取決于平移方向和像素在圖像上的位置，因此我們通過(guò)下式來(lái)計(jì)算：
其中，分子是搜索逆深度值的間隔的長(zhǎng)度，分母是搜索的極線段的長(zhǎng)度。
2.1.4測(cè)量深度值融合
獲得了當(dāng)前幀的深度測(cè)量值之后，我們就要將其融合的深度圖中，如果沒(méi)有對(duì)應(yīng)的先驗(yàn)假設(shè)，我們就將其直接合并進(jìn)深度圖中，如果存在先驗(yàn)深度，我們就需要將觀測(cè)值與先驗(yàn)值合并起來(lái)更新深度圖，對(duì)應(yīng)的更新公式如下：
2.1.對(duì)不確定度的總結(jié)
- 光度誤差：取決于圖像沿極線方向的梯度大小
- 幾何誤差：取決于圖像方向與極線方向的夾角
- 像素到逆深度比例：取決于相機(jī)平移、焦距和像素位置。
2.2深度圖傳播
深度圖的傳播指的是根據(jù)估計(jì)得到的對(duì)應(yīng)像素的逆深度d0，計(jì)算其3D位置，再將其投影到新的一幀中，從而得到新的逆深度d1的過(guò)程。
我們假設(shè)相機(jī)旋轉(zhuǎn)很小，d1可以如下計(jì)算：
tz是相機(jī)沿著光軸方向的平移，d1對(duì)應(yīng)的方差可以如下給定
sigma_p表示預(yù)測(cè)不確定度。
如果兩個(gè)深度值傳遞給了同一個(gè)像素，我們會(huì)采用如下兩種方式處理：
- 如果它們是相似的，即小于2倍sigma，就將其合并起來(lái)（使用深度值融合公式）
- 否則，離相機(jī)較遠(yuǎn)的點(diǎn)被去除，因?yàn)槠浔灰暈檎趽醯狞c(diǎn)。
2.3深度圖的正則化
我們使用鄰域像素逆深度的加權(quán)平均作為該點(diǎn)的逆深度值，權(quán)重即為各像素對(duì)應(yīng)的方差的倒數(shù)。為了保留邊緣，逆深度的差大于2倍sigma的鄰域像素不參與平均。在正則化過(guò)程中，每個(gè)像素對(duì)應(yīng)的方差不發(fā)生變化，我們?cè)诙x立體搜索范圍時(shí)，使用鄰域中最小的方差，并將其作為tracking線程中的權(quán)重因子。
2.3.1局外點(diǎn)（outlier）移除
在傳遞過(guò)程中，我們將每個(gè)逆深度假設(shè)的有效性（即其為局外點(diǎn)或變無(wú)效的概率）作為其是否為局外點(diǎn)的標(biāo)準(zhǔn)，如果一個(gè)像素的亮度發(fā)生顯著變化，或者其圖像梯度下降到一定的閾值，立體匹配就是失敗的，其無(wú)效的概率就會(huì)上升。當(dāng)概率大于某一閾值時(shí)，就將其移除，同樣地，當(dāng)其小于一定的閾值時(shí)，就要利用其鄰域像素，將該像素逆深度值創(chuàng)建出來(lái)。
三、直接法跟蹤相機(jī)位姿
該部分可參考我的博客直接法估計(jì)相機(jī)位姿。在此不做詳細(xì)的說(shuō)明。
參考文獻(xiàn)
[1] Engel J, Sturm J, Cremers D. Semi-dense visual odometry for a monocular camera[C]//Proceedings of the IEEE international conference on computer vision. 2013: 1449-1456.
