视觉激光融合——VLOAM / LIMO算法解析
Posted Jichao_Peng
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了视觉激光融合——VLOAM / LIMO算法解析相关的知识，希望对你有一定的参考价值。
视觉激光融合——VLOAM / LIMO算法解析
视觉激光融合——VLOAM / LIMO算法解析
在激光SLAM领域，LOAM、Lego-LOAM属于纯激光领域，除此之外还衍生处理视觉激光结合、激光IMU结合，甚至三者结合的算法，本篇博客介绍的VLOAM和LIMO算法就是属于视觉和激光结合的算法，其中VLOAM算法更偏向于是一种激光SLAM算法，而LIMO算法更偏向于是一种视觉SLAM算法，具体地如下：
1. VLOAM算法
VLOAM算法是在2015年ICRA上提出的，但至今在KITTI榜的Odometry数据集上成绩仍然靠前
VLOAM原论文名为《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》，作者并没有对代码进行开源，复现的版本有两个，分别是YukunXia/VLOAM-CMU-16833和Jinqiang/demo_lidar，其中，前者是在去年CMU的几位大佬在A-LOAM代码基础上进行改进的，并通过报告对结果进行了详细的分析，本博客就是在此基础上VLOAM的原理和代码进行简单总结。
对A-LOAM还不了解的同学可以参考下博客学习LOAM笔记——特征点提取与匹配
1.1 总体框架
VLOAM算法的总体框架如下：
VLOAM算法整体分为视觉里程计VO和激光里程计LO两部分，视觉里程计以60HZ的频率运行，视觉里程计通过激光获一部分视觉特征点的深度，其输出位姿变换积分后为帧间变换估计（Sweep to Sweep Refinement）提供先验，而激光里程计以1HZ进行帧间变换估计以及帧到地图的位姿估计（Sweep to Map Registraion），并最终融合视觉里程计的60Hz输出和激光里程计的1Hz输出作为最终的60Hz输出，可看出来，VLOAM主要有如下特点：
- 核心其实以LOAM为基础的激光里程计，视觉里程计仅仅是提供了先验，视觉里程计完全可以由IMU或者其他高频里程计代替；
- 视觉里程计的优势是可以提供高频输出，但是没有尺度且对于光照、纹理等要求比较强，激光里程计的优势是对环境要求较低并可以构建地图，但是输出频率较低，因此从某种成都上说VLOAM算法正式很好地利用了他们的特点进行了互补才达到相对较好的效果；
下面具体分析下各个部分的算法
1.2 视觉里程计
VLOAM视觉里程计中追踪部分没啥特别的，使用光流法或者特征点匹配都可以，较为特别的一点是VLOAM使用激光来恢复视觉特征点的深度（在图像系下寻找最近的三个激光点拟合平面或者求均值，在后面LIMO算法中详细介绍），从而获得了具备深度的特征点
X
i
k
=
[
x
i
k
,
y
i
k
,
z
i
k
]
T
\\mathbf{X}_{i}^{k}=\\left[\\begin{array}{lll}x_{i}^{k}, & y_{i}^{k}, & z_{i}^{k}\\end{array}\\right]^{T}
Xik=[xik,yik,zik]T以及不具备深度的特征点
X
^
i
k
=
[
x
^
i
k
,
y
^
i
k
,
z
^
i
k
]
T
\\hat{\\mathbf{X}}_{i}^{k}=\\left[\\begin{array}{lll}\\hat{x}_{i}^{k}, & \\hat{y}_{i}^{k}, & \\hat{z}_{i}^{k}\\end{array}\\right]^{T}
X^ik=[x^ik,y^ik,z^ik]T两种类型，前后帧匹配特征点均满足变换方程
X
i
k
+
1
=
R
X
i
k
+
T
\\mathbf{X}_{i}^{k+1}=\\mathbf{R} \\mathbf{X}_{i}^{k}+\\mathbf{T}
Xik+1=RXik+T 以上是关于视觉激光融合——VLOAM / LIMO算法解析的主要内容，如果未能解决你的问题，请参考以下文章 激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析 激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析 激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析 自动驾驶感知算法实战15——纯视觉感知和传感器融合方案对比，特斯拉九头蛇的进化
对于具备深度的点：
对于前一帧有深度的点，下一帧未必有深度，因此我们可以总是假设下一帧该点无法通过激光获得该点深度，有
X
i
k
+
1
=
d
i
k
+
1
X
^
i
k
+
1
\\mathbf{X}_{i}^{k+1}=d_{i}^{k+1} \\hat{\\mathbf{X}}_{i}^{k+1}
Xik+1=dik+1X^ik+1，其中
d
i
k
+
1
d_{i}^{k+1}
dik+1是下一帧该点假设深度，通过联立该方程和变换方程， 并消除
d
i
k
+
1
d_{i}^{k+1}
dik+1就可以获得如下方程组
(
z
^
i
k
+
1
R
1
−
x
^
i
k
+
1
R
3
)
X
i
k
+
z
^
i
k
+
1
T
1
−
x
^
i
k
+
1
T
3
=
0
\\left(\\hat{z}_{i}^{k+1} \\mathbf{R}_{1}-\\hat{x}_{i}^{k+1} \\mathbf{R}_{3}\\right) \\mathbf{X}_{i}^{k}+\\hat{z}_{i}^{k+1} T_{1}-\\hat{x}_{i}^{k+1} T_{3}=0
(z^ik+1R1−x^ik+1R3)Xik+z^ik+1T1−x^ik+1T3=0
(
z
^
i
k
+
1
R
2
−
y
^
i
k
+
1
R
3
)
X
i
k
+
z
^
i
k
+
1
T
2
−
y
^
i
k
+
1
T
3
=
0
\\left(\\hat{z}_{i}^{k+1} \\mathbf{R}_{2}-\\hat{y}_{i}^{k+1} \\mathbf{R}_{3}\\right) \\mathbf{X}_{i}^{k}+\\hat{z}_{i}^{k+1} T_{2}-\\hat{y}_{i}^{k+1} T_{3}=0
(z^ik+1R2−y^ik+1R3)Xik+z^ik+1T2−y^ik+1T3=0其中
R
1
R
2
,
R
3
\\mathbf{R}_{1}\\,\\mathbf{R}_{2},\\mathbf{R}_{3}
R1R2,R3分别为旋转矩阵的第一、第二和第三行，
T
1
,
T
2
,
T
3
T_{1},T_{2},T_{3}
T1,T2,T3分别为平移向量的第一、第二和第三行。
对于不具备深度的点
我们有
d
i
k
X
^
i
k
d_{i}^{k} \\hat{\\mathbf{X}}_{i}^{k}