R3Live系列学习（五）R3Live源码阅读

另一篇长文预警~放下自己的懒惰，继续学习。。。

上一章结束时我曾反复从业务上强调纹理对点云的重要性，并对本系列的当前版本r3live加以推崇，只要开销承担的起，永远都是既要还要又要的东西在现有的市场更有竞争力。

编译r3live的时候可能会报“'ScalarBinaryOpTraits' in namespace 'Eigen' does not names a template type”之类的信息，这可能是由于电脑中存在多个Eigen库，可以在电脑上搜索出可能存在的Eigen库，不出意外的话会有包括ScalarBinaryOpTraits的也有没有包括的，直接在CMakeLists里面强行指定include路径，或者屏蔽没包括的那些Eigen库即可。

还是先看系统框图，作为对比我也po出了r2live的框图，如果已阅读了r2live的论文可能会比较熟悉。从系统框图上看，r3live将之前分开的点云地图（公开发布）和视觉地图（作为隐性参考）合并为纹理点云地图，LIO在维护点云的结构特征，VIO在维护点云中每个点的rgb值与置信度，与SVO中对像素逆深度的维护有些类似。另外的不同点是，r2live将LIO与VIO独立估计，而r3live建立的地图会为它们各自提供先验依据。在r2live中，作者会维护vio的滑动窗口，而在新作中，全部交给了global map，vio将更像是一个单纯的里程计。

 从launch文件上看，运行的进程是r3live_LiDAR_front_end和r3live_mapping，它们分别从src/loam/LiDAR_front_end.cpp和src/r3live.cpp编译得到。但和r2live一样，同在loam文件夹下的不同文件在不同的进程中运行，同学们不要看错了。

一、r3live_LiDAR_front_end进程

这个进程依旧是雷达前端的处理，输出的是点云角点和面点。虽然写法上稍有不同，但总体思路与之前的loam-livox一致，在此不再详写了。

    pub_full = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud", 100 );
    pub_surf = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_flat", 100 );
    pub_corn = n.advertise< sensor_msgs::PointCloud2 >( "/laser_cloud_sharp", 100 );

二、r3live_mapping进程

首先映入眼帘的特性是线程池，在这不打算评价使用线程池特性的优劣，正好上一篇写了一个简单的适应性线程池，也可以作为一种工具类进行参考，希望大家都能熟悉线程池的作用与使用逻辑。

m_thread_pool_ptr->commit_task(&R3LIVE::service_LIO_update, this);
m_thread_pool_ptr->commit_task( &R3LIVE::service_process_img_buffer, this );
m_thread_pool_ptr->commit_task( &R3LIVE::service_pub_rgb_maps, this);
m_thread_pool_ptr->commit_task( &R3LIVE::service_VIO_update, this);

 在R3LIVE类的构造函数与image callback函数中分别为线程池注册了这四个长期运行的函数。对于长期运行的线程，在池中注册和显式的调用线程api初始化没有差别。

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 ) );

在service_VIO_update函数中又会注册若干短期计算任务，用于渲染全局地图的rgb值与置信度。在线程池中可以将空闲的线程用于并发处理这些短期的计算。

我们根据功能的上下游，依次分析。

1、回调函数imu_cbk

从系统框图就能得知，imu数据一式两份分别供给vio与lio，这是整个系统轮询的驱动。一组imu数据有两个作用，给vio做预积分以及估计，给lio做点云去畸变。

imu_buffer_lio.push_back( msg );
imu_buffer_vio.push_back( msg );

 对imu误差状态的eskf递推，在ImuProcess::lic_state_propagate函数中实现，它使用了pvq、ba、bg的状态量，并且重点更新了误差状态的协方差，误差状态的均值并不重要，在这里就直接用周期内积分后的量。再次学习一下高博的简明ESKF推导简明ESKF推导 - 知乎。

 

 

2、lio线程函数service_LIO_update

在lio开始更新时，我们会获得一个“先验”，不管它是vio的结果还是frame到map的结果，它接下来的流程是老生常谈，流程与r2live一致，我们可以复习一下r2live论文的式20与附录D，它详细描述了lidar观测量对误差状态量p、q、v的雅可比矩阵的计算过程。

也就是对应着下面这一段，平面点构造对应关系完毕后，计算lidar观测量对误差状态量p、q、v的雅可比矩阵Hsub的过程。

/*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/
Eigen::MatrixXd Hsub( laserCloudSelNum, 6 );
Eigen::VectorXd meas_vec( laserCloudSelNum );
Hsub.setZero();

for ( int i = 0; i < laserCloudSelNum; i++ )
{
    const PointType &laser_p = laserCloudOri->points[ i ];
    Eigen::Vector3d  point_this( laser_p.x, laser_p.y, laser_p.z );
    point_this += Lidar_offset_to_IMU;
    Eigen::Matrix3d point_crossmat;
    point_crossmat << SKEW_SYM_MATRIX( point_this );

    /*** get the normal vector of closest surface/corner ***/
    const PointType &norm_p = coeffSel->points[ i ];
    Eigen::Vector3d  norm_vec( norm_p.x, norm_p.y, norm_p.z );

    /*** calculate the Measuremnt Jacobian matrix H ***/
    Eigen::Vector3d A( point_crossmat * g_lio_state.rot_end.transpose() * norm_vec );
    Hsub.row( i ) << VEC_FROM_ARRAY( A ), norm_p.x, norm_p.y, norm_p.z;

    /*** Measuremnt: distance to the closest surface/corner ***/
    meas_vec( i ) = -norm_p.intensity;
}

 本作的一大创新点在于vio更新完毕后，再做一次scan to map的更新，也就是本帧的rgb值映射到全局地图rgb值上，作者认为这个操作除了美观之外能够有效降低漂移量。那么在lio结束更新之后，会用lidar点云的xyz值，更新到全局地图的voxel上，这个过程和三维重建的TSDF函数有点相似，都是一种加权融合。

m_number_of_new_visited_voxel = m_map_rgb_pts.append_points_to_global_map(
                        *laserCloudFullResColor, Measures.lidar_end_time - g_camera_lidar_queue.m_first_imu_time, &pts_last_hitted, m_append_global_map_point_step );

 当新的一批地图点与全局地图“碰撞”，先算地图点落在哪个栅格中，然后视情况更新栅格，但在lio线程中只扩展了xyz坐标，纹理要等到vio线程中渲染。

template <typename T>
int Global_map::append_points_to_global_map(pcl::PointCloud<T> &pc_in, double  added_time,  std::vector<std::shared_ptr<RGB_pts>> *pts_added_vec, int step)
{
    m_in_appending_pts = 1;
    Common_tools::Timer tim;
    tim.tic();
    int acc = 0;
    int rej = 0;
    if (pts_added_vec != nullptr)
    {
        pts_added_vec->clear();
    }
    std::unordered_set< std::shared_ptr< RGB_Voxel > > voxels_recent_visited;
    if (m_recent_visited_voxel_activated_time == 0)
    {
        voxels_recent_visited.clear();
    }
    else
    {
        m_mutex_m_box_recent_hitted->lock();
        /// 选出最近命中的栅格，备用于vio线程中用最新的帧渲染纹理
        voxels_recent_visited = m_voxels_recent_visited;
        m_mutex_m_box_recent_hitted->unlock();
        for( Voxel_set_iterator it = voxels_recent_visited.begin(); it != voxels_recent_visited.end();  )
        {
            if ( added_time - ( *it )->m_last_visited_time > m_recent_visited_voxel_activated_time )
            {
                it = voxels_recent_visited.erase( it );
                continue;
            }
            it++;
        }
        cout << "Restored voxel number = " << voxels_recent_visited.size() << endl;
    }
    int number_of_voxels_before_add = voxels_recent_visited.size();
    int pt_size = pc_in.points.size();
    // step = 4;
    for (int pt_idx = 0; pt_idx < pt_size; pt_idx += step)
    {
        int add = 1;
        /// 点云的最小分辨率是0.05m，栅格的最小分辨率是0.1m，在更新地图的时候用到的是栅格voxel数据
        int grid_x = std::round(pc_in.points[pt_idx].x / m_minimum_pts_size);
        int grid_y = std::round(pc_in.points[pt_idx].y / m_minimum_pts_size);
        int grid_z = std::round(pc_in.points[pt_idx].z / m_minimum_pts_size);
        int box_x =  std::round(pc_in.points[pt_idx].x / m_voxel_resolution);
        int box_y =  std::round(pc_in.points[pt_idx].y / m_voxel_resolution);
        int box_z =  std::round(pc_in.points[pt_idx].z / m_voxel_resolution);
        /// 如果需要保存地图点或者进行稠密重建则将当前地图点保存到地图点的hashmap中
        if (m_hashmap_3d_pts.if_exist(grid_x, grid_y, grid_z))
        {
            add = 0;
            if (pts_added_vec != nullptr)
            {
                pts_added_vec->push_back(m_hashmap_3d_pts.m_map_3d_hash_map[grid_x][grid_y][grid_z]);
            }
        }
        /// 新建一个栅格，记录下最近命中与时间，视情况是否扩展整个地图栅格
        RGB_voxel_ptr box_ptr;
        if(!m_hashmap_voxels.if_exist(box_x, box_y, box_z))
        {
            std::shared_ptr<RGB_Voxel> box_rgb = std::make_shared<RGB_Voxel>();
            m_hashmap_voxels.insert( box_x, box_y, box_z, box_rgb );
            box_ptr = box_rgb;
        }
        else
        {
            box_ptr = m_hashmap_voxels.m_map_3d_hash_map[box_x][box_y][box_z];
        }
        voxels_recent_visited.insert( box_ptr );
        box_ptr->m_last_visited_time = added_time;
        if (add == 0)
        {
            rej++;
            continue;
        }
        acc++;
        /// 每个lidar点都新建一个RGB_pts，但现在只有xyz坐标，待渲染
        std::shared_ptr<RGB_pts> pt_rgb = std::make_shared<RGB_pts>();
        pt_rgb->set_pos(vec_3(pc_in.points[pt_idx].x, pc_in.points[pt_idx].y, pc_in.points[pt_idx].z));
        pt_rgb->m_pt_index = m_rgb_pts_vec.size();
        m_rgb_pts_vec.push_back(pt_rgb);
        m_hashmap_3d_pts.insert(grid_x, grid_y, grid_z, pt_rgb);
        box_ptr->add_pt(pt_rgb);
        if (pts_added_vec != nullptr)
        {
            pts_added_vec->push_back(pt_rgb);
        }
    }
    m_in_appending_pts = 0;
    m_mutex_m_box_recent_hitted->lock();
    m_voxels_recent_visited = voxels_recent_visited ;
    m_mutex_m_box_recent_hitted->unlock();
    return (m_voxels_recent_visited.size() -  number_of_voxels_before_add);
}

3、vio线程函数service_VIO_update
从论文中可知，在这个线程中做了两步更新，第一步也就是和r2live一样做帧到帧的更新，但这两个函数长得实在太像了，在此就只分析vio_photometric函数。作者将vio中的全局地图调用全部挪到了Rgbmap_tracker类中，在这个类里对一帧图像和全局地图做了索引，图像间的跟踪点在地图中有对应的指针，这样无论是帧帧估计还是到地图的估计，都能快速建立残差对应关系。

 使用global map的另一个好处就是，不用再维护边缘化滑窗了。在帧间更新完毕后，作者提出了创新点（帧到地图的更新），残差的构造和求导和lio中的点到面距离的形式类似，可以参考r2live论文的附录E。

 

  vio_photometric函数使用的也是eskf，重要的是求观测量对误差状态向量的各个偏导数（H矩阵），这样能够使观测量的协方差与预测的协方差统一尺度。在这里，直接拿vio_esikf函数计算的状态作为预测。

bool R3LIVE::vio_photometric( StatesGroup &state_in, Rgbmap_tracker &op_track, std::shared_ptr< Image_frame > &image )
{
    Common_tools::Timer tim;
    tim.tic();
    StatesGroup state_iter = state_in;
    if ( !m_if_estimate_intrinsic )     // When disable the online intrinsic calibration.
    {
        state_iter.cam_intrinsic << g_cam_K( 0, 0 ), g_cam_K( 1, 1 ), g_cam_K( 0, 2 ), g_cam_K( 1, 2 );
    }
    if ( !m_if_estimate_i2c_extrinsic ) // When disable the online extrinsic calibration.
    {
        state_iter.pos_ext_i2c = m_inital_pos_ext_i2c;
        state_iter.rot_ext_i2c = m_inital_rot_ext_i2c;
    }
    Eigen::Matrix< double, -1, -1 >                       H_mat, R_mat_inv;
    Eigen::Matrix< double, -1, 1 >                        meas_vec;
    Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > G, H_T_H, I_STATE;
    Eigen::Matrix< double, DIM_OF_STATES, 1 >             solution;
    Eigen::Matrix< double, -1, -1 >                       K, KH;
    Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > K_1;
    Eigen::SparseMatrix< double >                         H_mat_spa, H_T_H_spa, R_mat_inv_spa, K_spa, KH_spa, vec_spa, I_STATE_spa;
    I_STATE.setIdentity();
    I_STATE_spa = I_STATE.sparseView();
    double fx, fy, cx, cy, time_td;

    int                   total_pt_size = op_track.m_map_rgb_pts_in_current_frame_pos.size();
    std::vector< double > last_reprojection_error_vec( total_pt_size ), current_reprojection_error_vec( total_pt_size );
    if ( total_pt_size < minimum_iteration_pts )
    {
        state_in = state_iter;
        return false;
    }

    int err_size = 3;
    H_mat.resize( total_pt_size * err_size, DIM_OF_STATES );
    meas_vec.resize( total_pt_size * err_size, 1 );
    R_mat_inv.resize( total_pt_size * err_size, total_pt_size * err_size );

    double last_repro_err = 3e8;
    int    avail_pt_count = 0;
    double last_avr_repro_err = 0;
    int    if_esikf = 1;

    double acc_photometric_error = 0;
    for ( int iter_count = 0; iter_count < 2; iter_count++ )
    {
        mat_3_3 R_imu = state_iter.rot_end;
        vec_3   t_imu = state_iter.pos_end;
        vec_3   t_c2w = R_imu * state_iter.pos_ext_i2c + t_imu;
        mat_3_3 R_c2w = R_imu * state_iter.rot_ext_i2c; // world to camera frame

        fx = state_iter.cam_intrinsic( 0 );
        fy = state_iter.cam_intrinsic( 1 );
        cx = state_iter.cam_intrinsic( 2 );
        cy = state_iter.cam_intrinsic( 3 );
        time_td = state_iter.td_ext_i2c_delta;

        vec_3   t_w2c = -R_c2w.transpose() * t_c2w;
        mat_3_3 R_w2c = R_c2w.transpose();
        int     pt_idx = -1;
        acc_photometric_error = 0;
        vec_3               pt_3d_w, pt_3d_cam;
        vec_2               pt_img_measure, pt_img_proj, pt_img_vel;
        eigen_mat_d< 2, 3 > mat_pre;
        eigen_mat_d< 3, 2 > mat_photometric;
        eigen_mat_d< 3, 3 > mat_d_pho_d_img;
        eigen_mat_d< 3, 3 > mat_A, mat_B, mat_C, mat_D, pt_hat;
        R_mat_inv.setZero();
        H_mat.setZero();
        solution.setZero();
        meas_vec.setZero();
        avail_pt_count = 0;
        int iter_layer = 0;
        tim.tic( "Build_cost" );
        for ( auto it = op_track.m_map_rgb_pts_in_last_frame_pos.begin(); it != op_track.m_map_rgb_pts_in_last_frame_pos.end(); it++ )
        {
            if ( ( ( RGB_pts * ) it->first )->m_N_rgb < 3 )
            {
                continue;
            }
            pt_idx++;
            pt_3d_w = ( ( RGB_pts * ) it->first )->get_pos();
            /// 对齐相机曝光时间和imu的时间差
            pt_img_vel = ( ( RGB_pts * ) it->first )->m_img_vel;
            pt_img_measure = vec_2( it->second.x, it->second.y );
            /// 将地图点转到相机坐标系下后投影到相机平面
            pt_3d_cam = R_w2c * pt_3d_w + t_w2c;
            pt_img_proj = vec_2( fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ) + cx, fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 ) + cy ) + time_td * pt_img_vel;
            
            /// 每个图像帧的跟踪点在地图中存有一个索引，因此可获取上次融合后的地图点rgb值和协方差
            vec_3   pt_rgb = ( ( RGB_pts * ) it->first )->get_rgb();
            mat_3_3 pt_rgb_info = mat_3_3::Zero();
            mat_3_3 pt_rgb_cov = ( ( RGB_pts * ) it->first )->get_rgb_cov();
            /// 作为观测量，协方差矩阵在更新公式中出现在逆的位置
            for ( int i = 0; i < 3; i++ )
            {
                pt_rgb_info( i, i ) = 1.0 / pt_rgb_cov( i, i ) / 255;
                R_mat_inv( pt_idx * err_size + i, pt_idx * err_size + i ) = pt_rgb_info( i, i );
                // R_mat_inv( pt_idx * err_size + i, pt_idx * err_size + i ) =  1.0;
            }
            vec_3  obs_rgb_dx, obs_rgb_dy;
            /// 获取这一帧中跟踪点的亚像素级rgb值
            vec_3  obs_rgb = image->get_rgb( pt_img_proj( 0 ), pt_img_proj( 1 ), 0, &obs_rgb_dx, &obs_rgb_dy );
            vec_3  photometric_err_vec = ( obs_rgb - pt_rgb );
            /// huber loss可以显著降低离群点对优化的影响
            double huber_loss_scale = get_huber_loss_scale( photometric_err_vec.norm() );
            photometric_err_vec *= huber_loss_scale;
            double photometric_err = photometric_err_vec.transpose() * pt_rgb_info * photometric_err_vec;

            acc_photometric_error += photometric_err;

            last_reprojection_error_vec[ pt_idx ] = photometric_err;

            avail_pt_count++;
            /// 内参投影对地图点的雅可比矩阵
            mat_pre << fx / pt_3d_cam( 2 ), 0, -fx * pt_3d_cam( 0 ) / pt_3d_cam( 2 ), 0, fy / pt_3d_cam( 2 ), -fy * pt_3d_cam( 1 ) / pt_3d_cam( 2 );
            mat_d_pho_d_img = mat_photometric * mat_pre;

            pt_hat = Sophus::SO3d::hat( ( R_imu.transpose() * ( pt_3d_w - t_imu ) ) );

            /// 观测量对误差的雅可比矩阵的推导参照r2live附录E
            /// 对姿态R的雅可比
            mat_A = state_iter.rot_ext_i2c.transpose() * pt_hat;
            /// 对位移t的雅可比
            mat_B = -state_iter.rot_ext_i2c.transpose() * ( R_imu.transpose() );
            /// 对外参旋转量的雅可比
            mat_C = Sophus::SO3d::hat( pt_3d_cam );
            /// 对外参位移量的雅可比
            mat_D = -state_iter.rot_ext_i2c.transpose();
            meas_vec.block( pt_idx * 3, 0, 3, 1 ) = photometric_err_vec ;

            /// 链式求导的结果
            H_mat.block( pt_idx * 3, 0, 3, 3 ) = mat_d_pho_d_img * mat_A * huber_loss_scale;
            H_mat.block( pt_idx * 3, 3, 3, 3 ) = mat_d_pho_d_img * mat_B * huber_loss_scale;
            if ( 1 )
            {
                if ( m_if_estimate_i2c_extrinsic )
                {
                    H_mat.block( pt_idx * 3, 18, 3, 3 ) = mat_d_pho_d_img * mat_C * huber_loss_scale;
                    H_mat.block( pt_idx * 3, 21, 3, 3 ) = mat_d_pho_d_img * mat_D * huber_loss_scale;
                }
            }
        }
        R_mat_inv_spa = R_mat_inv.sparseView();
        
        last_avr_repro_err = acc_photometric_error;
        if ( avail_pt_count < minimum_iteration_pts )
        {
            break;
        }
        // Esikf
        tim.tic( "Iter" );
        if ( if_esikf )
        {
            /// sparseView使矩阵无需申请过大内存
            /// 经典的计算增益K的过程
            H_mat_spa = H_mat.sparseView();
            Eigen::SparseMatrix< double > Hsub_T_temp_mat = H_mat_spa.transpose();
            vec_spa = ( state_iter - state_in ).sparseView();
            H_T_H_spa = Hsub_T_temp_mat * R_mat_inv_spa * H_mat_spa;
            /// 这一帧与地图重叠部分越多视觉测量权重越大
            Eigen::SparseMatrix< double > temp_inv_mat =
                ( H_T_H_spa.toDense() + ( state_in.cov * m_cam_measurement_weight ).inverse() ).inverse().sparseView();
            Eigen::SparseMatrix< double > Ht_R_inv = ( Hsub_T_temp_mat * R_mat_inv_spa );
            KH_spa = Ht_R_inv * H_mat_spa;
            solution =
                ( temp_inv_mat * ( Ht_R_inv * ( ( -1 * meas_vec.sparseView() ) ) ) - ( I_STATE_spa - temp_inv_mat * KH_spa ) * vec_spa ).toDense();
        }
        state_iter = state_iter + solution;
        // state_iter.cov = ((I_STATE_spa - KH_spa) * state_iter.cov.sparseView()).toDense();
        if ( ( acc_photometric_error / total_pt_size ) < 10 ) // By experience.
        {
            break;
        }
        if ( fabs( acc_photometric_error - last_repro_err ) < 0.01 )
        {
            break;
        }
        last_repro_err = acc_photometric_error;
    }
    if ( if_esikf && avail_pt_count >= minimum_iteration_pts )
    {
        state_iter.cov = ( ( I_STATE_spa - KH_spa ) * state_iter.cov.sparseView() ).toDense();
    }
    state_iter.td_ext_i2c += state_iter.td_ext_i2c_delta;
    state_iter.td_ext_i2c_delta = 0;
    state_in = state_iter;
    return true;
}

但这样的方案很依赖初始的系统误差。如果lidar、imu没有出厂固连并标定的话，则需要比较精确的传感器标定和时间同步，而且初始时，需要静止一会儿并注册最开始的地图（使点云与纹理准确对应），才能获得比较好的效果。

 在两次状态估计完成后进行新帧对全局地图的纹理更新，它是利用render_pts_in_voxels_mp函数，在线程池中计算得到结果，这一块可以像TSDF融合一样用CUDA扩展。在线程中对一帧分块计算，思路比较简明。

static inline double thread_render_pts_in_voxel(const int & pt_start, const int & pt_end, const std::shared_ptr<Image_frame> & img_ptr,
                                                const std::vector<RGB_voxel_ptr> * voxels_for_render, const double obs_time)
{
    vec_3 pt_w;
    vec_3 rgb_color;
    double u, v;
    double pt_cam_norm;
    Common_tools::Timer tim;
    tim.tic();
    for (int voxel_idx = pt_start; voxel_idx < pt_end; voxel_idx++)
    {
        // continue;
        RGB_voxel_ptr voxel_ptr = (*voxels_for_render)[ voxel_idx ];
        for ( int pt_idx = 0; pt_idx < voxel_ptr->m_pts_in_grid.size(); pt_idx++ )
        {
            pt_w = voxel_ptr->m_pts_in_grid[pt_idx]->get_pos();
            if ( img_ptr->project_3d_point_in_this_img( pt_w, u, v, nullptr, 1.0 ) == false )
            {
                continue;
            }
            pt_cam_norm = ( pt_w - img_ptr->m_pose_w2c_t ).norm();
            // double gray = img_ptr->get_grey_color(u, v, 0);
            // pts_for_render[i]->update_gray(gray, pt_cam_norm);
            rgb_color = img_ptr->get_rgb( u, v, 0 );
            if (  voxel_ptr->m_pts_in_grid[pt_idx]->update_rgb(
                     rgb_color, pt_cam_norm, vec_3( image_obs_cov, image_obs_cov, image_obs_cov ), obs_time ) )
            {
                render_pts_count++;
            }
        }
    }
    double cost_time = tim.toc() * 100;
    return cost_time;
}

​

在RGB_pts类的声明中，每个像素点都拥有协方差，而融合的计算依据是观测帧与该地图点的距离，以及最新观测与之前观测的时间差，融合采取的是贝叶斯公式，用不同的时刻的协方差加权融合。

​

 公式33对应的就是update_rgb函数，可以看到，我们在更新纹理的过程中必须是相距比较近的一些帧做融合，这样误差才可控，如果另外又有远远的帧观测到了则无视之。另外，距离上一次观测的时间间距会作为噪声项，影响新观测的协方差。

int RGB_pts::update_rgb(const vec_3 &rgb, const double obs_dis, const vec_3 obs_sigma, const double obs_time)
{
    if (m_obs_dis != 0 && (obs_dis > m_obs_dis * 1.2))
    {
        return 0;
    }

    if( m_N_rgb == 0)
    {
        // For first time of observation.
        m_last_obs_time = obs_time;
        m_obs_dis = obs_dis;
        for (int i = 0; i < 3; i++)
        {
            m_rgb[i] = rgb[i];
            m_cov_rgb[i] = obs_sigma(i) ;
        }
        m_N_rgb = 1;
        return 0;
    }
    // State estimation for robotics, section 2.2.6, page 37-38
    for(int i = 0 ; i < 3; i++)
    {
        m_cov_rgb[i] = (m_cov_rgb[i] + process_noise_sigma * (obs_time - m_last_obs_time)); // Add process noise
        double old_sigma = m_cov_rgb[i];
        m_cov_rgb[i] = sqrt( 1.0 / (1.0 / m_cov_rgb[i] / m_cov_rgb[i] + 1.0 / obs_sigma(i) / obs_sigma(i)) );
        m_rgb[i] = m_cov_rgb[i] * m_cov_rgb[i] * ( m_rgb[i] / old_sigma / old_sigma + rgb(i) / obs_sigma(i) / obs_sigma(i) );
    }

    if (obs_dis < m_obs_dis)
    {
        m_obs_dis = obs_dis;
    }
    m_last_obs_time = obs_time;
    m_N_rgb++;
    return 1;
}

 融合完毕后，进行了两步后处理，重置了全局地图中最新投影的帧，以及刷新了光流跟踪的特征点，将本帧中融合纹理成功的点按阈值选取，便于下一次光流跟踪。

m_map_rgb_pts.update_pose_for_projection( img_pose, -0.4 );
op_track.update_and_append_track_pts( img_pose, m_map_rgb_pts, m_track_windows_size / m_vio_scale_factor, 1000000 );

这样便大致实现了vio-lio-global map的联动。从设计的角度上看，它将本来就不大的漂移量又缩小了一个数量级，在作者提供的数据集里，跑上几个来回也没有什么重影（也与地图更新的策略有关），这种更新策略有点和我之前做的扫描仪系统类似，有重合部分就留一去一，设计更加nb的回环策略，或者说更鲁棒更动态的自适应策略，是否是个让人遐想的方向？另外，好的效果前提就是它们是贵的传感器，而且帧的rgb值跟踪并更新map，本身就是一个很强的假设，曝光带来的光度误差是无法忽略的。

整个系列一路学习下来，其实在面对一个比较陌生的研究方向时，先阅读论文是最好的方法，在大致领悟到作者的意图之后，再将自己置身作者的角度，假设是自己来设计，如何达到这样的效果（前提是真的领悟到了意图并且有一定的领域内沉淀），在自己思考设计的过程中，如果有模糊不清的地方，再和论文、源码比对，这样就能将作者当成是一位师兄，阅读源码也就成了与作者隔空交流开组会的过程，能起到事半功倍的效果。尤其是在最初作者还未开源，我尝试复现global map模块的时候就采取了scan to map的思路，后面发现与作者不谋而合，这感觉再妙不过了。

接下来我将继续延伸R3Live的MVS和meshing模块，这是计算机视觉和三维重建领域比较经典的理论，值得学习，感兴趣的同学可以一起跟进。