R3live notes: image processing section

1. IMAGE_topic

subscribe ：

        sub_img = m_ros_node_handle.subscribe(IMAGE_topic.c_str(), 1000000, &R3LIVE::image_callback, this, ros::TransportHints().tcpNoDelay());

Images topic When data is transferred in , Callback function 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;

    //  Determine whether it is the first frame 
    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 );
    }

    //  Convert the image encoding to BGR8
    cv::Mat temp_img = cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 )->image.clone();
    //  Image data processing 
    process_image( temp_img, msg->header.stamp.toSec() );
}

Image data processing process_image：

void   R3LIVE::process_image( cv::Mat &temp_img, double msg_time )
{
    
    cv::Mat img_get;
    if ( temp_img.rows == 0 )
    {
    
        cout << "Process image error, image rows =0 " << endl;
        return;
    }
//  The timestamp of this frame is smaller than that of the previous frame   error 
    if ( msg_time < last_accept_time )
    {
    
        cout << "Error, image time revert!!" << endl;
        return;
    }
//  The interval between the timestamp of this frame and the timestamp of the previous frame is less than the release frequency   error 
    if ( ( msg_time - last_accept_time ) < ( 1.0 / m_control_image_freq ) * 0.9 )
    {
    
        cout << "Error, image time interval wrong!!" << endl;
        return;
    }
    last_accept_time = msg_time;

    //  In fact, it is the initialization operation when the first image comes in 
    if ( m_camera_start_ros_tim < 0 )
    {
    
        m_camera_start_ros_tim = msg_time;
        m_vio_scale_factor = 1280 * m_image_downsample_ratio / temp_img.cols; // 320 * 24
        // load_vio_parameters();
        //  Camera parameter initialization , Including internal reference 、 External reference 、 Distortion parameters, etc 
        set_initial_camera_parameter( g_lio_state, m_camera_intrinsic.data(), m_camera_dist_coeffs.data(), m_camera_ext_R.data(),
                                      m_camera_ext_t.data(), m_vio_scale_factor );
        cv::eigen2cv( g_cam_K, intrinsic );
        cv::eigen2cv( g_cam_dist, dist_coeffs );

        // TODO QJF： Distortion correction ,opencv Provided correction algorithm that can be used directly , and remap() Combine to handle 
        //  This function is used to calculate the distortion free and corrected conversion relationship , To remap , Express the result in the form of mapping . The undistorted image looks like the original image , It's like this image uses internal parameters as newCameraMatrix And distortion free camera acquisition . 
        initUndistortRectifyMap( intrinsic, dist_coeffs, cv::Mat(), intrinsic, cv::Size( 1280 / m_vio_scale_factor, 1024 / m_vio_scale_factor ),
                                 CV_16SC2, m_ud_map1, m_ud_map2 );
        m_thread_pool_ptr->commit_task( &R3LIVE::service_pub_rgb_maps, this);
        m_thread_pool_ptr->commit_task( &R3LIVE::service_VIO_update, this);
        m_mvs_recorder.init( g_cam_K, 1280 / m_vio_scale_factor, &m_map_rgb_pts );
        m_mvs_recorder.set_working_dir( m_map_output_dir );
    }
    //  Downsampling => Image size again   Set up 
    if ( m_image_downsample_ratio != 1.0 )
    {
    
        //  Image size again   Set up 
        cv::resize( temp_img, img_get, cv::Size( 1280 / m_vio_scale_factor, 1024 / m_vio_scale_factor ) );
    }
    else
    {
    
        img_get = temp_img; // clone ?
    }
    std::shared_ptr< Image_frame > img_pose = std::make_shared< Image_frame >( g_cam_K );

    //  Copy the original image to img_pose
    if ( m_if_pub_raw_img )
    {
    
        img_pose->m_raw_img = img_get;
    }
    //  Remap , It is the process of placing a pixel at a certain position in an image to a specified position in another image .
    // TODO  What's the role ？
    // INTER_LINEAR –  Bilinear interpolation 
    cv::remap( img_get, img_pose->m_img, m_ud_map1, m_ud_map2, cv::INTER_LINEAR );
    cv::imshow("sub Img", img_pose->m_img);
    img_pose->m_timestamp = msg_time;
    img_pose->init_cubic_interpolation();// Cubic interpolation Cubic interpolation initialization 
    img_pose->image_equalize();//  Histogram equalization processing 
    m_camera_data_mutex.lock();
    m_queue_image_with_pose.push_back( img_pose );
    m_camera_data_mutex.unlock();
    total_frame_count++;

    if ( m_queue_image_with_pose.size() > buffer_max_frame )
    {
    
        buffer_max_frame = m_queue_image_with_pose.size();
    }

    // cout << "Image queue size = " << m_queue_image_with_pose.size() << endl;
}

2. IMAGE_topic_compressed

decompression compressed Format image
subscribe ：

        sub_img_comp = m_ros_node_handle.subscribe(IMAGE_topic_compressed.c_str(), 1000000, &R3LIVE::image_comp_callback, this, ros::TransportHints().tcpNoDelay());

Callback ：

void R3LIVE::image_comp_callback( const sensor_msgs::CompressedImageConstPtr &msg )
{
    
    //  The mutex 
    std::unique_lock< std::mutex > lock2( mutex_image_callback );
    if ( sub_image_typed == 1 ) // 0: TBD 1: sub_raw, 2: sub_comp
    {
    
        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;
        m_thread_pool_ptr->commit_task( &R3LIVE::service_process_img_buffer, this );
    }
    return;
}