R3LIVE编译运行和代码分析_r3live跑自己的数据

说在前面

最近踩了一下R3LIVE的坑，花了很长时间，编译、运行、跑自己数据集、改代码加功能。

先说一下自己的环境吧，我是AMD5800H+NVIDIA 3060+Ubuntu 18.04+ROS Melodic，但其实这个框架不吃显卡，CPU和内存越高越好（大部分SLAM框架都是如此），内存最好要有16G（包括swap内存），否则应该会内存爆掉，因为全局地图会默认开辟大概12G的内存，非要用小内存电脑运行的话，可以在源代码中修改地图的大小。

先看看结果。

编译运行

这个框架编译最困难的地方应该就是opencv的版本要求比较高和高版本的opencv与cv_bridge的配合。其余的ROS还有各种依赖报错都挺好解决的，缺啥装啥，具体参考r3live的官方github。我只讲opencv相关的。

opencv4编译安装和使用

官方已经验证过能用的opencv版本是3.3.1, 3.4.16, 4.2.1 and 4.5.3，我使用的是4.5.1。首先就是下载对应版本的opencv和opencv_contrib，当然是用git命令啦，git clone https://github.com/opencv/opencv.git，git clone https://github.com/opencv/opencv_contrib.git，注意要进入克隆的文件夹中使用git checkout x.x.x来选择相应的版本，比如我是切换成4.5.1就用git checkout 4.5.1。然后在opencv的文件夹下建一个build文件夹并进入，一般的第三方库这时候cmake ..就行了，但是opencv不行，要配置一堆编译参数，使用以下命令。

cmake -DCMAKE_BUILD_TYPE=RELEASE \
-D CMAKE_INSTALL_PREFIX=/usr/local/opencv-4.5.1 \
-D OPENCV_EXTRA_MODULES_PATH=../opencv_contrib/modules \
-D WITH_CUDA=1 \
-D WITH_CUDNN=1 \
-D WITH_V4L=1 \
-D OPENCV_DNN_CUDA=1 \
-D CUDNN_VERSION='8.1.0' \
-D CUDNN_INCLUDE_DIR='/usr/local/cuda-11.1/targets/x86_64-linux/include' \
-D ENABLE_FAST_MATH=1 \
-D CUDA_FAST_MATH=1 \
-D WITH_CUBLAS=1 \
-D CUDA_nppi_LIBRARY=true \
-D OPENCV_GENERATE_PKGCONFIG=1 \
-D CUDA_GENERATION=Pascal  ..
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

其中的opencv_contrib路径根据自己的安装位置进行选择，以上是使用GPU加速的opencv（这个框架的计算资源的消耗本身就比较小，不加速也无所谓），需要提前配置好显卡驱动和CUDA，当然也可以编译不加速的opencv，只需要将WITH_CUDA、WITH_CUDNN和OPENCV_DNN_CUDA改为零，以及相应路径删掉即可，如下。

cmake -DCMAKE_BUILD_TYPE=RELEASE \
-D CMAKE_INSTALL_PREFIX=/usr/local/opencv-4.5.1 \
-D OPENCV_EXTRA_MODULES_PATH=../opencv_contrib/modules \
-D WITH_CUDA=0 \
-D WITH_CUDNN=0 \
-D WITH_V4L=1 \
-D OPENCV_DNN_CUDA=0 \
-D ENABLE_FAST_MATH=1 \
-D CUDA_FAST_MATH=1 \
-D WITH_CUBLAS=1 \
-D CUDA_nppi_LIBRARY=true \
-D OPENCV_GENERATE_PKGCONFIG=1 \
-D CUDA_GENERATION=Pascal  ..
1
2
3
4
5
6
7
8
9
10
11
12
13

如果遇到Configuring incomplete, errors occurred!，那非常遗憾，先检查编译的路径是不是都正确，并自己查看报错的原因尝试自己解决（这不正是玩linux的意义所在嘛），如果还是不行，可以去别人的博客找找解决方法（以上两条指令对我都是能正常使用的）。如果是Configuring done，那恭喜你，可以进行下一步了。

cmake完成后就需要make了，使用make -j 14多线程更快速编译完成，这一步要比较长时间。编译完成之后，不要sudo make install，因为可能你电脑上已有的程序一般会习惯用3版本的opencv，还有一个就是ros中的cv_bridge默认会使用3版本的opencv，如果将opencv4安装到系统中，再使用cv_bridge时会报错。其实这也是维护环境的一个技巧，你的电脑中不可能只有一个框架，多个框架可能依赖不同版本的第三方库，比如opencv，pcl，ceres，protbuf等等，都可以给每个框架编译一个专用的，而不是都从系统路径中寻找。

所以我们修改R3LIVE的CmakeList.txt来锁定opencv的路径，导入我们新编译的版本而不是系统中存在的版本。在find_package(opencv REQUIRED)之前加入set(OpenCV_DIR "/home/yhd/opencv-4.5.1-withGPU/build")即可，其中的路径需要改成读者自己编译的build路径。

cv_bridge和高版本opencv的适配

opencv搞定之后就只有cv_bridge和高版本opencv之间的适配了。为什么要做适配呢，是因为R3LIVE用到了cv_bridge，而cv_bridge又依赖opencv，在ros中默认是opencv3，这个从/opt/ros/melodic/share/cv_bridge/cmake/cv_bridge-extras.cmake可以看出，那么我们要修改cv_bridge的链接。如果不做适配的话，编译会有几个不容易注意的warning（如下图），虽然编译不会报错，但是一运行就会崩溃。

修改cv_bridge的链接方向有两个方法。

第一可以修改/opt/ros/melodic/share/cv_bridge/cmake中的两个文件，具体可参考这篇博客（其中的版本号和路径要根据实际情况修改），但这是ros源代码，不建议修改，可能会带来不必要的问题。

第二种方法就是手动编译cv_bridge，这种方法比较安全，建议使用。从https://github.com/fizyr-forks/vision_opencv下载cv_bridge源码，注意要先选择分支为opencv4，在download压缩包，而不是git clone然后git checkout，（讲道理两者应该都可以，但是亲测只有下载压缩包的方式可以）。然后将vision_opencv-opencv4作为一个ROS功能包去编译，将cv_bridge包中的CMakeList.txt手动指定为opencv4的版本，同样是通过上面说过的set(OpenCV_DIR "/home/yhd/opencv-4.5.1-withGPU/build")的方式。编译成功之后可以看一下devel/share/cv_bridge/cmake，该路径下的文件和/opt/ros/melodic/share/cv_bridge/cmake中的三个文件名相同。最后在R3LIVE的CmakeLists.txt中指定cv_bridge的位置，和opencv的方法类似，将set(cv_bridge_DIR "/home/yhd/cv_bridge_ws/devel/share/cv_bridge/cmake")添加到find_package(cv_bridge)之前即可。

最后R3LIVE编译成功，并且运行时不崩溃就算是成功了，之后就可以好好感受R3LIVE的魅力了。至于我自己的项目和R3LIVE的改动的介绍，最近是没时间讲了。有时间可以讲讲R3LIVE每部分代码的作用。

代码分析

R3LIVE中有两个激光惯性里程计LIO子系统和视觉惯性里程计VIO子系统，R3LIVE之外还有一个激光前端，本文也分来来讲这三部分。除此之外，R3LIVE还有众多数据结构需要理解，包括Image_frame、Camera_Lidar_queue、MeasureGroup、StatesGroup、RGB_pts、RGB_Voxel、Rgbmap_tracker、Global_map，以及R3LIVE用到的一些开发工具，比如日志系统、线程池、时间系统，有时间的话我都会讲讲。

LiDAR_front_end

这一部分主要做雷达型号适配和特征提取的工作，作者提供了LIVOX-MID、LIVOX-HORIZON、VELODYNE16、OUSTER64这四种接口，其他型号的雷达也可以模仿这几个接口来写，直接套用特征提取的函数give_feature()，唯一需要注意的是把一帧点云中的每个点距离第一个点的时间间隔计算出来并以毫秒为单位存在曲率的位置pl_full[i].curvature，这在IMU去除运动畸变是也需要用到。而特征提取实际上也没有进行，不同于fast-lio2使用特征点云，R3LIVE使用的是原始点云，可以在配置文件中的using_raw_point来控制。

LIO

VIO在R3LIVE构造时就启动了。TODO

VIO

与LIO在R3LIVE构造时就启动不同，VIO是在接收到图像信息才开始工作的。VIO预留了原始图像R3LIVE::image_callback()和压缩图像R3LIVE::image_comp_callback()的接口，为了节省资源，R3LIVE采用的策略是，如果有压缩图像可用，就不再接收原始图像的消息。以压缩图像为例，接收到图像后，会在将图像加入到缓存中，并在线程池中申请一个线程来处理图像缓存R3LIVE::service_process_img_buffer，主要是做了解压缩和图像去畸变处理，并在线程池中申请两个线程来做全局地图的输出R3LIVE::service_pub_rgb_maps和VIO主函数R3LIVE::service_VIO_update。

• 主函数中先做了一些准备工作，包括相机内参传入op_track.set_intrinsic()、显示控制面板、等待激光处理避免堆积、图像缓存异常的处理（图像过少和过多）。
• 从缓存中取出图像后，如果是第一帧，就从全局地图中挑选一些能被图像帧观测到的地图点m_map_rgb_pts.selection_points_for_projection()进行跟踪op_track.init()。如果不是第一帧，就需要进行状态估计。
• 先通过IMU预积分算出一个大致的ESIKF状态作为图像帧的初始位姿，再进行光流跟踪并更新图像帧位姿。
• PnP去除跟踪不好的点，并用最终的跟踪结果来更新图像帧位姿op_track.remove_outlier_using_ransac_pnp()。
• 分别计算重投影误差和光度（RGB）误差来更新ESIKF的状态，更新之后的状态作为当前系统状态和当前图像帧的最终位姿。
• 多线程渲染地图点的颜色render_pts_in_voxels_mp()。
• 更新全局地图中图像帧的位姿（为更新投影点做准备）m_map_rgb_pts.update_pose_for_projection()，更新光流跟踪的特征点op_track.update_and_append_track_pts()。
• 发布一些ROS消息，方便可视化和debug。

数据结构

R3LIVE框架中包含了许多自己设计的数据结构，使用起来非常方便，包括Image_frame、Camera_Lidar_queue、MeasureGroup、StatesGroup、RGB_pts、RGB_Voxel、Rgbmap_tracker、Global_map，下面一一讲解。

Image_frame

Image_frame中比较重要的变量有相机内参m_cam_K、世界到相机位姿m_pose_w2c_q, m_pose_w2c_t、相机到世界的位姿（为了方便投影计算）m_pose_c2w_q, m_pose_c2w_t、帧号m_frame_idx、去畸变图像m_img、原始图像m_raw_img、灰度图像m_img_gray。

比较重要的操作有设置当前帧的位姿set_pose()，获取3D点投影到图像帧中的像素位置project_3d_to_2d()（作者舍去了在相机坐标系中Z值很小的点以及Z值为负的点），判断图像中某个二维坐标的像素是否可观if_2d_points_available（作者认为太靠近图像边缘的点也是不可观的），获取图像中某个二维坐标的颜色get_rgb()（还重载了一个顺带能计算横纵向RGB梯度的版本（VIO中计算光度误差用）），图像直方图均衡化image_equalize()（在图像加入到缓存之前就做均衡化），直接给3D点投影上色project_3d_point_in_this_img()（前面几个函数的组合）。

Camera_Lidar_queue

Camera_Lidar_queue主要是用来记录激光雷达、相机、IMU三种传感器数据加入时间，根据时间先后来判断运行LIO还是VIO。

MeasureGroup

Camera_Lidar_queue包含了一帧点云、点云的开始和结束时间、一个能够覆盖住这两个时间的IMU队列。这样的数据结构方便后面做点云运动畸变处理。

StatesGroup

StatesGroup存储了系统的状态，论文中提到的29维的状态量在这一一写出，分别是世界到IMU的旋转（3维）和平移（3维）、IMU的速度（3维）、陀螺仪的偏置（3维）、加速度计的偏置（3维）、重力向量（3维）、IMU到相机的旋转（3维）和平移（3维）、IMU到相机的时间偏置（1维）、相机的内参（4维）。

除了状态变量的定义以外，还有一些状态之间的运算，为了方便编程，重载了+、-、+=，这部分的运算在ESIKF中被调用。

RGB_pts

RGB_pts是一个信息丰富的三维点结构体，包括点的位置、RGB颜色、颜色的方差，并且提供了更新RGB颜色的接口（渲染时要用，同时还要跟新RGB颜色的方差）。

RGB_Voxel

RGB_Voxel包含了一个存储三维点指针的容器和一个最近访问时间（用来判断体素是否处于激活状态）。

Rgbmap_tracker

Rgbmap_tracker是VIO中用来进行光流跟踪的类，底层使用的是金字塔LK光流法。比较重要的变量有上一帧和当前帧的时间m_last_frame_time, m_current_frame_time、内参矩阵m_intrinsic、畸变参数矩阵m_dist_coeffs、上一阵和当前帧跟踪到的像素点m_last_tracked_pts, m_current_tracked_pts、保存跟踪点颜色的容器m_colors、上一帧的三维点容器m_rgb_pts_ptr_vec_in_last_frame、上一帧和当前帧的三维点和像素坐标对应的哈希表m_map_rgb_pts_in_last_frame_pos, m_map_rgb_pts_in_current_frame_pos（该类里面的所有void *都指的是RGB_pts *），LK光流法内核m_lk_optical_flow_kernel。

初始化时init()，将传入的三维点数组和像素点数组全部作为上一帧相关数据保存，然后做一次光流跟踪。当有新图像进来时track_img()，如果上一次的个跟踪点较少，则认为跟踪不可靠；否则做一次光流跟踪，并计算F矩阵，用随机一致采样法提出一些外点，还有稀疏化处理reduce_vector()。该类还给出了一个用随机一致采样PnP来提出外点的接口remove_outlier_using_ransac_pnp()，这个函数本质是调用了opencv中的solvePnPRansac()，这里经常会因为点的个数不够而抛出异常，这里的少量异常不会影响程序运行。

Global_map

Global_map是一个全局地图的结构体，能够实时根据Image_frame的位姿来更新适合投影的三维点。其中有一个标志位是否用体素中的所有点来建图m_if_get_all_pts_in_boxes_using_mp，各种各样的锁，用于更新投影点的图像帧，从三维坐标指向RGB_ptr的三层哈希表m_hashmap_3d_pts，从三维坐标指向体素的三层哈希表m_hashmap_voxels，最近访问的体素m_voxels_recent_visited，上一次用到的三维点m_voxels_recent_visited。

该类构造函数中有一个根据硬件内存大小自适应调整全局地图大小的功能，然后就是申请了一个线程根据图像帧的位姿选择投影点，通过帧号来避免反复挑选，仅在图像帧更新之后再重新选择投影点。最重要的就是往全局地图中增加三维点append_points_to_global_map()，这个函数在lio中被调用，根据要加入的点云创建体素，这里面巧妙地运用了三层哈希表，最终获得了一个处于激活状态的体素集合，在渲染时被使用（其中值得注意的是，如果m_recent_visited_voxel_activated_time不为零的话，也就是体素会保持一段时间的激活状态，这时除了即将加入的点云对应的体素被激活，还有很多近期加入的体素仍处于激活状态，这会扩大建图范围，使全局地图更加稠密，但是会牺牲一部分实时性）。

================ 未完待续 ================