VIO在走廊弱纹理环境下的优化

0. 前言

本项目是由我和leeayu共同完成，历时一个半月时间，采用的框架是VINS-mono，开源代码如下：

VINS-Mono-Optimization

运行结果视频

VINS-Mono点线紧耦合优化视频

下面主要从四个方面进行技术报告。

1. 思路概述：该部分主要总结了我们前期进行的文献调研，对于弱纹理下SLAM的算法优化一共有三种方式，我们进行了对比和分析。
2. 算法实施：我们通过上述的的分析对其中两种方案（Edge SLAM和PL SLAM）进行了实施，该部分主要详细介绍了这两种方案的算法细节。
3. 实验结果：我们选取了PL SLAM作为我们的研究重点进行了数据集测试，该部分介绍了实验结果。
4. 结论：我们对算法的实验结果和存在的不足进行了详细分析。

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上，在后端进行进一步处理， 下图是线特征跟踪的效果图，其中颜色越红代表更踪的时间越长。

2.2.2 线特征的构建和管理

在点特征传到后端之后，首先是将点特征放入FeatureManager类中进行特征管理和三角化，同样的，我们实现了一个LineFeatureManager类，实现的函数和FeatureManager类中基本是一致的，只是采用的方法不同。其中值得说明的是如下两点：

1. 直线三角化的方法：我们采用的通过匹配线的两端点以及相机坐标系的原点构建一个平面，然后观测到特征线相距最远的两帧下的两个平面相交所获得的直线就是两匹配线段构成的空间直线，原理如下图所示：

   
   
   
   假设两个端点的归一化坐标分别${\mathbf{s}}^{c_1}={\left[u_s,v_s,1\right]}^{\top }$和${\mathbf{e}}^{c_1}={\left[u_e,v_e,1\right]}^{\top }$,相机原点世界坐标系坐标$\mathbf{C}={\left[x_0,y_0,z_0\right]}^{\top }$，那么对应平面$\pi$的计算公式为$${\pi }_x\left(x-x_0\right)+{\pi }_y\left(y-y_0\right)+{\pi }_z\left(z-z_0\right)=0$$其中$$\left[\begin{array}{c}{\pi }_x\\ {\pi }_y\\ {\pi }_z\end{array}\right]={\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}}^{\ast }=\left[\begin{array}{cc}[\mathbf{d}{]}_{\times }& \mathbf{n}\\ -{\mathbf{n}}^{\top }& 0\end{array}\right]={\pi }_1{\mathbf{\pi }}_2^{\top }-{\mathbf{\pi }}_2{\mathbf{\pi }}_1^{\top }\in {\mathbb{R}}^{4\times 4}$$上述计算公式的代码在void LineFeatureManager::line_triangulate()函数当中。

2. 直线特征的维护方式，上面通过直线三角化出来的是普吕克矩阵$${\mathbf{L}}^{\ast }=\left[\begin{array}{cc}[\mathbf{d}{]}_{\times }& \mathbf{n}\\ -{\mathbf{n}}^{\top }& 0\end{array}\right]$$我们可以非常方便地从中提取线特征的普吕克坐标$\mathbf{d}$和$\mathbf{n}$。普吕克坐标方便用来进行空间变换，但是不方便用来进行优化后更新，因此我们在程序中采用的维护方式是在LineFeatureManager类中维护线特征的正交表示形式，以一个五维向量进行存储，方便后端优化更新时可以直接进行更新，稍微不方便的是在进行空间变换的时候再将其转化为原始的普吕克坐标形式才进行变换。
   普吕克坐标的原始表示形式为$\mathcal{L}={\left({\mathbf{n}}^{\top },{\mathbf{d}}^{\top }\right)}^{\top }$，其中$\mathbf{n}$和$\mathbf{d}$是和空间直线相关的两个向量，可以通过下图理解：

   
   
   普吕克坐标$\mathcal{L}={\left({\mathbf{n}}^{\top },{\mathbf{d}}^{\top }\right)}^{\top }$在矩阵${\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}}^{\top },{\mathbf{d}}^{\top }\right)}^{\top }$转化为正交表示形式的公式如下：$$\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 (\varphi )& -\sin (\varphi )\\ \sin (\varphi )& \cos (\varphi )\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}$直接通过$\varphi$进行存储，因此我们在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}}^{\top }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{\partial {\mathbf{r}}_l}{\partial {\mathbf{l}}^{c_i}}\frac{\partial {\mathbf{l}}^{c_i}}{\partial {\mathcal{L}}^{c_i}}\left[\frac{\partial {\mathcal{L}}^{c_i}}{\partial \delta {\mathbf{x}}^i}\frac{\partial {\mathcal{L}}^{c_i}}{\partial {\mathcal{L}}^w}\frac{\partial {\mathcal{L}}^w}{\partial \delta \mathcal{O}}\right]$$其中$$\frac{\partial {\mathbf{r}}_l}{\partial {\mathbf{l}}^{c_i}}={\left[\begin{array}{ccc}\frac{-l_1{\left({\mathbf{s}}_l^{c_i}\right)}^{\top }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)}^{\top }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)}^{\top }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)}^{\top }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}\right]}_{2\times 3}$$$$\frac{\partial {\mathbf{l}}^{c_i}}{\partial {\mathcal{L}}^{c_i}}=[\mathcal{I}0{]}_{3\times 6}$$$$\frac{\partial {\mathbf{r}}_l}{\partial {\mathbf{l}}^{c_i}}={\left[\begin{array}{ccccc}{\mathcal{T}}_{bc}^{-1}\left[\begin{array}{c}{\mathbf{R}}_{wb}^{\top }\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}^{\top }\left({\mathbf{n}}^w+\left[{\mathbf{d}}^w\right]\times {\mathbf{p}}_{wb}\right)\right]\times ]\\ {\left[{\mathbf{R}}_{wb}^{\top }{\mathbf{d}}^w\right]}_{\times }\end{array}\right]& 0& 0& 0\end{array}\right]}_{6\times 15}$$$$\frac{\partial {\mathcal{L}}^{c_i}}{\partial {\mathcal{L}}^w}\frac{\partial {\mathcal{L}}^w}{\partial \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{\partial {\mathbf{l}}^{c_i}}{\partial {\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 \varphi \\ \delta \varphi & 0\end{array}\right]\right)\end{array}$$但是经过我们讨论，和论文中不同的是，我们对于$\mathbf{U}$采用的是四元数的更新方式，而$\mathbf{W}$采用的是$\varphi +\delta \varphi$的更新方式，这一部分工作在LineLocalParameterization类中。

2.2.4 线特征的边缘化操作

对于线特征的变换化操作和点特征有所不同的是，因为点特征的深度是建立在起始帧的坐标系上的，点特征边缘化的时候需要将起始帧为最老帧上的点特征的深度从最老帧的坐标系转移到次老帧坐标系，而在线特征中，因为我们建立的线特征是在世界坐标系下，因此不需要进行上述操作，相对来说要简单一些，线特征的相关操作在LineFeatureManager类中。

3. 实验结果

由于我们始终没能拿到官方要求的弱纹理环境测试数据集，因此我们在EUROC数据集上测试了我们的算法，并拿开源版本的VINS-mono在同一台机器上进行了对比，我们使用的评价工具是开源工具evo[6],对比内容是RMSE误差，结果如下：

其中PL VINS-Mono就是我们实现的算法，从表格对比中可以看出来，在大部分数据集上我们的算法的精度都有所提高，达到了预期的效果，下图分别是我们的算法在MH_01_easy、MH_02_easy、MH_04_difficult、MH_05_difficult在evo工具下画的轨迹图。（另外在我们的附件中由我们的算法在MH-05-difficult数据集上运行的视频）

4. 结论

我们通过一个月的课余时间完成了这个项目，但是因为临近七月底之后要开始准备秋招找工作，因此就没有继续再花太多时间在这个上面，我们对这个算法进行一个总结：

1. 因为还没有在弱纹理环境的数据集上进行测试，实际算法对于弱纹理环境提升了多少现在还并不能下一个定论，但是从我们的实验中来看，轨迹精度是有一定的提升效果的。但是这里也需要说明的是，精度虽然提升了，因为计算量的提升导致系统运行产生了延迟，我们将线段提取的数量限制在100以内，如果超过太多，系统就有能因为太卡而直接崩掉。
2. 线特征的匹配准确度并不高，通过调试发现，虽然没帧追踪的线段有将近100个，但是通过滤波和三角化之后的剔除，最后LineFeatureManager中管理的线特征的数量通常不到50个，尤其在线特征的三角化的时候会有很大一部分被剔除掉，我们还尝试过不剔除的情况，那样系统会在一开始就飞掉。
3. 目前仅仅是将线特征加入到了后端优化中，其实如果想完善这个系统的化还要很多工作可以做，比如：（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