R3live笔记：r3live的几个线程分析_service_pub_rgb_maps

今天在r3live类的构造函数里看到这样一行代码：

        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

大意是初始化一个包含6个线程的线程池子，然后作者注释
// At least 5 threads are needs, here we allocate 6 threads.
说明至少需要5个线程，就仔细来数一下都有哪些：

1. 一个取出image处理线程

订阅IMAGE_topic的回调函数image_callback和image_comp_callback里，开启一个取出image处理线程service_process_img_buffer:

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

这个线程里的主要内容：
首先为防止等待vio处理的图像队列m_queue_image_with_pose占用过多的缓存，m_queue_image_with_pose超过4帧时，先减少内存的占用
然后，根据输入图像是压缩图还是原图作出相应的处理，主要是把ros格式的图像转换成cv格式
最后，调用process_image函数处理图像：

        process_image( image_get, img_rec_time );
1

2. 一个VIO线程

VIO线程之前介绍过参考：[R3live笔记：视觉-惯性里程计VIO部分]

3. 一个LIO线程

在
R3LIVE()构造函数最后一行代码:

        m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this);//开启lio线程
1

开启lio线程

LIO线程来源于r2live,fast-lio，

主要是接收雷达数据，畸变补偿，特征提取，IMU状态传播，LIO状态更新

LIO线程参考：R3live笔记：从代码看lio线程

4. 一个发布rgb map的线程

在R3LIVE::process_image函数里，开启VIO线程和service_pub_rgb_maps线程：

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

VIO发布了一系列/RGB_map_xx话题（参见R3live：整体分析），这是将RGB点云地图拆分成了子点云发布，由一个线程专门负责，这个线程就是service_pub_rgb_maps线程

// /RGB_map_xx：将RGB点云地图拆分成子点云发布，由一个线程专门负责
void R3LIVE::service_pub_rgb_maps()
{
    int last_publish_map_idx = -3e8;
    int sleep_time_aft_pub = 10;  
    int number_of_pts_per_topic = 1000;
    if ( number_of_pts_per_topic < 0 )
    {
        return;
    }
    while ( 1 )
    {
        ros::spinOnce();
        std::this_thread::sleep_for( std::chrono::milliseconds( 10 ) );
        pcl::PointCloud< pcl::PointXYZRGB > pc_rgb;
        sensor_msgs::PointCloud2            ros_pc_msg;
        int pts_size = m_map_rgb_pts.m_rgb_pts_vec.size();
        pc_rgb.resize( number_of_pts_per_topic );
        // for (int i = pts_size - 1; i > 0; i--)
        int pub_idx_size = 0;
        int cur_topic_idx = 0;
        if ( last_publish_map_idx == m_map_rgb_pts.m_last_updated_frame_idx )
        {
            continue;
        }
        last_publish_map_idx = m_map_rgb_pts.m_last_updated_frame_idx;
        for ( int i = 0; i < pts_size; i++ )
        {
            if ( m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_N_rgb < 1 )
            {
                continue;
            }
            pc_rgb.points[ pub_idx_size ].x = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_pos[ 0 ];
            pc_rgb.points[ pub_idx_size ].y = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_pos[ 1 ];
            pc_rgb.points[ pub_idx_size ].z = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_pos[ 2 ];
            pc_rgb.points[ pub_idx_size ].r = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_rgb[ 2 ];
            pc_rgb.points[ pub_idx_size ].g = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_rgb[ 1 ];
            pc_rgb.points[ pub_idx_size ].b = m_map_rgb_pts.m_rgb_pts_vec[ i ]->m_rgb[ 0 ];
            // pc_rgb.points[i].intensity = m_map_rgb_pts.m_rgb_pts_vec[i]->m_obs_dis;
            pub_idx_size++;
            if ( pub_idx_size == number_of_pts_per_topic )
            {
                pub_idx_size = 0;
                pcl::toROSMsg( pc_rgb, ros_pc_msg );
                ros_pc_msg.header.frame_id = "world";       
                ros_pc_msg.header.stamp = ros::Time::now(); 
                if ( m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ] == nullptr )
                {
                    m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ] =
                        std::make_shared< ros::Publisher >( m_ros_node_handle.advertise< sensor_msgs::PointCloud2 >(
                            std::string( "/RGB_map_" ).append( std::to_string( cur_topic_idx ) ), 100 ) );
                }
                m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ]->publish( ros_pc_msg );
                std::this_thread::sleep_for( std::chrono::microseconds( sleep_time_aft_pub ) );
                ros::spinOnce();
                cur_topic_idx++;
            }
        }

        pc_rgb.resize( pub_idx_size );
        pcl::toROSMsg( pc_rgb, ros_pc_msg );
        ros_pc_msg.header.frame_id = "world";       
        ros_pc_msg.header.stamp = ros::Time::now(); 
        if ( m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ] == nullptr )
        {
            m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ] =
                std::make_shared< ros::Publisher >( m_ros_node_handle.advertise< sensor_msgs::PointCloud2 >(
                    std::string( "/RGB_map_" ).append( std::to_string( cur_topic_idx ) ), 100 ) );
        }
        std::this_thread::sleep_for( std::chrono::microseconds( sleep_time_aft_pub ) );
        ros::spinOnce();

        // /RGB_map_xx：将RGB点云地图拆分成子点云发布，由一个线程专门负责
        m_pub_rgb_render_pointcloud_ptr_vec[ cur_topic_idx ]->publish( ros_pc_msg );
        cur_topic_idx++;
        if ( cur_topic_idx >= 45 ) // Maximum pointcloud topics = 45.
        {
            number_of_pts_per_topic *= 1.5;
            sleep_time_aft_pub *= 1.5;
        }
    }
}

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
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83

5. 一个将点投影到图像的线程

在VIO线程里，又单独申请一个线程render_pts_in_voxels_mp，并行地将点云地图的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

6. 一个refresh线程

refresh线程严格讲不是前面讲的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 );
1

主要用于更新m_img_for_projection的位姿和id，以触发refresh线程

        m_thread_service = std::make_shared< std::thread >( &Global_map::service_refresh_pts_for_projection, this );
1

该线程将点云的active点投影到2D（方便后面新增跟踪点）