R3LIVE代码详解（二）

0. 简介

我们在上文中提到R3LIVE主要由两个node节点所依赖的cpp文件组成，我们在上一节中完成了r3live_LiDAR_front_end 简单介绍，下面我们需要详细的看/r3live_mapping这个节点。这个节点也是我们R3LIVE的核心。

1.主函数进入

首先我们知道R3LIVE的主函数进入在r3live.cpp这个文件夹中，其中cpp文件中没有很多值得关注的东西。基本是初始化一些基础的参数，唯一值得我们关注的就是new R3LIVE()函数了，我们到r3live.hpp文件中来详细看一下。

Camera_Lidar_queue g_camera_lidar_queue;
MeasureGroup Measures;
StatesGroup g_lio_state;
std::string data_dump_dir = std::string("/mnt/0B3B134F0B3B134F/color_temp_r3live/");

int main(int argc, char **argv)
{
    printf_program("R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package");
    Common_tools::printf_software_version();
    Eigen::initParallel();
    ros::init(argc, argv, "R3LIVE_main");
    R3LIVE * fast_lio_instance = new R3LIVE();
    ros::Rate rate(5000);
    bool status = ros::ok();
    ros::spin();
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

2. 初始化以及线程创建

在r3live.hpp文件中也存在大量的参数初始化，我们主要关注R3LIVE的构造函数。首先构造函数第一部分就是创建一系列Topic，用于订阅和发布信息。并获取一些param信息，在一般的代码中也是如此，没有什么需要特别讲的。

        pubLaserCloudFullRes = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/cloud_registered", 100);
        pubLaserCloudEffect = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/cloud_effected", 100);
        pubLaserCloudMap = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/Laser_map", 100);
        pubOdomAftMapped = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 10);
        pub_track_img = m_ros_node_handle.advertise<sensor_msgs::Image>("/track_img",1000);
        pub_raw_img = m_ros_node_handle.advertise<sensor_msgs::Image>("/raw_in_img",1000);
        m_pub_visual_tracked_3d_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/track_pts", 10);
        m_pub_render_rgb_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/render_pts", 10);
        pubPath = m_ros_node_handle.advertise<nav_msgs::Path>("/path", 10);

        pub_odom_cam = m_ros_node_handle.advertise<nav_msgs::Odometry>("/camera_odom", 10);
        pub_path_cam = m_ros_node_handle.advertise<nav_msgs::Path>("/camera_path", 10);
        std::string LiDAR_pointcloud_topic, IMU_topic, IMAGE_topic, IMAGE_topic_compressed;

        get_ros_parameter<std::string>(m_ros_node_handle, "/LiDAR_pointcloud_topic", LiDAR_pointcloud_topic, std::string("/laser_cloud_flat") );
        get_ros_parameter<std::string>(m_ros_node_handle, "/IMU_topic", IMU_topic, std::string("/livox/imu") );
        get_ros_parameter<std::string>(m_ros_node_handle, "/Image_topic", IMAGE_topic, std::string("/camera/image_color") );
        IMAGE_topic_compressed = std::string(IMAGE_topic).append("/compressed");
        if(1)
        {
            scope_color(ANSI_COLOR_BLUE_BOLD);
            cout << "======= Summary of subscribed topics =======" << endl;
            cout << "LiDAR pointcloud topic: " << LiDAR_pointcloud_topic << endl;
            cout << "IMU topic: " << IMU_topic << endl;
            cout << "Image topic: " << IMAGE_topic << endl;
            cout << "Image compressed topic: " << IMAGE_topic << endl;
             cout << "=======        -End-                =======" << endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }

        sub_imu = m_ros_node_handle.subscribe(IMU_topic.c_str(), 2000000, &R3LIVE::imu_cbk, this, ros::TransportHints().tcpNoDelay());
        sub_pcl = m_ros_node_handle.subscribe(LiDAR_pointcloud_topic.c_str(), 2000000, &R3LIVE::feat_points_cbk, this, ros::TransportHints().tcpNoDelay());
        sub_img = m_ros_node_handle.subscribe(IMAGE_topic.c_str(), 1000000, &R3LIVE::image_callback, this, ros::TransportHints().tcpNoDelay());
        sub_img_comp = m_ros_node_handle.subscribe(IMAGE_topic_compressed.c_str(), 1000000, &R3LIVE::image_comp_callback, this, ros::TransportHints().tcpNoDelay());

        m_ros_node_handle.getParam("/initial_pose", m_initial_pose);
        m_pub_rgb_render_pointcloud_ptr_vec.resize(1e3);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

然后我们来看一下本章的重点，即线程。在下面的代码中明确了线程数。我们来梳理一下整个代码中的线程。

 m_thread_pool_ptr = std::make_shared<Common_tools::ThreadPool>(6, true, false); // At least 5 threads are needs, here we allocate 6 threads.
1

2.1 LIO线程

在r3live.hpp同文件中，作者就完成了LIO线程的创建，这个线程是直接调用LIO计算ESIKF的操作。

// 启动LIO线程
        m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this);
1
2

2.2 VIO线程—图像处理线程

在r3live_vio.cpp中的回调函数image_callback和image_comp_callback函数中，会开启一个取出image处理线程service_process_img_buffer:用来专门处理原始的图像数据。

m_thread_pool_ptr->commit_task( &R3LIVE::service_process_img_buffer, this );
1

2.3 VIO线程—处理线程

在r3live_vio.cpp中，在第一次运行的时候会触发主线程，用于直接调用VIO计算ESIKF的操作。

m_thread_pool_ptr->commit_task( &R3LIVE::service_VIO_update, this); // vio线程
1

2.4 VIO线程—地图发布线程

在r3live_vio.cpp中，在第一次运行的时候也会触发rgb map的发布线程，将RGB点云地图拆分成了子点云发布。

m_thread_pool_ptr->commit_task( &R3LIVE::service_pub_rgb_maps, this);
1

2.5 VIO线程—点云与图像投影

最后一个线程也是在r3live_vio.cpp中，主要作用是将点云地图的active点投影到图像上，用对应的像素对地图点rgb的均值和方差用贝叶斯迭代进行更新。

 m_render_thread = std::make_shared< std::shared_future< void > >( m_thread_pool_ptr->commit_task(
                    render_pts_in_voxels_mp, img_pose, &m_map_rgb_pts.m_voxels_recent_visited, img_pose->m_timestamp ) );
1
2

此外在pointcloud_rgbd.cpp中也存在一个线程，只是该线程不是m_thread_pool_ptr线程池中的线程。它属于m_thread_service 线程池，里面只有一个线程service_refresh_pts_for_projection，在VIO线程中会通过
m_map_rgb_pts.update_pose_for_projection( img_pose, -0.4 );
完成调用，并更新位姿，完成active激光点与图像的投影

3. LIO线程

我们在上节中对整个线程进行了梳理，其中我们最熟悉的应该就是LIO线程了。因为这里面本质上整体流程和FAST-LIO2基本一致。我们大致的来看一下LIO线程对应的R3LIVE::service_LIO_update()函数。

首先会对比相机和lidar队列头的时间戳，如果camera的时间戳更早则等待vio线程把更早的图像处理完

 // 检查是否有可用的雷达数据用于处理，这里将等待图像的第一帧先处理，否则不往下继续
 while ( g_camera_lidar_queue.if_lidar_can_process() == false )
 {
     ros::spinOnce();
     std::this_thread::yield();
     std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
 }
1
2
3
4
5
6
7

然后通过下面的函数完成IMU与雷达在接收到数据buffer后的处理，完成打包

 // 将激光数据和IMU数据保存到MeasureGroup Measures
 if ( sync_packages( Measures ) == 0 )
 {
     continue;
 }
1
2
3
4
5

然后下一步就是调用Process 函数补偿点云畸变，并用imu数据进行系统状态预测。这里对应着FAST-LIO2中的UndistortPcl函数

…详情请参照古月居