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

Posted on 2021-10-17 Edited on 2022-05-17 In SLAM Waline: Views:

关注港大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 的基础上开发的，该系统有更高的鲁棒性和准确性。

关键步骤原理

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

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

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

帧间追踪

对于帧间追踪，假设上一帧追踪到\(m\)个地图点\(\mathcal{P}=\left\{\mathbf{P}_{1}, \ldots, \mathbf{P}_{m}\right\}\)，它们在上一帧图像平面上的投影点也是知道的，通过LK光流可以得到这些投影点在当前帧上的估计位置。随后构建重投影误差，通过ESIKF的方式更新状态\({\mathbf{x}}_{k}\)。

帧追踪地图

对于帧与地图之间的追踪，利用帧间追踪之后的状态\(\check{\mathbf{x}}_{k}\)以及追踪到的第\(s\)个地图点\(\mathbf{P}_{s} \in \mathcal{P}\)，构建如下光度误差:

\(\mathbf{o}\left(\check{\mathbf{x}}_{k},{ }^{G} \mathbf{p}_{s}, \mathbf{c}_{s}\right)=\mathbf{c}_{s}-\gamma_{s}\)

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

地图点颜色更新

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