VIO在走廊弱紋理環(huán)境下的優(yōu)化——VINS-Mono的點線緊耦合優(yōu)化

• VIO在走廊弱紋理環(huán)境下的優(yōu)化
• • 0. 前言
  • 1. 思路概述
  • • 1.1 Super Pixel SLAM
    • 1.2 Edge SLAM
    • 1.3 PL SLAM
  • 2. 算法實施
  • • 2.1 Edge SLAM方案的實施（簡述）
    • 2.2 PL SLAM方案的實施
    • • 2.2.1 線特征的提取和跟蹤
      • 2.2.2 線特征的構(gòu)建和管理
      • 2.2.3 線特征的非線性優(yōu)化
      • 2.2.4 線特征的邊緣化操作
  • 3. 實驗結(jié)果
  • 4. 結(jié)論
  • 參考文獻(xiàn)

VIO在走廊弱紋理環(huán)境下的優(yōu)化

0. 前言

本項目是由我和leeayu共同完成，歷時一個半月時間，采用的框架是VINS-mono，開源代碼如下：

VINS-Mono-Optimization

運行結(jié)果視頻

VINS-Mono點線緊耦合優(yōu)化視頻

下面主要從四個方面進(jìn)行技術(shù)報告。

思路概述：該部分主要總結(jié)了我們前期進(jìn)行的文獻(xiàn)調(diào)研，對于弱紋理下SLAM的算法優(yōu)化一共有三種方式，我們進(jìn)行了對比和分析。

算法實施：我們通過上述的的分析對其中兩種方案（Edge SLAM和PL SLAM）進(jìn)行了實施，該部分主要詳細(xì)介紹了這兩種方案的算法細(xì)節(jié)。

實驗結(jié)果：我們選取了PL SLAM作為我們的研究重點進(jìn)行了數(shù)據(jù)集測試，該部分介紹了實驗結(jié)果。

結(jié)論：我們對算法的實驗結(jié)果和存在的不足進(jìn)行了詳細(xì)分析。

1. 思路概述

對于課題要求我們先是進(jìn)行了大量的文獻(xiàn)調(diào)研，調(diào)研結(jié)果一共有如下三種方案。

1.1 Super Pixel SLAM

這是2014年的一篇文章[1]，框架的思路大致是利用超像素作為前端，采用優(yōu)化的方式進(jìn)行匹配。因為超像素是像素的集合，對于低紋理環(huán)境擁有較好的描述，因此可以用在低紋理環(huán)境下。這是論文的視頻鏈接，我們認(rèn)為這種思路只是理論上可行，在論文的視頻里可以看出來，運行的環(huán)境其實是有限的而且非常慢，因此我們首先排除掉了這種方案。

1.2 Edge SLAM

這里主要是2017年的一篇文章[2][3]，算法的思路大致是不是角點比較少嘛，但是肯定有邊的呀，沒有角點我們跟蹤邊界點就好了嘛，因此文章就采用了一種較為魯棒的的邊界點提取方式，然后采用光流法進(jìn)行跟蹤，看上去合情合理，視頻鏈接看上去效果也不錯，因此我們第一步先實施了這種方案，但是效果并不理想，進(jìn)而我們轉(zhuǎn)戰(zhàn)PL-SLAM。

1.3 PL SLAM

點線結(jié)合的SLAM相關(guān)文章由很多，我們主要參考的是賀博在2018年的PL-VIO[4]和謝曉佳師兄的畢業(yè)碩士論文[5]，實施的具體細(xì)節(jié)在后文中進(jìn)行展開，我們最后提交的就是這個方案實施的代碼。

2. 算法實施

上文提到，我們主要對Edge SLAM和PL SLAM兩種方案進(jìn)行了實施，因此我們把兩部分的工作都介紹下，其中Edge SLAM的方案因為最后沒有采用所以就進(jìn)行簡述

2.1 Edge SLAM方案的實施（簡述）

我們剛開始是希望尋找一種簡便的方式完成任務(wù)，Edge SLAM的方案只需要修改前端，后端完全不用改，工作量相對較小（事實證明我們想多了，移植算法也花了不少時間，關(guān)鍵效果還不好），論文中對于算法是這樣描述的：

Our proposed Edge SLAM pipeline detects edge points from images and tracks those using optical flow for point correspondence. We further refine these point correspondences using geometrical relationship among three views.

大致意思就是說先對邊界點進(jìn)行提取，然后采用光流法進(jìn)行跟蹤，采用三視圖的方法進(jìn)行外點的剔除，其中對邊界點的提取方法如下：

e find the DoG based edge detector is reliable due to its robustness in illumination and contrast changes. We thin the edges further to generate edges of a single pixel width. We apply an edge filtering process described by Juan and Sol upon the thinned edges to calculate connectivity of the edge points.

采用高斯差分算子進(jìn)行提取，然后通過濾波的方式進(jìn)行邊緣細(xì)化，使得每條邊界只有一個像素點寬。
當(dāng)我們按照論文中的方式實施算法之后發(fā)現(xiàn)，進(jìn)行光流跟蹤之后匹配的邊界點會沿著邊界進(jìn)行滑移，由于這種滑移會使得邊界點在外點剔除后數(shù)量急劇減少，因此導(dǎo)致邊界點過少而跟蹤失敗。

我們對此進(jìn)行了原因分析，我們的結(jié)論是：因為光流法假設(shè)的是光度不變性，而邊界上的點都會具有相似的光度，因此光流法跟蹤會使得匹配的邊界點沿著邊界進(jìn)行滑移，這是光流法的原理導(dǎo)致的問題，我們并不清楚原論文中采用了怎樣的trick解決的這個問題，再我們開會討論之后決定放棄這種方案，轉(zhuǎn)戰(zhàn)點線特征結(jié)合的SLAM。

2.2 PL SLAM方案的實施

我們整個的方案實施實在VINS-mono上完成的，關(guān)于VINS-mono的介紹我就不再贅述了，這里主要說明下線特征是如何添加到VINS-mono中的。

2.2.1 線特征的提取和跟蹤

線特征的提取和描述采用的是LSD提取算法+LBD描述子的經(jīng)典方案，關(guān)于這兩者的細(xì)節(jié)也不在此展開，我們調(diào)用的是OpenCV庫里面的接口。
線特征的更蹤和點特征稍由不同的是我們提取的是每一幀圖片上的線特征，然后進(jìn)行前后幀匹配，以達(dá)到更蹤的效果，而不是像點特征那樣采用光流法更蹤。
描述線特征的是線段的兩個端點，我們通過將這兩個端點投影到歸一化平面上，然后和點特征一起發(fā)布到同一個topic上，在后端進(jìn)行進(jìn)一步處理， 下圖是線特征跟蹤的效果圖，其中顏色越紅代表更蹤的時間越長。

這三張圖片是在EROUC數(shù)據(jù)集中運行截取的圖片，從第三張圖片中可以看出來在某些低紋理環(huán)境下，線特征仍然保留這較好的更蹤效果。

2.2.2 線特征的構(gòu)建和管理

在點特征傳到后端之后，首先是將點特征放入FeatureManager類中進(jìn)行特征管理和三角化，同樣的，我們實現(xiàn)了一個LineFeatureManager類，實現(xiàn)的函數(shù)和FeatureManager類中基本是一致的，只是采用的方法不同。其中值得說明的是如下兩點：

直線三角化的方法：我們采用的通過匹配線的兩端點以及相機坐標(biāo)系的原點構(gòu)建一個平面，然后觀測到特征線相距最遠(yuǎn)的兩幀下的兩個平面相交所獲得的直線就是兩匹配線段構(gòu)成的空間直線，原理如下圖所示：

假設(shè)兩個端點的歸一化坐標(biāo)分別${\mathbf{s}}^{c_1}={\left[u_s,v_s,1\right]}^{?}$和${\mathbf{e}}^{c_1}={\left[u_e,v_e,1\right]}^{?}$,相機原點世界坐標(biāo)系坐標(biāo)$\mathbf{C}={\left[x_0,y_0,z_0\right]}^{?}$，那么對應(yīng)平面$\pi$的計算公式為$${\pi }_x\left(x?x_0\right)+{\pi }_y\left(y?y_0\right)+{\pi }_z\left(z?z_0\right)=0$$其中$$?\begin{array}{c}{\pi }_x\\ {\pi }_y\\ {\pi }_z\end{array}?={\left[{\mathbf{s}}^{c_1}\right]}_{\times }{\mathbf{e}}^{c_1},{\pi }_w={\pi }_x{x}_0+{\pi }_y{y}_0+{\pi }_z{z}_0$$而由平面${\pi }_1$和平面${\pi }_2$可以得到特征直線在世界坐標(biāo)系下的坐標(biāo)：$${\mathbf{L}}^{?}=\left[\begin{array}{cc}[\mathbf{d}{]}_{\times }& \mathbf{n}\\ ?{\mathbf{n}}^{?}& 0\end{array}\right]={\pi }_1{\mathbf{\pi }}_2^{?}?{\mathbf{\pi }}_2{\mathbf{\pi }}_1^{?}\in {\mathbb{R}}^{4\times 4}$$上述計算公式的代碼在void LineFeatureManager::line_triangulate()函數(shù)當(dāng)中。

直線特征的維護(hù)方式，上面通過直線三角化出來的是普呂克矩陣$${\mathbf{L}}^{?}=\left[\begin{array}{cc}[\mathbf{d}{]}_{\times }& \mathbf{n}\\ ?{\mathbf{n}}^{?}& 0\end{array}\right]$$我們可以非常方便地從中提取線特征的普呂克坐標(biāo)$\mathbf{d}$和$\mathbf{n}$。普呂克坐標(biāo)方便用來進(jìn)行空間變換，但是不方便用來進(jìn)行優(yōu)化后更新，因此我們在程序中采用的維護(hù)方式是在LineFeatureManager類中維護(hù)線特征的正交表示形式，以一個五維向量進(jìn)行存儲，方便后端優(yōu)化更新時可以直接進(jìn)行更新，稍微不方便的是在進(jìn)行空間變換的時候再將其轉(zhuǎn)化為原始的普呂克坐標(biāo)形式才進(jìn)行變換。
普呂克坐標(biāo)的原始表示形式為$\mathcal{L}={\left({\mathbf{n}}^{?},{\mathbf{d}}^{?}\right)}^{?}$，其中$\mathbf{n}$和$\mathbf{d}$是和空間直線相關(guān)的兩個向量，可以通過下圖理解：

普呂克坐標(biāo)$\mathcal{L}={\left({\mathbf{n}}^{?},{\mathbf{d}}^{?}\right)}^{?}$在矩陣${\mathbf{T}}_{cw}=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\mathbf{p}}_{cw}\\ 0& 1\end{array}\right]$作用下的空間變換公式如下，操作起來非常方便：$${\mathcal{L}}^c=\left[\begin{array}{l}{\mathbf{n}}^c\\ {\mathbf{d}}^c\end{array}\right]={\mathcal{T}}_{cw}{\mathcal{L}}_w=\left[\begin{array}{cc}{\mathbf{R}}_{cw}& {\left[{\mathbf{p}}_{cw}\right]}_{\times }{\mathbf{R}}_{cw}\\ 0& {\mathbf{R}}_{cw}\end{array}\right]{\mathcal{L}}^w$$普呂克坐標(biāo)$\mathcal{L}={\left({\mathbf{n}}^{?},{\mathbf{d}}^{?}\right)}^{?}$轉(zhuǎn)化為正交表示形式的公式如下：$$\mathbf{U}=\mathbf{R}(\psi )=\left[\begin{array}{ccc}\mathbf{n}& \frac{\mathbf{d}}{\Vert \mathbf{d}\Vert }& \frac{\mathbf{n}\times \mathbf{d}}{\Vert \mathbf{n}\times \mathbf{d}\Vert }\end{array}\right]$$$$\mathbf{W}=\left[\begin{array}{cc}\cos (?)& ?\sin (?)\\ \sin (?)& \cos (?)\end{array}\right]=\frac{1}{\sqrt{\left(\Vert \mathbf{n}{\Vert }^2+\Vert \mathbf{d}{\Vert }^2\right)}}\left[\begin{array}{cc}\Vert \mathbf{n}\Vert & ?\Vert \mathbf{d}\Vert \\ \Vert \mathbf{d}\Vert & \Vert \mathbf{n}\Vert \end{array}\right]$$其中$\mathbf{U}$是和直線方向有關(guān)旋轉(zhuǎn)矩陣，$\mathbf{W}$是和直線距離有關(guān)的矩陣，在我們的程序中，我們將$\mathbf{U}$表示成四元數(shù)的形式，而$\mathbf{U}$直接通過$?$進(jìn)行存儲，因此我們在LineFeatureManager類中維護(hù)線特征是一個五維的向量。和這一部分變換相關(guān)的代碼在utility類當(dāng)中。

2.2.3 線特征的非線性優(yōu)化

因為我們維護(hù)的線特征在世界坐標(biāo)系下的普呂克坐標(biāo)，因此我們想求得線特征的參差的話只需要將線特征的空間坐標(biāo)投影到某一觀測幀的歸一化平面上（估計值），然后通過這幀上觀測值（線段的兩端點），按照下面方式定義線特征的殘差：$${\mathbf{r}}_l\left({\mathbf{z}}_{{\mathcal{L}}_l^{\prime }}^{c_i},\mathcal{X}\right)=\left[\begin{array}{l}d\left({\mathbf{s}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\\ d\left({\mathbf{e}}_l^{c_i},{\mathbf{l}}_l^{c_i}\right)\end{array}\right]$$其中$$d(\mathbf{s},\mathbf{l})=\frac{{\mathbf{s}}^{?}1}{\sqrt{l_1^2+l_2^2}}$$其中$\mathbf{l}$為空間線特征投影到歸一化平面的直線，${\mathbf{e}}_l^{c_i}$和${\mathbf{e}}_l^{e_i}$分別為觀測值的兩端點。
然后我們定義和線特征相關(guān)的優(yōu)化變量為：世界坐標(biāo)系下的線特征的普呂克坐標(biāo)的正交表示形式${\mathcal{O}}_l$和對應(yīng)觀測幀的空間位姿$x^i$。
雅克比矩陣對優(yōu)化變量的雅克比矩陣如下：$${\mathbf{J}}_l=\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}\left[\frac{?{\mathcal{L}}^{c_i}}{?\delta {\mathbf{x}}^i}\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}\right]$$其中$$\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}={?\begin{array}{ccc}\frac{?l_1{\left({\mathbf{s}}_l^{c_i}\right)}^{?}1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{u_s}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\left({\mathbf{s}}_l^{c_i}\right)}^{?}1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_S}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\\ \frac{?l_1{\left({\mathbf{e}}_l^{c_i}\right)}^{?}1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{u_e}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{?l_2{\left({\mathbf{e}}_l^{c_i}\right)}^{?}1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{3}{2}\right)}}+\frac{v_e}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}& \frac{1}{{\left(l_1^2+l_2^2\right)}^{\left(\frac{1}{2}\right)}}\end{array}?}_{2\times 3}$$$$\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}=[\mathcal{I}0{]}_{3\times 6}$$$$\frac{?{\mathbf{r}}_l}{?{\mathbf{l}}^{c_i}}={\left[\begin{array}{ccccc}{\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}{\mathbf{R}}_{wb}^{?}\left[{\mathbf{d}}^w\right]\times \\ 0_{3\times 3}\end{array}\right]& {\mathcal{T}}_{bc}^{?1}\left[\begin{array}{c}\left[{\mathbf{R}}_{wb}^{?}\left({\mathbf{n}}^w+\left[{\mathbf{d}}^w\right]\times {\mathbf{p}}_{wb}\right)\right]\times ]\\ {\left[{\mathbf{R}}_{wb}^{?}{\mathbf{d}}^w\right]}_{\times }\end{array}\right]& 0& 0& 0\end{array}\right]}_{6\times 15}$$$$\frac{?{\mathcal{L}}^{c_i}}{?{\mathcal{L}}^w}\frac{?{\mathcal{L}}^w}{?\delta \mathcal{O}}={\mathcal{T}}_{wc}^{?1}{\left[\begin{array}{cccc}0& ?w_1{\mathbf{u}}_3& w_1{\mathbf{u}}_2& ?w_2{\mathbf{u}}_1\\ w_2{\mathbf{u}}_3& 0& ?w_2{\mathbf{u}}_1& w_1{\mathbf{u}}_2\end{array}\right]}_{6\times 4}$$其中，因為我們程序中觀測值為歸一化平面上的坐標(biāo)點，因此$\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}$中第一項為單位陣，如果是圖像平面上的點的話第一項為內(nèi)參矩陣。在程序中我們寫了一個LineProjectionFactor類來玩成這項工作。
在完成非線性優(yōu)化之后就需要對優(yōu)化變量進(jìn)行更新，優(yōu)化變量的更新方式在賀博的文章中是按照下面方式進(jìn)行更新的$$\begin{array}{rl}{\mathbf{U}}^{\prime }& \approx \mathbf{U}\left(\mathbf{I}+[\delta \psi {]}_x\right)\\ {\mathbf{W}}^{\prime }& \approx \mathbf{W}\left(\mathbf{I}+\left[\begin{array}{cc}0& ?\delta ?\\ \delta ?& 0\end{array}\right]\right)\end{array}$$但是經(jīng)過我們討論，和論文中不同的是，我們對于$\mathbf{U}$采用的是四元數(shù)的更新方式，而$\mathbf{W}$采用的是$?+\delta ?$的更新方式，這一部分工作在LineLocalParameterization類中。

2.2.4 線特征的邊緣化操作

對于線特征的變換化操作和點特征有所不同的是，因為點特征的深度是建立在起始幀的坐標(biāo)系上的，點特征邊緣化的時候需要將起始幀為最老幀上的點特征的深度從最老幀的坐標(biāo)系轉(zhuǎn)移到次老幀坐標(biāo)系，而在線特征中，因為我們建立的線特征是在世界坐標(biāo)系下，因此不需要進(jìn)行上述操作，相對來說要簡單一些，線特征的相關(guān)操作在LineFeatureManager類中。

3. 實驗結(jié)果

由于我們始終沒能拿到官方要求的弱紋理環(huán)境測試數(shù)據(jù)集，因此我們在EUROC數(shù)據(jù)集上測試了我們的算法，并拿開源版本的VINS-mono在同一臺機器上進(jìn)行了對比，我們使用的評價工具是開源工具evo[6],對比內(nèi)容是RMSE誤差，結(jié)果如下：

RMSEVINS-MonoPL VINS-Mono

MH_01_easy	0.257660	0.252635
MH_02_easy	0.269083	0.254644
MH_04_difficult	0.514302	0.506168
MH_05_difficult	0.453249	0.478479

其中PL VINS-Mono就是我們實現(xiàn)的算法，從表格對比中可以看出來，在大部分?jǐn)?shù)據(jù)集上我們的算法的精度都有所提高，達(dá)到了預(yù)期的效果，下圖分別是我們的算法在MH_01_easy、MH_02_easy、MH_04_difficult、MH_05_difficult在evo工具下畫的軌跡圖。（另外在我們的附件中由我們的算法在MH-05-difficult數(shù)據(jù)集上運行的視頻）

4. 結(jié)論

我們通過一個月的課余時間完成了這個項目，但是因為臨近七月底之后要開始準(zhǔn)備秋招找工作，因此就沒有繼續(xù)再花太多時間在這個上面，我們對這個算法進(jìn)行一個總結(jié)：

因為還沒有在弱紋理環(huán)境的數(shù)據(jù)集上進(jìn)行測試，實際算法對于弱紋理環(huán)境提升了多少現(xiàn)在還并不能下一個定論，但是從我們的實驗中來看，軌跡精度是有一定的提升效果的。但是這里也需要說明的是，精度雖然提升了，因為計算量的提升導(dǎo)致系統(tǒng)運行產(chǎn)生了延遲，我們將線段提取的數(shù)量限制在100以內(nèi)，如果超過太多，系統(tǒng)就有能因為太卡而直接崩掉。

線特征的匹配準(zhǔn)確度并不高，通過調(diào)試發(fā)現(xiàn)，雖然沒幀追蹤的線段有將近100個，但是通過濾波和三角化之后的剔除，最后LineFeatureManager中管理的線特征的數(shù)量通常不到50個，尤其在線特征的三角化的時候會有很大一部分被剔除掉，我們還嘗試過不剔除的情況，那樣系統(tǒng)會在一開始就飛掉。

目前僅僅是將線特征加入到了后端優(yōu)化中，其實如果想完善這個系統(tǒng)的化還要很多工作可以做，比如：（1）將線特征加入到回環(huán)檢測中，在謝曉佳師兄的論文里提到了線特征的詞袋模型，如果能將線特征的回環(huán)殘差加入到后端優(yōu)化中可能能使精度進(jìn)一步提高；（2）系統(tǒng)初始化的時候僅僅使用了點特征，因此在弱紋理的環(huán)境下初始化可能仍然會由困難，可以考慮將線特征加入初始化中；（3）我們還有一個特別想完成的工作是建立線特征的稀疏地圖并將其可視化，因為實在太忙就暫時還沒有實現(xiàn)這個功能。

總而言之，算法雖然取得了一定的效果，但是還是由很大空間可以提升的。

最后想說的是，我們決定將代碼和文檔開源，有任何問題歡迎大家指正交流，希望和更多學(xué)SLAM的朋友一起進(jìn)步，學(xué)SLAM雖然很辛苦，但是也真心很有趣！

參考文獻(xiàn)

[1] Concha, A. , & Civera, J. . (2014). Using superpixels in monocular SLAM. IEEE International Conference on Robotics & Automation. IEEE.
[2] Maity, S., Saha, A., & Bhowmick, B. (2017). Edge slam: Edge points based monocular visual slam. In Proceedings of the IEEE International Conference on Computer Vision (pp. 2408-2417).
[3] Tomono, M. (2009, May). Robust 3D SLAM with a stereo camera based on an edge-point ICP algorithm. In 2009 IEEE International Conference on Robotics and Automation (pp. 4306-4311). IEEE.
[4]He, Y., Zhao, J., Guo, Y., He, W., & Yuan, K. (2018). Pl-vio: Tightly-coupled monocular visual–inertial odometry using point and line features. Sensors, 18(4), 1159.
[5] (2017). 基于點線綜合特征的雙目視覺SLAM方法. (Doctoral dissertation, 浙江大學(xué)).
[6] https://github.com/MichaelGrupp/evo

總結(jié)

以上是生活随笔為你收集整理的VIO在走廊弱纹理环境下的优化——VINS-Mono的点线紧耦合优化的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。