笔记：港大 MARS 实验室 R3LIVE（R2LIVE 升级）鲁棒实时 RGB 雷达视觉惯导紧耦合状态估计

关注港大 MARS 实验室的小伙伴应该都听过 R2LIVE (Robust, Realtime)，它以紧耦合的方式融合相机，雷达，IMU 数据，能够较好地应对设备故障以及环境因素导致的视觉或激光退化现象。这几天 MARS 实验室又有大动作，他们在 GitHub 新建了一个叫 R3LIVE (Robust, Realtime, RGB-colored) 的项目，这个项目在之前的版本的基础上改进了 VIO 算法并给激光点云上了颜色。目前该项目已上传论文，后续会公布代码。

项目地址：github.com/hku-mars/r3live 论文链接：https://arxiv.org/abs/2109.07982

Fig1. (a): R3LIVE 能够实时重建一个密集的、三维 RGB 彩色的环境点云。白色的路径是我们收集数据的行驶轨迹。(b): 用我们提供的离线工具重建的 mesh 图。(c): 用顶点颜色进行纹理处理后的 mesh，这由 VIO 子系统进行渲染完成

下面是本文的视频演示：

框架

这篇文章提出了一种雷达+惯导+视觉融合的 SLAM 框架 R3LIVE。它包含两个子系统，即激光雷达-惯性测距（LIO）和视觉-惯性测距（VIO）。LIO 子系统（FAST-LIO）利用 LiDAR 和惯性传感器的测量结果，建立全局地图的几何结构（即三维点的位置）。VIO 子系统利用视觉-惯性传感器的数据，渲染地图的纹理（即三维点的颜色）。更为具体地，VIO 子系统为了避免特征提取与匹配过程，通过类似直接法最小化光度误差的方式实现 frame-to-map 的数据关联（此为本文的改进点）。R3LIVE 是在 R2LIVE 的基础上开发的，该系统有更高的鲁棒性和准确性。

Fig1. R3LIVE 框架图

关键步骤原理

为方便对比，将 R2LIVE 框架图放在下方。

Fig2. R2LIVE 框架图

相比于 R2LIVE 最大的区别在于 VIO 以及地图。R2LIVE 的视觉地图（VIO 中采用 FAST 角点+KLT 光流数据关联+重投影误差+ESIKF 状态更新+因子图优化）与 LIDAR 地图（面面距残差+ESIKF 状态更新）是两个地图，二者没有直接的数据关联；而 R3LIVE 激光雷达与视觉共同维护一个全局地图（global map），另外 VIO 分为帧间追踪（frame-to-frame）与帧与地图之间追踪（frame-to-map）。

LIO 部分基本复用，此处省去，接下来对 VIO 以及地图颜色渲染过程进行简单讲解。

帧间追踪

对于帧间追踪，假设上一帧追踪到 $m$ 个地图点：

P = {P_{1}, \dots, P_{m}}

它们在上一帧图像平面上的投影点也是知道的，通过 LK 光流可以得到这些投影点在当前帧上的估计位置。随后构建重投影误差，通过 ESIKF 的方式更新状态 $x_{k}$ 。

帧追踪地图

对于帧与地图之间的追踪，利用帧间追踪之后的状态 $\overset{ˇ}{x}_{k}$ 以及追踪到的第 $s$ 个地图点：

P_{s} \in P

构建如下光度误差：

o (\overset{ˇ}{x}_{k},^{G} p_{s}, c_{s}) = c_{s} - γ_{s}

其中的 $c_{s}$ 表示全局地图点中保存的 RGB 颜色信息， $γ_{s}$ 表示当前帧上的观测颜色，同样地采用 ESIKF 的方式最小化上述光度误差以更新状态。

地图点颜色更新

还有一步就是给全局地图渲染颜色（即地图点颜色更新）：同样以某个地图点为例，若它被当前帧观测到，通过优化后的位姿以及内参投影到当前帧的位置可能非整数点（这会造成该点没有对应的观测颜色），此时通过邻域插值得到的该观测颜色，之后通过贝叶斯更新过程将这两个颜色进行“融合”。（最后一步是更新 VIO 追踪的地图点，此时会删除一些光度误差较大的点，并将地图点投影到当前帧新增追踪到的点）