经典文献阅读之--R3LIVE

0. 简介

之前作者在今年年初写了激光与摄像头混合的博客《从LVI-SAM来看激光与视觉的紧耦合系统》，当时一直想写R3LIVE的。但是一直苦于没有时间好好地去深入钻研探索。这段时间正好有比较充裕的时间做这个事情，所以这里我们对《R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package》这篇文章进行学习与解析。这篇文章中中有很多值得我们借鉴和学习的地方。与LVI-SAM一样，它也是一个激光-惯性-视觉融合的传感器融合框架，利用激光雷达、惯性和视觉传感器的测量来实现鲁棒性和准确性的状态估计。由于R3LIVE是由R2LIVE进一步拓展来的。这里我们放上其他博主翻译的R2LIVE的论文。

1. R2LIVE回顾

如上面所说，我们要了解学习R3LIVE之前，我们需要先学习R2LIVE。

我们可以看到R2LIVE的激光雷达测量部分，主要继承了Fast-LIO的面元特征思想，$u$为面法向量，$q/p$为代表面的特征点，残差如下:
$$r_l({\check{X}}_{k+1}{,}^L{p}_j)=u_j^T{(}^G{p}_j-q_j)$$

IMU部分主要涉及到离散IMU状态向量使用。这部分其实也是Fast-LIO里面的相关知识。然后及可以根据ESKF完成与雷达数据的融合。

$$\begin{gathered} x_i={\left[{}^G{R}_{I_i}^T \\ { \\ }^G{P}_{I_i}^T{ \\ }^I{R}_{C_i}^T{ \\ }^I{P}_{C_i}^T{ \\ }^G{v}_i^T \\ {b}_{g_i}^T \\ {b}_{a_i}^T\right]}^T \end{gathered}$$

$$\begin{gathered} \delta {\hat{X}}_i=X_i\boxminus {\hat{X}}_i \\ ={\left[{}^G{\hat{R}}_{I_i}^T \\ { \\ }^G{\hat{P}}_{I_i}^T \\ { \\ }^I{\hat{R}}_{C_i}^T \\ { \\ }^I{\hat{P}}_{C_i}^T \\ { \\ }^G{\hat{v}}_i^T \\ {\hat{b}}_{g_i}^T \\ {\hat{b}}_{a_i}^T\right]}^T \\ =(X_i⊞(\Delta t\ast f(X_i,u_i,w_i)))\boxminus ({\hat{x}}_i⊞(\Delta t\ast f({\hat{x}}_i,u_i,0))) \end{gathered}$$

视觉测量信息理论上与LiDAR数据相同，如果第k+1帧是相机帧，那么误差就为重投影误差:
$${(}^C{P}_s={(}^G{\check{R}}_{I_{k+1}}{}^I{\check{R}}_{C_{k+1}}{)}^T{}^G{P}_s-{(}^I{\check{R}}_{C_{k+1}}{)}^T{}^G{\check{p}}_{I\_k+1}{-}^I{\check{p}}_{C_{k+1}}$$

因子图优化，总体来说和VINS-MONO一样，不过就是多了一个雷达位姿节点，用IMU预积分将雷达位姿节点和相机位姿节点进行关联。

下面是R2LIVE的代码结构。

2. R3LIVE学习

与R2LIVE类似，R3LIVE系统也是以IMU作为核心，利用激光雷达和相机的观测修正系统状态。算法相当于不断以imu的预测为基础，每当得到了新的相机或激光的观测，就对系统状态进行更新，实现了多传感器信息融合。其中激光雷达的观测与LIO相同，相机的观测则分为两步：先是用跟踪点的重投影误差，再基于更新结果用跟踪点的光度误差再次进行状态更新。

这里我们找到了stella博主的一篇文章，非常清晰的阐述了R3LIVE的整个流程。

…详情请参照古月居