R3LIVE相关参考：
R3LIVE(升级R2LIVE)：编译与运行

https://blog.csdn.net/handily_1/article/details/122271243?spm=1001.2014.3001.5502
R3live：整体分析
https://blog.csdn.net/handily_1/article/details/122360134?spm=1001.2014.3001.5502
运行代码报错：Failed to load module “canberra-gtk-module“
https://blog.csdn.net/handily_1/article/details/122359275
R3live笔记：（图像处理）视觉-惯性里程计VIO部分
https://blog.csdn.net/handily_1/article/details/122377514

r3live VIO

r3live的VIO子系统绘制了全局地图的纹理，通过最小化光度误差来估计系统的状态，即：
将全局地图中一定数量的点（即跟踪的点）投影到当前图像中，
然后最小化这些点的光度误差，在ESIKF框架内迭代估计系统状态。
为了提高效率，跟踪的地图点是稀疏的，这通常需要构建输入图像的金字塔。
然而，金字塔对平移或旋转都不是不变的，也需要被估计。
r3live框架中，利用单个地图点的颜色来计算光度误差,
因为颜色是地图点的固有属性，对相机的平移和旋转都是不变的。
根据r3live框图的VIO部分：

首先利用一个帧对帧（frame-to-frame）的光流来跟踪地图点，
通过最小化跟踪地图点的Perspective-n-Point（PnP）投影误差，
然后，通过最小化跟踪点之间的帧到地图（frame-to-map的光度误差来进一步细化系统的状态估计，
有了收敛的状态估计和原始输入图像，执行纹理渲染来更新全局地图中点的颜色。

1. 帧对帧（frame-to-frame）

假设在最后一个图像帧中Ik-1跟踪到了 m个地图点 ，它们在 Ik-1上的投影为 [公式] ，利用Lucas-Kanade光流来找出它们在当前图像帧 Ik中的位置，标记为 。然后，通过ESIKF滤波器，我们计算和优化它们的投影误差来估计状态。

1）PnP投影误差 Perspective-n-Point projection error

投影误差计算如下：

其中：

公式(5)中，第一项是针孔相机模型的投影函数，第二项为在线时间校正因子（Online-temporal correction factor）。
误差公式(4)中的测量噪声包含两个源：
一个是像素跟踪误差

另一个是地图点位置误差

2）帧到帧视觉里程计ESIKF更新 Frame-to-frame VIO ESIKF update

公式(8)构成了xk的观测分布，它可以与IMU传播的先验分布相结合，以得到最大后验MAP估计：

2.帧到地图 视觉惯性里程计 Frame-to-map Visual-Inertial odometry

3.渲染全局地图的纹理 Render the texture of global map

在帧到地图的VIO更新后，我们得到了当前图像的精确位姿，然后我们执行渲染功能来更新地图点的颜色。

4.视觉惯性里程计子系统的跟踪点更新 Update of the tracking points of VIO subsystem

参考：【论文阅读】R3LIVE：A Robust RealTime RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation

---

代码

1. IMAGE_topic

订阅：

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

图像topic数据传入的时候，回调函数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 );
    }

    // 将图像编码转换为BGR8
    cv::Mat temp_img = cv_bridge::toCvCopy( msg, sensor_msgs::image_encodings::BGR8 )->image.clone();
    // 图像数据进行处理
    process_image( temp_img, msg->header.stamp.toSec() );
}

图像数据进行处理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;
    }
// 这一帧的时间戳小于上一帧的时间戳    错误
    if ( msg_time < last_accept_time )
    {
    
        cout << "Error, image time revert!!" << endl;
        return;
    }
// 这一帧的时间戳与上一帧的时间戳间隔小于发布频率 错误
    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;

    // 实际上是第一帧图像进来的时候进行的初始化操作
    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();
        // 相机参数初始化，包括内参、外参、畸变参数等
        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：畸变校正，opencv提供的可以直接使用的矫正算法，和remap()组合来处理
        // 这个函数用于计算无畸变和修正转换关系，为了重映射，将结果以映射的形式表达。无畸变的图像看起来就像原始的图像，就像这个图像是用内参为newCameraMatrix的且无畸变的相机采集得到的。 
        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 );
    }
    // 降采样=>图像大小重新 设置
    if ( m_image_downsample_ratio != 1.0 )
    {
    
        // 图像大小重新 设置
        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 );

    // 原图复制给img_pose
    if ( m_if_pub_raw_img )
    {
    
        img_pose->m_raw_img = img_get;
    }
    // 重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
    // TODO 什么作用？
    // INTER_LINEAR – 双线性插值
    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立方插值初始化
    img_pose->image_equalize();// 直方图均衡化处理
    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

解压缩compressed格式图像
订阅：

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

回调：

void R3LIVE::image_comp_callback( const sensor_msgs::CompressedImageConstPtr &msg )
{
    
    // 互斥锁
    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;
}