论文阅读笔记：(2021.09， RA-L) R3LIVE

paper: https://arxiv.org/abs/2109.07982 

code: https://github.com/hku-mars/r3live

一、创新点/主要贡献；

1. 一个能够实时定位，建图和染色的LVIO系统；

2. 设计了一个基于RGB染色后地图的VIO，能进行基于PnP的更新和基于光度(Photometric)的更新；

3. 高精度， 1.5km只有0.16m的平移误差和3.9°的旋转误差；

4. 代码开源，并提供了一些辅助工具；

二、精度/性能

实验1： 激光退化且视觉低纹理

 

实验2： 室内/室外大场景高精度建图

可以看到，即使不加回环等额外处理， 整体的drift也非常小

实验3： 和GPS对比 

港口场景， translation飘半米左右， rotation飘0.25度；

实验4：运行时间

PC: Intel i7-9700K CPU and 32GB RAM

OB: Intel i7-8550u CPU and 8GB RAM

在OB上，要把Pt_res和image_size往低精度调整，才能实时跑；

 

三、应用

3.1 网格重建和纹理贴图

能用离线用CGAL进行网格重建，能离线导出.pcd, .ply, .obj等格式的染色点云；

3.2 基于重建结果， 做一些3D应用，比如车或者无人机的模拟器，3D游戏等；

四、实现

LIO 系统负责进行几何重建；

VIO系统通过最小化PnP重投影误差和光度误差 ，给点云地图“染色”；

4.1 状态变量和地图元素说明

状态变量是29维的， 认为激光雷达和IMU紧固且时间同步（livox的激光麻烦imu和激光雷达放在一起了， 满足这个假设）； 相机到IMU的外参， 相机到imu的同步误差以及相机的内参是在线估计的；

地图按照voxel进行划分， 每个点有时间戳， 空间位置和颜色等信息，空间位置和颜色的协方差也记录了下来；

4.2 LIO子系统

基本沿用fast-lio2的设计， 状态收敛之后的点直接加到地图里面，并未VIO系统提供深度信息；

fast-lio2之前我也写了一个阅读笔记，参考： 论文阅读笔记： fast-lio系列_chaoqinyou的博客-CSDN博客

4.2 VIO子系统

4.2.1 帧到帧的视觉里程计

第一步是通过最小化PnP重投影误差，来初步估计状态，在ESIKF的框架内， loss是每一步迭代的误差状态的函数：

其中：

        每次迭代后的重投影误差（误差状态为0），是光流结果和投影结果的差值：

         投影结果需要对IMU和相机之间的同步误差进行补偿：

计算kalman gain，并更新每一步迭代的状态 

其中kalman gain和观测的数量有关：

4.2.2 帧到地图的视觉里程计

在frame to frame的VIO获得的状态的基础上，通过最小化光度误差， 进一步优化状态。在ESIKF的框架内， loss是每一步迭代的误差状态的函数：

其中：

         每次迭代后的光度误差（误差状态为0），是地图点颜色和投影点颜色的差值，注意地图点颜色是噪声，包含两部分：a. 上次渲染时的噪声；b. 当前帧和上次渲染时的光照变化带来的随机游走噪声，和这两个时刻差值有关：

 

 计算kalman gain 和 状态更新参考帧到帧的VIO;

4.2.3 地图纹理渲染

对于能投影到当前帧的地图点，用线性差值获得它们在当前帧的颜色，并用bayesian update更新到地图里面：

 

4.2.4 更新VIO系统追踪的点

设P{}代表VIO系统追踪的点的几何

1. P{}中去除已经不在图片内的点；

2. P{}中去除重投影误差或者光度误差太大的点；

3. 把地图点投影到图片上，如果它旁边（比如50pixel）没有没有其它tracked point, 则加到P{}； 

五、重要参考文献

R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping