R3LIVE源码解析(10) — R3LIVE中r3live_vio.cpp文件

目录

1 r3live_vio.cpp简介

2 r3live_vio.cpp源码解析

1 r3live_vio.cpp简介

R3LIVE主要的公式推导在VIO上，所以我们来细细的分析这部分的功能。R3LIVE将VIO分成了两步，一是直接通过帧间的光流来追踪地图点，并且通过最小化追踪到的地图点的PNP投影误差来获取系统状态；二是通过这些地图点的出现的帧到地图的光度误差来优化状态。

2 r3live_vio.cpp源码解析

首先r3live_vio.cpp中，我们会先通过一个线程来获取图像的信息

void R3LIVE::image_comp_callback( const sensor_msgs::CompressedImageConstPtr &msg )
{
    std::unique_lock< std::mutex > lock2( mutex_image_callback );
    if ( sub_image_typed == 1 )
    {
        return; // Avoid subscribe the same image twice.
    }
    sub_image_typed = 2;
    g_received_compressed_img_msg.push_back( msg );
    
    // 如果是第一次收到图片，则启动一个线程
    if ( g_flag_if_first_rec_img )
    {
        g_flag_if_first_rec_img = 0;
        // 通过线程池k方法调用service_process_img_buffer函数来处理图像
        // 内部其实在循环调用process_image()函数
        m_thread_pool_ptr->commit_task( &R3LIVE::service_process_img_buffer, this );
    }
    return;
}
 
// ANCHOR - image_callback
void R3LIVE::image_callback( const sensor_msgs::ImageConstPtr &msg )
{
    std::unique_lock< std::mutex > lock( mutex_image_callback );
    if ( sub_image_typed == 2 )
    {
        return; // Avoid subscribe the same image twice.
    }
    sub_image_typed = 1;
 
    // 与上面的函数相同
    if ( g_flag_if_first_rec_img )
    {
        g_flag_if_first_rec_img = 0;
        m_thread_pool_ptr->commit_task( &R3LIVE::service_process_img_buffer, this );
    }
 
    // 将图像消息转opencv格式
    cv::Mat temp_img = cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 )->image.clone();
    // 图像预处理，然后保存到m_queue_image_with_pose队列
    process_image( temp_img, msg->header.stamp.toSec() );
}

 这里面通过 m_thread_pool_ptr->commit_task线程池的方式完成了图像buffer的压入与处理。在这个函数的末尾，会调用process_image函数

void R3LIVE::service_process_img_buffer()
{
    while ( 1 )
    {
        // To avoid uncompress so much image buffer, reducing the use of memory.
        // 如果m_queue_image_with_pose队列内的数据>4，表示这些数据还没被处理，暂时挂起预处理线程（丢一些数据）
        if ( m_queue_image_with_pose.size() > 4 )
        {
            while ( m_queue_image_with_pose.size() > 4 )
            {
                ros::spinOnce();
                std::this_thread::sleep_for( std::chrono::milliseconds( 2 ) );
                std::this_thread::yield();
            }
        }
        cv::Mat image_get;
        double  img_rec_time;
        
        // sub_image_typed == 2，表示接收的是压缩图像格式 
        if ( sub_image_typed == 2 )
        {
            // 如果队列中没有数据，暂停当前线程1s，以减少CPU的使用
            while ( g_received_compressed_img_msg.size() == 0 )
            {
                ros::spinOnce();
                std::this_thread::sleep_for( std::chrono::milliseconds( 1 ) );
                std::this_thread::yield();
            }
            // 从队列的前端获取一个压缩图像消息msg
            sensor_msgs::CompressedImageConstPtr msg = g_received_compressed_img_msg.front();
            try
            {
                // 将压缩图像消息转换为cv::Mat类型的图像数据
                cv_bridge::CvImagePtr cv_ptr_compressed = cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 );
                // 存储获取的时间和图像
                img_rec_time = msg->header.stamp.toSec();
                image_get = cv_ptr_compressed->image;
                // 释放内存
                cv_ptr_compressed->image.release();
            }
            catch ( cv_bridge::Exception &e )
            {
                printf( "Could not convert from '%s' to 'bgr8' !!! ", msg->format.c_str() );
            }
            mutex_image_callback.lock();
            g_received_compressed_img_msg.pop_front();
            mutex_image_callback.unlock();
        }
        else
        {
            // 如果队列中没有数据，暂停当前线程1s，以减少CPU的使用
            while ( g_received_img_msg.size() == 0 )
            {
                ros::spinOnce();
                std::this_thread::sleep_for( std::chrono::milliseconds( 1 ) );
                std::this_thread::yield();
            }
            // 与前面的流程类似，区别在于需要将接受的最前的图像pop
            sensor_msgs::ImageConstPtr msg = g_received_img_msg.front();
            image_get = cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 )->image.clone();
            img_rec_time = msg->header.stamp.toSec();
            mutex_image_callback.lock();
            g_received_img_msg.pop_front();
            mutex_image_callback.unlock();
        }
        process_image( image_get, img_rec_time );
    }
}

process_image函数主要做了三件事情，分别是检测时间戳，初始化参数，并启动service_pub_rgb_maps与service_VIO_update线程，以及去畸变与图像处理。

void R3LIVE::process_image( cv::Mat &tem