VIO在走廊弱紋理環境下的優化——VINS-Mono的點線緊耦合優化

• VIO在走廊弱紋理環境下的優化
• • 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 線特征的構建和管理
      • 2.2.3 線特征的非線性優化
      • 2.2.4 線特征的邊緣化操作
  • 3. 實驗結果
  • 4. 結論
  • 參考文獻

VIO在走廊弱紋理環境下的優化

0. 前言

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

VINS-Mono-Optimization

運行結果視頻

VINS-Mono點線緊耦合優化視頻

下面主要從四個方面進行技術報告。

思路概述：該部分主要總結了我們前期進行的文獻調研，對于弱紋理下SLAM的算法優化一共有三種方式，我們進行了對比和分析。

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

實驗結果：我們選取了PL SLAM作為我們的研究重點進行了數據集測試，該部分介紹了實驗結果。

結論：我們對算法的實驗結果和存在的不足進行了詳細分析。

1. 思路概述

對于課題要求我們先是進行了大量的文獻調研，調研結果一共有如下三種方案。

1.1 Super Pixel SLAM

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

1.2 Edge SLAM

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

1.3 PL SLAM

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

2. 算法實施

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

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

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

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.

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

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.

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

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

2.2 PL SLAM方案的實施

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

2.2.1 線特征的提取和跟蹤

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

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

2.2.2 線特征的構建和管理

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

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

假設兩個端點的歸一化坐標分別${\mathbf{s}}^{c_1}={\left[u_s,v_s,1\right]}^{?}$和${\mathbf{e}}^{c_1}={\left[u_e,v_e,1\right]}^{?}$,相機原點世界坐標系坐標$\mathbf{C}={\left[x_0,y_0,z_0\right]}^{?}$，那么對應平面$\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$可以得到特征直線在世界坐標系下的坐標：$${\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()函數當中。

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

普呂克坐標$\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$$普呂克坐標$\mathcal{L}={\left({\mathbf{n}}^{?},{\mathbf{d}}^{?}\right)}^{?}$轉化為正交表示形式的公式如下：$$\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}$是和直線方向有關旋轉矩陣，$\mathbf{W}$是和直線距離有關的矩陣，在我們的程序中，我們將$\mathbf{U}$表示成四元數的形式，而$\mathbf{U}$直接通過$?$進行存儲，因此我們在LineFeatureManager類中維護線特征是一個五維的向量。和這一部分變換相關的代碼在utility類當中。

2.2.3 線特征的非線性優化

因為我們維護的線特征在世界坐標系下的普呂克坐標，因此我們想求得線特征的參差的話只需要將線特征的空間坐標投影到某一觀測幀的歸一化平面上（估計值），然后通過這幀上觀測值（線段的兩端點），按照下面方式定義線特征的殘差：$${\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}$分別為觀測值的兩端點。
然后我們定義和線特征相關的優化變量為：世界坐標系下的線特征的普呂克坐標的正交表示形式${\mathcal{O}}_l$和對應觀測幀的空間位姿$x^i$。
雅克比矩陣對優化變量的雅克比矩陣如下：$${\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}$$其中，因為我們程序中觀測值為歸一化平面上的坐標點，因此$\frac{?{\mathbf{l}}^{c_i}}{?{\mathcal{L}}^{c_i}}$中第一項為單位陣，如果是圖像平面上的點的話第一項為內參矩陣。在程序中我們寫了一個LineProjectionFactor類來玩成這項工作。
在完成非線性優化之后就需要對優化變量進行更新，優化變量的更新方式在賀博的文章中是按照下面方式進行更新的$$\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}$$但是經過我們討論，和論文中不同的是，我們對于$\mathbf{U}$采用的是四元數的更新方式，而$\mathbf{W}$采用的是$?+\delta ?$的更新方式，這一部分工作在LineLocalParameterization類中。

2.2.4 線特征的邊緣化操作

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

3. 實驗結果

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

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就是我們實現的算法，從表格對比中可以看出來，在大部分數據集上我們的算法的精度都有所提高，達到了預期的效果，下圖分別是我們的算法在MH_01_easy、MH_02_easy、MH_04_difficult、MH_05_difficult在evo工具下畫的軌跡圖。（另外在我們的附件中由我們的算法在MH-05-difficult數據集上運行的視頻）

4. 結論

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

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

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

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

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

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

參考文獻

[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, 浙江大學).
[6] https://github.com/MichaelGrupp/evo

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。