R3LIVE源码解析(6) — R3LIVE流程详解

1 R3LIVE框架简介

R3LIVE是香港大学Mars实验室提出的一种融合imu、相机、激光的SLAM方法，R3LIVE由两个子系统组成，一个激光惯性里程计（LIO）和一个视觉惯性里程计（VIO）。LIO子系统（FAST-LIO）利用来自激光雷达和惯性传感器的测量数据，并且构建地图的几何结构（即3D点位置）。VIO子系统使用视觉惯性传感器的数据，并且渲染地图的纹理（即3D点颜色）。更具体而言，VIO子系统通过最小化帧到地图的光度误差直接且有效地融合视觉数据。本文所提出的系统R3LIVE是基于我们之前的工作R2LIVE开发的，它的VIO架构设计与R2LIVE完全不同。

2 R3LIVE的launch文件

在R3LIVE中我们可以看到有这三个launch文件，r3live_bag.launch是LIVOX激光雷达的launch文件，r3live_bag_ouster.launch是64线激光雷达的launch文件，r3live_reconstruct_mesh.launch是纹理贴图的launch文件。在运行自己录制的数据集和在线运行时，对相应的话题名进行更改。

首先第一个部分就是定义了一些R3LIVE处理的雷达话题以及IMU视觉以及config配置文件的导入。
然后第二个部分就是设置一些对应特定雷达的配置参数
第三步就是启动r3live_LiDAR_front_end节点和r3live_mapping节点，这也是我们下面需要结合论文仔细阅读的点
最后就是启动rviz

<launch>
    <!-- Subscribed topics -->
    <param name="LiDAR_pointcloud_topic" type="string" value= "/laser_cloud_flat" />    <!-- 订阅的lidar topic -->
    <param name="IMU_topic" type="string" value= "/livox/imu" />                        <!-- 订阅的IMU topic -->
    <param name="Image_topic" type="string" value= "/camera/image_color" />             <!-- 订阅的image topic -->
    <param name="r3live_common/map_output_dir" type="string" value="$(env HOME)/r3live_output" /><!-- 输出的map文件夹 -->
    <rosparam command="load" file="$(find r3live)/../config/r3live_config.yaml" />      <!-- 加载的yaml参数文件 -->

    <!-- 前端处理节点 -->
    <node pkg="r3live" type="r3live_LiDAR_front_end" name="r3live_LiDAR_front_end"  output="screen" required="true"/>
    <!-- 建图节点 -->
    <node pkg="r3live" type="r3live_mapping" name="r3live_mapping" output="screen" required="true" />
    
    <!-- rviz可视化 - 默认打开 -->
    <arg name="rviz" default="1" />
    <group if="$(arg rviz)">
        <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" 
            args="-d $(find r3live)/../config/rviz/r3live_rviz_config.rviz" />
    </group>
</launch>

3 R3LIVE的r3live_config文件

配置文件对各个模块的参数进行设置，值得注意的是：

lidar_type根据不同的雷达类型进行更改
在使用自己的数据集时，需要修改image_width、image_height、camera_intrinsic、camera_dist_coeffs、camera_ext_R、camera_ext_t，对应相机的尺寸、内外参。

注意：在R3LIVE中使用自己设置外参时，要对此处的得到的外参结果求逆，不然得不到最后的点云着色效果。具体操作是将R按对角线反转，T取负号。

Lidar_front_end:
   lidar_type: 1   # 1 for Livox-avia, 3 for Ouster-OS1-64
   N_SCANS: 6
   using_raw_point: 1
   point_step: 1
   
r3live_common:
   if_dump_log: 0                   # If recording ESIKF update log. [default = 0]
   record_offline_map: 1            # If recording offline map. [default = 1]
   pub_pt_minimum_views: 3          # Publish points which have been render up to "pub_pt_minimum_views" time. [default = 3]
   minimum_pts_size: 0.01           # The minimum distance for every two points in Global map (unit in meter). [default = 0.01] 
   image_downsample_ratio: 1        # The downsample ratio of the input image. [default = 1]
   estimate_i2c_extrinsic: 1        # If enable estimate the extrinsic between camera and IMU. [default = 1] 
   estimate_intrinsic: 1            # If enable estimate the online intrinsic calibration of the camera lens. [default = 1] 
   maximum_vio_tracked_pts: 600     # The maximum points for tracking. [default = 600]
   append_global_map_point_step: 4  # The point step of append point to global map. [default = 4]

r3live_vio:
   image_width: 1280
   image_height: 1024
   camera_intrinsic:
      [863.4241, 0.0, 640.6808,
      0.0,  863.4171, 518.3392,
      0.0, 0.0, 1.0 ] 
   camera_dist_coeffs: [-0.1080, 0.1050, -1.2872e-04, 5.7923e-05, -0.0222]  #k1, k2, p1, p2, k3
   # Fine extrinsic value. form camera-LiDAR calibration.
   camera_ext_R:
         [-0.00113207, -0.0158688, 0.999873,
            -0.9999999,  -0.000486594, -0.00113994,
            0.000504622,  -0.999874,  -0.0158682]
   # camera_ext_t: [0.050166, 0.0474116, -0.0312415] 
   camera_ext_t: [0,0,0] 
   # Rough extrinsic value, form CAD model, is not correct enough, but can be online calibrated in our datasets.
   # camera_ext_R:
   #    [0, 0, 1,
   #     -1, 0, 0,
   #     0, -1, 0]
   # camera_ext_t: [0,0,0] 
   
r3live_lio:        
   lio_update_point_step: 4   # Point step used for LIO update.  
   max_iteration: 2           # Maximum times of LIO esikf.
   lidar_time_delay: 0        # The time-offset between LiDAR and IMU, provided by user. 
   filter_size_corner: 0.30   
   filter_size_surf: 0.30
   filter_size_surf_z: 0.30
   filter_size_map: 0.30

4 R3LIVE从哪开始阅读

在CmakeList中我们注意到两个主要的node节点所依赖的cpp文件。我们可以知道r3live_LiDAR_front_end 节点会先运行

# 前端里程计节点
add_executable(r3live_LiDAR_front_end src/loam/LiDAR_front_end.cpp)
target_link_libraries(r3live_LiDAR_front_end ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_executable(test_timer src/tools/test_timer.cpp)

# 后端建图节点
add_executable(r3live_mapping src/r3live.cpp 
                src/r3live_lio.cpp
                src/loam/include/kd_tree/ikd_Tree.cpp
                src/loam/include/FOV_Checker/FOV_Checker.cpp 
                src/loam/IMU_Processing.cpp
                src/rgb_map/offline_map_recorder.cpp
                # From VIO
                src/r3live_vio.cpp
                src/optical_flow/lkpyramid.cpp
                src/rgb_map/rgbmap_tracker.cpp
                src/rgb_map/image_frame.cpp
                src/rgb_map/pointcloud_rgbd.cpp
              )
target_link_libraries(r3live_mapping 
                          ${catkin_LIBRARIES}
                          ${Boost_LIBRARIES}
                          ${Boost_FILESYSTEM_LIBRARY}
                          ${Boost_SERIALIZATION_LIBRARY} # serialization
                          ${OpenCV_LIBRARIES}
                          # ${OpenMVS_LIBRARIES}
                          pcl_common 
                          pcl_io)

这部分内容和FAST-LIO2的内容类似，激光点云首先在LiDAR_front_end节点中提取特征点，将处理完的信息通过/laser_cloud_flat完成节点的发送出去，与FAST-LIO2相同R3LIVE也只用到了面特征作为ESIKF融合。