SVO: 視覺SLAM中特征點(diǎn)法與直接法結(jié)合
點(diǎn)擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”
重磅干貨，第一時間送達(dá)
前面的話
VSLAM 是利用多視圖幾何理論，根據(jù)相機(jī)拍攝的圖像信息對相機(jī)進(jìn)行定位并同時構(gòu)建周圍環(huán)境地圖。VSLAM?前端為視覺里程計和回環(huán)檢測，相當(dāng)于是對圖像數(shù)據(jù)進(jìn)行關(guān)聯(lián)；后端是對前端輸出的結(jié)果進(jìn)行優(yōu)化，利用濾波或非線性優(yōu)化理論，得到最優(yōu)的位姿估計和全局一致性地圖。
?
特征點(diǎn)法精度高，直接法速度快，兩者是否可以結(jié)合呢？
SVO（Semi-Direct Monocular Visual Odometry）是蘇黎世大學(xué)?Scaramuzza?教授的實(shí)驗(yàn)室，在?2014?年發(fā)表的一種視覺里程計算法，它的名稱是半直接法視覺里程計，通俗點(diǎn)說，就是結(jié)合了特征點(diǎn)法和直接法的視覺里程計。
目前該算法已經(jīng)在 github 上面 開 源 （https://github.com/uzh-rpg/rpg_svo ）。?賀 一 家 在 它 的 開 源 版 本 上 面 進(jìn) 行 改 進(jìn) ， 形 成了 SVO_Edgelet（https://github.com/HeYijia/svo_edgelet）。相比原版， SVO_Edgelet 增加了一些功能，比如結(jié)合本質(zhì)矩陣和單應(yīng)矩陣來初始化，把邊緣特征點(diǎn)加入跟蹤等，對 SVO 的魯棒性有較大的改善。?
SVO 面向無人機(jī)航拍場合，將特征點(diǎn)法與直接法結(jié)合，跟蹤關(guān)鍵點(diǎn)，不計算描述子，根據(jù)關(guān)鍵點(diǎn)周圍的小圖像塊的信息估計相機(jī)的運(yùn)動。主要分運(yùn)動估計線程和地圖構(gòu)建線程兩個線程。
其中運(yùn)動估計分為如下三步：?
1）圖像對齊。如圖所示，通過當(dāng)前幀和參考幀中的特征點(diǎn)對的 patch(特征點(diǎn)周圍 4*4 區(qū)域)的灰度差異，構(gòu)建光度誤差的優(yōu)化函數(shù)
其中，
公式中第一步為根據(jù)圖像位置和深度逆投影到三維空間，第二步將三維坐標(biāo)點(diǎn)旋轉(zhuǎn)平移到當(dāng)前幀坐標(biāo)系下，第三步再將三維坐標(biāo)點(diǎn)投影回當(dāng)前幀圖像坐標(biāo)。當(dāng)然在優(yōu)化過程中，殘差的計算方式不止這一種形式：有前向(forwards)，逆向(inverse)之分，并且還有疊加式(additive)和構(gòu)造式(compositional)之分。
優(yōu)化變量為相機(jī)的變換矩陣 T，采用高斯牛頓迭代求解，然后尋找更多的地圖點(diǎn)到當(dāng)前幀圖像的對應(yīng)關(guān)系。?
2）特征對齊。如圖，由于深度估計和相機(jī)位姿的不準(zhǔn)導(dǎo)致預(yù)測的特征塊位置不準(zhǔn)，通過光流跟蹤對特征點(diǎn)位置進(jìn)行優(yōu)化，具體為：對每個當(dāng)前幀能觀察到的地圖點(diǎn) p（深度已收斂），找到觀察 p 角度最小的關(guān)鍵幀 r 上的對應(yīng)點(diǎn) ui，得到 p 在當(dāng)前幀上的投影。優(yōu)化的目標(biāo)函數(shù)是特征塊（8*8的 patch ） 及 其 在 仿 射 變 換 下 的 灰 度 差
其中：Ai 表示仿射變換；?優(yōu)化變量為特征點(diǎn)位置 ui’。?
3）位姿結(jié)構(gòu)優(yōu)化。?如圖所示，像素位置優(yōu)化后，利用建立的對應(yīng)關(guān)系對空間三維點(diǎn)和相機(jī)位置進(jìn)行分別優(yōu)化，構(gòu) 建 像 素 重 投 影 誤 差 的 優(yōu) 化 函 數(shù)
優(yōu)化變量為相機(jī)的變換矩陣 T或空間三維點(diǎn) P。
地圖構(gòu)建主要是深度估計。如圖所示，它采用概率模型， 高斯分布加上一個設(shè)定在最小與最大 深 度 之 間 的 均 勻 分 布 ：
并推導(dǎo)了均勻—高斯混合分布的深度濾波器，采用逆深度作為參數(shù)化形式。當(dāng)出現(xiàn)新的關(guān)鍵幀時，選取若干種子點(diǎn)，每個種子點(diǎn)根據(jù)變換矩陣得到對應(yīng)的極線，在極線上找到特征點(diǎn)的對應(yīng)點(diǎn)，通過三角測量計算深度和不確定性；然后不斷更新其估計， 直到深度估計收斂到一定程度，將該三維坐標(biāo)加入地圖。
?
SVO算法流程
主要流程圖為：
下面，結(jié)合代碼進(jìn)行算法流程的詳細(xì)分析：
1.跟蹤
其實(shí)， SVO 的跟蹤部分的本質(zhì)是跟 ORBSLAM 一樣的，只是匹配的方法從特征點(diǎn)法改成了灰度值匹配法。
但是，隨后，與 ORBSLAM 有不同的地方，?SVO 在優(yōu)化出相機(jī)位姿之后，還有可選項，可以再優(yōu)化地圖點(diǎn)，還可以再把地圖點(diǎn)和相機(jī)位姿一起優(yōu)化。
1.1 初始化
圖像剛進(jìn)來的時候，就獲取它的金字塔圖像， 5 層，比例為 2。
然后處理第一張圖像， processFirstFrame()。先檢測 FAST 特征點(diǎn)和邊緣特征。如果圖像中間的特征點(diǎn)數(shù)量超過 50 個，就把這張圖像作為第一個關(guān)鍵幀。
然后處理第一張之后的連續(xù)圖像， processSecondFrame()， 用于跟第一張進(jìn)行三角初始化。從第一張圖像開始，就用光流法持續(xù)跟蹤特征點(diǎn)， 把特征像素點(diǎn)轉(zhuǎn)換成在相機(jī)坐標(biāo)系下的深度歸一化的點(diǎn)，并進(jìn)行畸變校正,再讓模變成 1,映射到單位球面上面。
如果匹配點(diǎn)的數(shù)量大于閾值，并且視差的中位數(shù)大于閾值。如果視差的方差大的話，選擇計算 E 矩陣，如果視差的方差小的話，選擇計算 H 矩陣。如果計算完 H 或 E 后，還有足夠的內(nèi)點(diǎn)，就認(rèn)為這幀是合適的用來三角化的幀。根據(jù) H 或 E 恢復(fù)出來的位姿和地圖點(diǎn)，進(jìn)行尺度變換，把深度的中值調(diào)為 1。
然后把這一幀，作為關(guān)鍵幀，送入到深度濾波器中。（就是送到的深度濾波器的 updateSeedsLoop()線程中。?深度濾波器來給種子點(diǎn)在極線上搜索匹配點(diǎn)，更新種子點(diǎn)，種子點(diǎn)收斂出新的候選地圖點(diǎn)。如果是關(guān)鍵幀的話，就初始化出新的種子點(diǎn)， 在這幀圖像里面每層的每個 25x25 大小的網(wǎng)格里，取一個最大的 fast 點(diǎn)。在第 0 層圖像上，找出 canny 邊緣點(diǎn)。）
之后就是正常的跟蹤 processFrame()。
1.2 基于稀疏點(diǎn)亮度的位姿預(yù)估
把上一幀的位姿作為當(dāng)前幀的初始位姿。把上一幀作為參考幀。
先創(chuàng)建 n 行 16 列的矩陣 ref_patch_cache_， n 表示參考幀上面的特征點(diǎn)的個數(shù)， 16 代表要取的圖塊的像素數(shù)量。
再創(chuàng)建 6 行 n*16 列的矩陣 jacobian_cache_。?代表圖塊上的每個像素點(diǎn)誤差對相機(jī)位姿的雅克比。
要優(yōu)化的是參考幀相對于當(dāng)前幀的位姿。
把參考幀上的所有圖塊結(jié)合地圖點(diǎn)，往當(dāng)前幀圖像的金字塔圖像上投影。在當(dāng)前幀的金字塔圖像上，從最高層開始，一層層往低層算。每次繼承前一次的優(yōu)化結(jié)果。如果前一次的誤差相比前前次沒有減小的話，就繼承前前次的優(yōu)化后的位姿。?每層的優(yōu)化，迭代 30 次。
要優(yōu)化的殘差是，?參考幀 k-1上的特征點(diǎn)的圖塊與投影到當(dāng)前幀 k上的位置上的圖塊的亮度殘差。?投影位置是，參考幀中的特征點(diǎn)延伸到三維空間中到與對應(yīng)的地圖點(diǎn)深度一樣的位置，然后投影到當(dāng)前幀。
這是 SVO 的一個創(chuàng)新點(diǎn)，直接由圖像上的特征點(diǎn)延伸出來，而不是地圖點(diǎn)（因?yàn)榈貓D點(diǎn)與特征點(diǎn)之間也存在投影誤差），這樣子就保證了要投影的圖塊的準(zhǔn)確性。延伸出來的空間點(diǎn)肯定也與特征點(diǎn)以及光心在一條直線上。這樣子的針孔模型很漂亮。
SVO 的另外一個創(chuàng)新點(diǎn)是，以當(dāng)前幀上的投影點(diǎn)的像素值為基準(zhǔn)，通過優(yōu)化調(diào)整參考幀投影過來的像素點(diǎn)的位置，以此來優(yōu)化這兩者像素值殘差。?這樣子，投影過來的圖塊patch上的像素值關(guān)于像素點(diǎn)位置的雅克比，就可以提前計算并且固定了。（而以前普通的方法是，以參考幀投影過去的像素值為基準(zhǔn)，通過優(yōu)化投影點(diǎn)的位置，來優(yōu)化這兩者的殘差。）?
采用近似的思想。首先， 認(rèn)為空間點(diǎn) p 固定不動， 只調(diào)整參考幀k-1的位姿，所以殘差的擾動不影響在當(dāng)前幀上的投影點(diǎn)的位置，只會影響圖塊patch的內(nèi)容。然后，參考幀在新的位姿上重新生成新的空間點(diǎn)，再迭代下去。雖然只是近似優(yōu)化，
但每次迭代的方向都是對的， 假設(shè)步長也差不多， 所以最終也可以優(yōu)化成功。?
1.3 基于圖塊的特征點(diǎn)匹配
因?yàn)楫?dāng)前幀有了 1.1 的預(yù)估的位姿。對于關(guān)鍵幀鏈表里面的那些關(guān)鍵幀，把它們圖像上的分散的 5 點(diǎn)往當(dāng)前幀上投影，看是否能投影成功，如果能投影成功，就認(rèn)為共視。再把所有的共視關(guān)鍵幀，按照與當(dāng)前幀的距離遠(yuǎn)近來排序。然后，按照關(guān)鍵幀距離從近到遠(yuǎn)的順序，依次把這些關(guān)鍵幀上面的特征點(diǎn)對應(yīng)的地圖點(diǎn)都往當(dāng)前幀上面投影，同一個地圖點(diǎn)只被投影一次。如果地圖點(diǎn)在當(dāng)前幀上的投影位置，能取到 8x8 的圖塊，就把這個地圖點(diǎn)存入到當(dāng)前幀投影位置的網(wǎng)格中。
再把候選地圖點(diǎn)都往當(dāng)前幀上投影，如果在當(dāng)前幀上的投影位置，能取到 8x8 的圖塊，就把這個候選地圖點(diǎn)存入到當(dāng)前幀投影位置的網(wǎng)格中。如果一個候選點(diǎn)有 10 幀投影不成功，就把這個候選點(diǎn)刪除掉。
然后，對于每一個網(wǎng)格， 把其中對應(yīng)的地圖點(diǎn)，按照地圖點(diǎn)的質(zhì)量進(jìn)行排序（TYPE_GOOD> TYPE_UNKNOWN> TYPE_CANDIDATE>TYPE_DELETED）。如果是 TYPE_DELETED，則在網(wǎng)格中把它刪除掉。
遍歷網(wǎng)格中的每個地圖點(diǎn)，找到這個地圖點(diǎn)被觀察到的所有的關(guān)鍵幀。獲取那些關(guān)鍵幀光心與這個地圖點(diǎn)連線，與地圖點(diǎn)與當(dāng)前幀光心連線的夾角。選出夾角最小的那個關(guān)鍵幀作為參考幀，以及對應(yīng)的特征點(diǎn)。（注意，這里的這種選夾角的情況，
是只適合無人機(jī)那樣的視角一直朝下的情況的，應(yīng)該改成 ORBSLAM 那樣，還要再把視角轉(zhuǎn)換到對應(yīng)的相機(jī)坐標(biāo)系下，再篩選一遍）。這個對應(yīng)的特征點(diǎn)，必須要在它自己的對應(yīng)的層數(shù)上， 能獲取 10x10 的圖塊。
然后，計算仿射矩陣。首先，獲取地圖點(diǎn)在參考幀上的與光心連線的模。然后它的對應(yīng)的特征點(diǎn)，在它對應(yīng)的層數(shù)上，取右邊的第 5 個像素位置和下邊的第 5 個像素位置，再映射到第 0 層。再轉(zhuǎn)換到單位球上，再映射到三維空間中，直到與地圖點(diǎn)的模一樣的長度。把對應(yīng)的特征點(diǎn)也映射到三維空間中，直到與地圖點(diǎn)的模一樣的長度。然后，再把這 3 個點(diǎn)映射到當(dāng)前幀的（有畸變的） 圖像上。根據(jù)它們與中心投影點(diǎn)的位置變換， 算出了仿射矩陣 A_cur_ref。A_cur_ref.col(0) = (px_du-px_cur)/halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;。?
仿射矩陣 A，就是把參考幀上的圖塊在它自己對應(yīng)的層數(shù)上， 轉(zhuǎn)換到當(dāng)前幀的第 0 層上。（這種把比例變換轉(zhuǎn)換成矩陣表示的方法，很好）。
然后，?計算在當(dāng)前幀的目標(biāo)搜索層數(shù)。通過計算仿射矩陣 A_cur_ref 的行列式，其實(shí)就是面積放大率。如果面積放大率超過3，就往上一層，面積放大率變?yōu)樵瓉淼乃姆种弧Ｖ烂娣e放大率不再大于 3，或者到最高層。就得到了目標(biāo)要搜索的層數(shù)。
然后，計算仿射矩陣的逆仿射矩陣?A_ref_cur。然后，這樣子，如果以投影點(diǎn)為中心（5,5），取 10x10 的圖塊，則圖塊上每個像素點(diǎn)的（相對中心點(diǎn)的） 位置，都可以通過逆仿射矩陣，得到對應(yīng)的參考幀上的對應(yīng)層數(shù)圖像上的（相對中心點(diǎn)的） 像素位置。進(jìn)行像素插值。就是，把參考幀上的特征點(diǎn)附近取一些像素點(diǎn)過來，可以組成， 映射到當(dāng)前幀上的對應(yīng)層數(shù)的投影點(diǎn)位置的附近，這些映射到的位置剛好組成 10x10 的圖塊。
然后， 從映射過來的 10x10 的圖塊中取出 8x8 的圖塊，作為參考圖塊。對這個圖塊的位置進(jìn)行優(yōu)化調(diào)整，使得它與目標(biāo)位置的圖塊匹配。
在這里， SVO 有兩個創(chuàng)新點(diǎn)。
第一個創(chuàng)新的地方是。因?yàn)橐话闱闆r下，是基于自己圖塊不變，通過優(yōu)化u'使得投影位置的圖塊跟自己最接近。而?SVO 是投影位置的圖塊不變， 通過優(yōu)化u使得自己圖塊與投影位置的圖塊最接近。這樣的話，就可以避免重復(fù)計算投影位置圖塊像素關(guān)于位置的雅克比了。因?yàn)樽约簣D塊是固定的，所以雅克比是固定的，所以只需要計算一次。其實(shí)，這個創(chuàng)新點(diǎn)與 1.2 中的反向創(chuàng)新點(diǎn)一樣，都是用近似優(yōu)化的方法來。因?yàn)?，如果是一般的方法的話，計算目?biāo)投影位置的圖塊的雅克比，是知道自己參考圖塊重新移動后，會遇到怎樣的目標(biāo)圖塊。而這個反向的方法，并不知道重新移動后會遇到怎樣的圖塊，只知道移動后，對當(dāng)前的目標(biāo)圖塊可以匹配得更好。也是一種迭代，近似優(yōu)化的方法，但速度可以塊很多，避免了重復(fù)計算雅克比。
第二個創(chuàng)新的地方是。一般情況下，兩圖塊的均值差m，都是直接把兩個圖塊的均值相減的。但是，這樣子的話，可能容易受某些極端噪聲的影響。所以， SVO 中，直接把m也作為優(yōu)化變量了。?
1.4?進(jìn)一步優(yōu)化位姿和地圖點(diǎn)
針對每個地圖點(diǎn)， 優(yōu)化地圖點(diǎn)的三維位置， 使得它在它被觀察到的每個關(guān)鍵幀上的重投影誤差最小。?每個地圖點(diǎn)優(yōu)化 5 次。
如果這次優(yōu)化的誤差平方和小于上次優(yōu)化的誤差平方和，就接受這次的優(yōu)化結(jié)果。（注意，這里的平方和也是在優(yōu)化之前算的，其實(shí)應(yīng)該在優(yōu)化之后算）。如果是邊緣點(diǎn)的話，則把重投影誤差映射到梯度方向上，成為梯度方向上的模，就是與梯度方向進(jìn)行點(diǎn)積。相應(yīng)的，雅克比也左乘對應(yīng)的梯度方向。相當(dāng)于是，優(yōu)化重投影誤差在梯度方向上的映射。
2.創(chuàng)建地圖點(diǎn)
特征點(diǎn)提取的方法，放在了地圖線程里。因?yàn)榕c ORBSLAM 不同的是，它跟蹤的時候，不需要找特征點(diǎn)再匹配，而是直接根據(jù)圖塊亮度差匹配的。而如果是 vins 的話，也可以參考這個方法，把特征點(diǎn)提取放到地圖線程里，連續(xù)幀之間的特征點(diǎn)用光流匹配。但光流要求幀與幀之間不能差別太大。
而在 SVO 中，后端的特征點(diǎn)是只在關(guān)鍵幀上提取的，用 FAST 加金字塔。而上一個關(guān)鍵幀的特征點(diǎn)在這一個關(guān)鍵幀上找匹配點(diǎn)的方法，是用極線搜索，尋找亮度差最小的點(diǎn)。最后再用 depthfilter 深度濾波器把這個地圖點(diǎn)準(zhǔn)確地濾出來。
選取 30 個地圖點(diǎn)，如果這 30 個地圖點(diǎn)在當(dāng)前幀和最近一個關(guān)鍵幀的視差的中位數(shù)大于 40， 或者與窗口中的關(guān)鍵幀的距離大于一定閾值， 就認(rèn)為需要一個新的關(guān)鍵幀。然后把當(dāng)前幀設(shè)置為關(guān)鍵幀，對當(dāng)前幀進(jìn)行操作。
SVO 中重定位，實(shí)現(xiàn)很簡單，就是在跟丟之后，仍然假設(shè)當(dāng)前幀的位姿和前一幀一樣，往這個位姿上投地圖點(diǎn)，用第 1 部分中的方法去優(yōu)化計算，如果優(yōu)化成功，就重定位回來，如果優(yōu)化不成功，就繼續(xù)下一幀。所以，在跟丟后，只能再回到跟丟時的位置，才能重定位回來。
?
?
SVO 在運(yùn)動估計中的思路很新穎，運(yùn)行速度非常快，由于不用計算描述子，也不用處理過多的地圖點(diǎn)云，在普通 PC上也能達(dá)到 100 Frame /S 以上。
但是正因?yàn)樗哪繕?biāo)應(yīng)用平臺是無人機(jī)，所以它在其他的場合應(yīng)用是不適合的，至少是需要修改的。例如在單目初始化時，是采用分解 H，這需要假設(shè)前兩個關(guān)鍵幀的特征點(diǎn)位于一個平面上。再者，在關(guān)鍵幀的選取策略上，采用的是平移量，沒有考慮旋轉(zhuǎn)。而且它是一個輕量級的相機(jī)運(yùn)動估計，沒有閉環(huán)功能，沒有重定位，建圖功能也基本沒有，即使如此，它也不失為一個優(yōu)秀的SLAM-DEMO。
2016 年， Forster 等人對 SVO 進(jìn)行改進(jìn)，形成 SVO2.0 版本，新的版本作出了很大的改進(jìn)，增加了邊緣的跟蹤，并且考慮了 IMU 的運(yùn)動先驗(yàn)信息，支持大視場角相機(jī)（如魚眼相機(jī)和全景相機(jī)）和多相機(jī)系統(tǒng)。
值得一提的是， Foster 對 VIO 的理論也進(jìn)行了詳細(xì)的推導(dǎo)， 尤其是關(guān)于預(yù)積分的論文成為后續(xù) VSLAM 系統(tǒng)融合 IMU 的理論指導(dǎo)。
SVO 的定位很好，抖動很小。尤其在重復(fù)紋理的環(huán)境中，表現(xiàn)得比基于特征點(diǎn)法的 ORBSLAM2 要出色。將來可以在上面增加更魯棒的重定位，回環(huán)閉環(huán)，全局地圖的功能，來滿足更多的實(shí)際應(yīng)用場景，比如室內(nèi)機(jī)器人、無人機(jī)、無人車。
參考：
1. Forster C, Pizzoli M, Scaramuzza D. SVO: Fast semi-direct monocular visual odometry[C]// IEEE International Conference on Robotics and Automation.
IEEE, 2014:15-22.
2.??https://blog.csdn.net/heyijia0327/article/details/51083398
3.? https://www.cnblogs.com/luyb/p/5773691.html
4. 《視覺SLAM十四講：從理論到實(shí)踐 第2版》高翔等人
交流群
歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學(xué)影像、GAN、算法競賽等微信群（以后會逐漸細(xì)分），請掃描下面微信號加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進(jìn)入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~