R3LIVE源码解析(9) — R3LIVE中r3live_lio.cpp文件

目录

1 r3live_lio.cpp文件简介

2 r3live_lio.cpp源码解析


1 r3live_lio.cpp文件简介

在r3live.cpp文件中创建LIO线程后,R3LIVE中的LIO线程本质上整体流程和FAST-LIO2基本一致。

2 r3live_lio.cpp源码解析

函数最开始会进行一系列的声明和定义,发布的lidar路径,协方差矩阵,kalman增益矩阵和4个重要的点云容器,IMU前向/后向传播处理器等

    /**
     * ===================================================================================================
     * @note (1) 定义用到的变量 : 发布的lidar路径,协方差矩阵,kalman增益矩阵和4个重要的点云容器,IMU前向/后向传播处理器等.
     * ===================================================================================================
     */
    nav_msgs::Path path;    // Lidar的路径 : 主要有两个成员变量: header和pose
    path.header.stamp = ros::Time::now();   // header的时间
    path.header.frame_id = "/world";        // header的id
    /*** variables definition ***/
    // DIM_OF_STATES=29. G:, H_T_H, I_STATE:
    // G : 用于传递协方差用的矩阵, H_T_H : 用于计算kalman增益用的, I_STATE : 单位阵
    Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > G, H_T_H, I_STATE;
    G.setZero();
    H_T_H.setZero();
    I_STATE.setIdentity();

    cv::Mat matA1( 3, 3, CV_32F, cv::Scalar::all( 0 ) );    // 后面没用到
    cv::Mat matD1( 1, 3, CV_32F, cv::Scalar::all( 0 ) );    // 后面没用到
    cv::Mat matV1( 3, 3, CV_32F, cv::Scalar::all( 0 ) );    // 后面没用到
    cv::Mat matP( 6, 6, CV_32F, cv::Scalar::all( 0 ) );    // 后面没用到

    PointCloudXYZINormal::Ptr feats_undistort( new PointCloudXYZINormal() );//去除畸变后的点云
    PointCloudXYZINormal::Ptr feats_down( new PointCloudXYZINormal() );     // 保存下采样后的点云
    // 有M个特征点找到了M个对应的最近平面Si,则coeffSel存储Si的平面方程系数,和点到平面的残差
    PointCloudXYZINormal::Ptr coeffSel( new PointCloudXYZINormal() );// 存放M个最近平面信息的容器: 平面方程,点-面残差
    PointCloudXYZINormal::Ptr laserCloudOri( new PointCloudXYZINormal() );// 存放找到了最近平面的M个点的容器

    /*** variables initialize ***/
    FOV_DEG = fov_deg + 10; // fov_deg=360
    HALF_FOV_COS = std::cos( ( fov_deg + 10.0 ) * 0.5 * PI_M / 180.0 );// cos(185)

    for ( int i = 0; i < laserCloudNum; i++ )   // laserCloudNum = 48x48x48
    {
        // featsArray[48x48x48]
        featsArray[ i ].reset( new PointCloudXYZINormal() );
    }

    std::shared_ptr< ImuProcess > p_imu( new ImuProcess() );    // 定义用于前向/后向传播的IMU处理器
    m_imu_process = p_imu;
    //------------------------------------------------------------------------------------------------------
    ros::Rate rate( 5000 );
    bool      status = ros::ok();
    g_camera_lidar_queue.m_liar_frame_buf = &lidar_buffer;//取出lidar数据
    set_initial_state_cov( g_lio_state );   //初始化g_lio_state的状态协方差矩阵

然后开始LIO线程的主循环。首先会对比相机和lidar队列头的时间戳,如果camera的时间戳更早则等待vio线程把更早的图像处理完。

  • td::this_thread::yield()函数的作用是让出当前线程的执行权,让其他线程有机会执行。它可以用来提高多线程程序的效率,避免某个线程长时间占用CPU资源。
  • std::this_thread::sleep_for()函数的作用是让当前线程暂停执行一段时间。它接受一个时间间隔作为参数,参数类型为std::chrono::milliseconds,表示以毫秒为单位的时间间隔。在代码中,THREAD_SLEEP_TIM是一个时间间隔的变量,它的值可以根据需要进行设置,代码中设置为1s。
while ( g_camera_lidar_queue.if_lidar_can_process() == false )
{   // 考虑camera和lidar在频率上的时间对齐关系
    // 判断当前时间是否可以处理lidar数据, 不可以则sleep一会
    ros::spinOnce();
    std::this_thread::yield();
    std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
}

从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据,存放至Measures中,如果没有读取到数据则退出。

/**
* @note (2-2) 从lidar_buffer和imu_buffer_lio中分别获取雷达和imu数据,存放至Measures中
*              MeasureGroup Measures; // 存放雷达数据和IMU数据的结构体全局变量
*/             
if ( sync_packages( Measures ) == 0 )
{
    continue;   // 提取数据失败
}

然后下一步就是调用Process 函数补偿点云畸变,并用imu数据进行系统状态预测。

  • 如果没有成功去除点云运动畸变,则重置当前处理lidar帧的起始时间。
  • 接着进行时间判断, 当前帧中lidar的开始时间,必须<=记录的帧开始时间,如果时间满足关系,开始EKF过程
/**
* @note (2-3) 通过测量数据Measures(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
*             前向传播 + 后向传播
*      LIO状态结果保存在g_lio_state,去运动畸变后的点保存在feats_undistort中
*/
p_imu->Process( Measures, g_lio_state, feats_undistort );
g_camera_lidar_queue.g_noise_cov_acc = p_imu->cov_acc;  // 获取加速度误差状态传递的协方差
g_camera_lidar_queue.g_noise_cov_gyro = p_imu->cov_gyr; // 获取角速度误差状态传递的协方差
StatesGroup state_propagate( g_lio_state ); // 状态传播值(先验):通过计算得到的状态实例化一个StatesGroup变量

// 输出lio上一帧更新的时间 : 上一帧更新记录时间 - lidar开始时间
// cout << "G_lio_state.last_update_time =  " << std::setprecision(10) << g_lio_state.last_update_time -g_lidar_star_tim  << endl;
if ( feats_undistort->empty() || ( feats_undistort == NULL ) )  // 没有成功去除点云运动畸变
{
    // 重置当前处理lidar帧的起始时间 : 比如当前处理t1-t2间的lidar但失败了
    // 因此后面处理时间为t1-t3,所以需要把t1保存进frame_first_pt_time
    frame_first_pt_time = Measures.lidar_beg_time;
    std::cout << "not ready for odometry" << std::endl;
    continue;
}

// 当前帧中lidar的开始时间,必须<=记录的帧开始时间
// 按理应该相等,这里的判断估计是实际使用中发生的问题(从理解的角度-不一定正确:如果lidar中损失了初始点,那么
// 当前最开始的lidar点的时间就<记录下来的帧开始时间)
if ( ( Measures.lidar_beg_time - frame_first_pt_time ) < INIT_TIME ) // INIT_TIME=0
{
    flg_EKF_inited = false;
    std::cout << "||||||||||Initiallizing LiDAR||||||||||" << std::endl;
}
else    // 时间满足关系,开始EKF过程
{
    flg_EKF_inited = true;
}

p_imu->Process() 函数的主要作用是通过测量数据meas(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据。

/**
 * @note 通过测量数据meas(IMU数据和Lidar数据)计算LIO的状态和去畸变后的Lidar点云数据
 * @param meas : 输入的Lidar和IMU数据
 * @param stat : Lidar状态
 * @param cur_pcl_un_ : 去畸变后的点云
 */ 
void ImuProcess::Process( const MeasureGroup &meas, StatesGroup &stat, PointCloudXYZINormal::Ptr cur_pcl_un_ )
{
    // double t1, t2, t3;
    // t1 = omp_get_wtime();

    if ( meas.imu.empty() ) // 判断IMU数据
    {
        // std::cout << "no imu data" << std::endl;
        return;
    };
    ROS_ASSERT( meas.lidar != nullptr );    // 判断Lidar数据

    if ( imu_need_init_ )   // 初始化IMU数据
    {
        /// The very first lidar frame 初始化第一帧雷达数据帧
        // 初始化重力,陀螺仪bias,加速度和陀螺仪协方差
        // 归一化加速度测量值到单位重力下
        IMU_Initial( meas, stat, init_iter_num );

        imu_need_init_ = true;

        last_imu_ = meas.imu.back();    // 获取当前处理的最后一个imu数据

        if ( init_iter_num > MAX_INI_COUNT ) // (1 > 20) : 默认不进入if
        {
            imu_need_init_ = false;
            // std::cout<<"mean acc: "<

然后就是完成localmap边界更新,便于完成降采样当前帧点云。

lasermap_fov_segment();
// 降采样
downSizeFilterSurf.setInputCloud( feats_undistort );
downSizeFilterSurf.filter( *feats_down );

然后就是使用下采样后得到的特征点云构造ikd树。

  • ikdtree.set_downsample_param(filter_size_map_min)是设置IKD树的下采样参数。filter_size_map_min用于指定下采样的大小。
  • ikdtree.Build(feats_down->points)是构建IKD树的操作。feats_down->points表示输入的特征点。
/**
* @note (2-6)initialize the map kdtree 使用下采样后得到的特征点云构造ikd树**
* @note 判断条件 : if(下采样后特征点数量大于1) && (ikd树根节点为空) : 
* 重点:判断条件表明这里只进来一次,且第一帧lidar数据用来构造了ikd树后,就进入下一次循环了
*       所以可知,第一帧Lidar数据直接用来构造ikd树,第>=2帧Lidar数据不再直接添加到树上,而
*       是找与树上最近点,由最近点构成平面,然后计算点到平面的残差,并运用EKF迭代更新后,才加
*       到树上
*/
if ( ( feats_down->points.size() > 1 ) && ( ikdtree.Root_Node == nullptr ) )
{
    // std::vector points_init = feats_down->points;
    ikdtree.set_downsample_param( filter_size_map_min ); // filter_size_map_min默认=0.4
    ikdtree.Build( feats_down->points );    // 构造idk树
    flg_map_initialized = true;
    continue;   // 进入下一次循环
}

if ( ikdtree.Root_Node == nullptr ) // 构造ikd树失败
{
    flg_map_initialized = false;
    std::cout << "~~~~~~~ Initialize Map iKD-Tree Failed! ~~~~~~~" << std::endl;
    continue;
}
int featsFromMapNum = ikdtree.size();   // ikd树的节点数
int feats_down_size = feats_down->points.size();    // 下采样过滤后的点数

在拿到ikd-tree构建的树后就是使用激光信息进行ESIKF状态更新。

  • 在ros上发布ikd特征点云数据,默认不发布
  • 遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点), 由最近点集通过PCA方法拟合最近平面,再计算点-面残差,残差定义为点到平面的距离。
  • 从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)
  • 计算测量Jacobian矩阵和测量向量.
  • Iterative Kalman Filter Update
  • 收敛判断和协方差更新
  • 更新维持的固定大小的map立方体
  • 将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树
  • ICP迭代+Kalman更新完成
if ( featsFromMapNum >= 5 ) // ***重点*** : 正式开始ICP和迭代Kalman : ikd树上至少有5个点才进行操作
            {
                t1 = omp_get_wtime();

                /**
                 * @note (2-7-1) : 在ros上发布特征点云数据 - 默认不发布
                 */ 
                if ( m_if_publish_feature_map ) 
                {
                    PointVector().swap( ikdtree.PCL_Storage );
                    // flatten会将需要删除的点放入Points_deleted或Multithread_Points_deleted中
                    ikdtree.flatten( ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD );
                    featsFromMap->clear();
                    featsFromMap->points = ikdtree.PCL_Storage;

                    sensor_msgs::PointCloud2 laserCloudMap;
                    pcl::toROSMsg( *featsFromMap, laserCloudMap );  // 将点云数据格式转换为发布的消息格式
                    laserCloudMap.header.stamp = ros::Time::now(); // ros::Time().fromSec(last_timestamp_lidar);
                    // laserCloudMap.header.stamp.fromSec(Measures.lidar_end_time); // ros::Time().fromSec(last_timestamp_lidar);
                    laserCloudMap.header.frame_id = "world";
                    pubLaserCloudMap.publish( laserCloudMap );
                }

                /**
                 * @note (2-7-2) : 定义后面点-面计算时需要用到的变量
                 * 变量的理解举例: E为所有特征点,P点属于E,现在为P点找平面S:
                 *      当成功为P点找到平面S,则point_selected_surf[P] = true, 否则=false
                 *      平面S由m个点构成:pointSearchInd_surf[0]到pointSearchInd_surf[m]
                 *      E中离P点按距离排序后的点放在Nearest_Points[]中:
                 *        Nearest_Points是二维数组(PointVector是一维),存储每个点的最近点集合
                 */                 
                std::vector< bool >               point_selected_surf( feats_down_size, true ); // 记录有那些点成功找到了平面
                std::vector< std::vector< int > > pointSearchInd_surf( feats_down_size );       // 构成平面的点的index
                std::vector< PointVector >        Nearest_Points( feats_down_size );    // 二维数组,存点i的最近点排序后的集合

                int  rematch_num = 0;
                bool rematch_en = 0;
                flg_EKF_converged = 0;
                deltaR = 0.0;
                deltaT = 0.0;
                t2 = omp_get_wtime();
                double maximum_pt_range = 0.0;
                // cout <<"Preprocess 2 cost time: " << tim.toc("Preprocess") << endl;

                /**
                 * @note (2-7-3) 进行误差Kalman迭代
                 *      //TODO
                 */ 
                for ( iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++ ) // NUM_MAX_ITERATIONS默认为4
                {
                    tim.tic( "Iter" );  // 本次迭代起始时间
                    match_start = omp_get_wtime();
                    laserCloudOri->clear(); // 清空存放找到了最近平面的点的容器
                    coeffSel->clear();      // 清空存放最近平面信息的容器

                    /**
                     * @note (2-7-3-1) : closest surface search and residual computation *
                     *      遍历所有(下采样后)特征点,搜索每个点在树上的最近点集(5个点),
                     *      由最近点集通过PCA方法拟合最近平面,再计算点-面残差
                     */
                    for ( int i = 0; i < feats_down_size; i += m_lio_update_point_step ) // m_lio_update_point_step默认为1
                    {
                        double     search_start = omp_get_wtime();
                        PointType &pointOri_tmpt = feats_down->points[ i ]; // 获取当前下标的特征点 - 原始点
                        // 计算特征点与原点的距离
                        double     ori_pt_dis =
                            sqrt( pointOri_tmpt.x * pointOri_tmpt.x + pointOri_tmpt.y * pointOri_tmpt.y + pointOri_tmpt.z * pointOri_tmpt.z );
                        maximum_pt_range = std::max( ori_pt_dis, maximum_pt_range );// 保存离原点最远的点产生的最远距离
                        PointType &pointSel_tmpt = feats_down_updated->points[ i ]; // 获取当前下标的特征点 - 更新后的点

                        /* transform to world frame */
                        pointBodyToWorld( &pointOri_tmpt, &pointSel_tmpt );// 将特征点转到世界坐标下,并保存至可变点pointSel_tmpt中
                        std::vector< float > pointSearchSqDis_surf; // 搜索点-平面时产生的距离序列

                        auto &points_near = Nearest_Points[ i ];    // 下标i的特征点的最近点集合(一维数组)

                        /**
                         * @note (2-7-3-1-1) : 搜索与点最近平面上的5个点
                         * ***注意*** 只有第一次迭代时才进入,后面都用迭代的方法,就不找平面了
                         *  (猜测应该是寻找平面耗时,后面直接用第一次找到的平面迭代优化就好了)
                         */
                        if ( iterCount == 0 || rematch_en ) // 第一次迭代或者重匹配使能时才在ikd树上搜索最近平面
                        {
                            point_selected_surf[ i ] = true;
                            /** Find the closest surfaces in the map 在地图中找到最近的平面**/
                            // NUM_MATCH_POINTS=5
                            ikdtree.Nearest_Search( pointSel_tmpt, NUM_MATCH_POINTS, points_near, pointSearchSqDis_surf );
                            float max_distance = pointSearchSqDis_surf[ NUM_MATCH_POINTS - 1 ];//最近点集的最后一个元素自然最远
                            //  max_distance to add residuals
                            // ANCHOR - Long range pt stragetry
                            if ( max_distance > m_maximum_pt_kdtree_dis )   // 超出限定距离,放弃为这个点寻找平面
                            {
                                point_selected_surf[ i ] = false;   // 当前点寻找平面失败
                            }
                        }

                        kdtree_search_time += omp_get_wtime() - search_start;
                        if ( point_selected_surf[ i ] == false )    // 当前点寻找平面失败,进入下一个点的寻找流程
                            continue;

                        // match_time += omp_get_wtime() - match_start;
                        double pca_start = omp_get_wtime();
                        /**
                         * @note (2-7-3-2) : 使用PCA方法拟合平面 (using minimum square method)
                         * ***注意*** : 具体PCA解释待补充
                         */
                        cv::Mat matA0( NUM_MATCH_POINTS, 3, CV_32F, cv::Scalar::all( 0 ) );
                        cv::Mat matB0( NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all( -1 ) );
                        cv::Mat matX0( NUM_MATCH_POINTS, 1, CV_32F, cv::Scalar::all( 0 ) );

                        for ( int j = 0; j < NUM_MATCH_POINTS; j++ )
                        {
                            matA0.at< float >( j, 0 ) = points_near[ j ].x;
                            matA0.at< float >( j, 1 ) = points_near[ j ].y;
                            matA0.at< float >( j, 2 ) = points_near[ j ].z;
                        }

                        cv::solve( matA0, matB0, matX0, cv::DECOMP_QR ); // TODO

                        float pa = matX0.at< float >( 0, 0 );
                        float pb = matX0.at< float >( 1, 0 );
                        float pc = matX0.at< float >( 2, 0 );
                        float pd = 1;

                        float ps = sqrt( pa * pa + pb * pb + pc * pc );
                        pa /= ps;
                        pb /= ps;
                        pc /= ps;
                        pd /= ps;

                        /**
                         * @note (2-7-3-1-3) : 检测计算平面的有效性 - 将最近点集合带入平面方程中求残差做判断
                         */
                        bool planeValid = true;
                        for ( int j = 0; j < NUM_MATCH_POINTS; j++ )    // NUM_MATCH_POINTS=5
                        {
                            // ANCHOR -  Planar check : 将特征点在平面上的最近5个点带入平面公式中,得到的结果>0.05 (表明当前点不在拟合平面上)
                            if ( fabs( pa * points_near[ j ].x + pb * points_near[ j ].y + pc * points_near[ j ].z + pd ) >
                                 m_planar_check_dis ) // Raw 0.05
                            {
                                // ANCHOR - Far distance pt processing
                                // ori_pt_dis:当前点到原点的距离, maximum_pt_range:特征点云中的最远点离原点的距离
                                // m_long_rang_pt_dis:lidar点最远有效距离(默认500)
                                if ( ori_pt_dis < maximum_pt_range * 0.90 || ( ori_pt_dis < m_long_rang_pt_dis ) )
                                // if(1)
                                {
                                    planeValid = false;
                                    point_selected_surf[ i ] = false;   // 当前特征点寻找平面失败
                                    break;
                                }
                            }
                        }

                        /**
                         * @note (2-7-3-1-4) : 前面计算平面有效时,计算残差res_last[] : 残差定义为点到平面的距离
                         *                     对应FAST-LIO公式(12)
                         */ 
                        if ( planeValid )
                        {
                            // 将特征点(世界坐标系下) 代入平面方程,计算残差 (在平面的点代入=0,不在平面的点代入会产生残差)
                            // 按照点到平面的距离公式,这里省略了除以分母的内容 : 对应FAST-LIO公式(12)
                            float pd2 = pa * pointSel_tmpt.x + pb * pointSel_tmpt.y + pc * pointSel_tmpt.z + pd;
                            // s计算了后面没用 : s考虑了除以分母的步骤.
                            float s = 1 - 0.9 * fabs( pd2 ) /
                                              sqrt( sqrt( pointSel_tmpt.x * pointSel_tmpt.x + pointSel_tmpt.y * pointSel_tmpt.y +
                                                          pointSel_tmpt.z * pointSel_tmpt.z ) );
                            // ANCHOR -  Point to plane distance
                                                    // 点到原点的距离<500 ? 0.3 : 1.0
                            double acc_distance = ( ori_pt_dis < m_long_rang_pt_dis ) ? m_maximum_res_dis : 1.0;
                            if ( pd2 < acc_distance )   // 残差小于0.3或者1.0
                            {
                                {// if(std::abs(pd2) > 5 * res_mean_last)
                                // {
                                //     point_selected_surf[i] = false;
                                //     res_last[i] = 0.0;
                                //     continue;
                                // }
                                }
                                point_selected_surf[ i ] = true;    // 当前点寻找平面成功
                                coeffSel_tmpt->points[ i ].x = pa;  // 记录当前特征点对应最近平面的平面方程
                                coeffSel_tmpt->points[ i ].y = pb;
                                coeffSel_tmpt->points[ i ].z = pc;
                                coeffSel_tmpt->points[ i ].intensity = pd2;
                                res_last[ i ] = std::abs( pd2 );    // 当前特征点代入平面方程产生的残差
                            }
                            else
                            {
                                point_selected_surf[ i ] = false;// 当前点寻找平面失败
                            }
                        }
                        pca_time += omp_get_wtime() - pca_start;
                    }  // 遍历所有特征点寻找平面,计算点-面残差完成

                    tim.tic( "Stack" );
                    double total_residual = 0.0;
                    laserCloudSelNum = 0;

                    /**
                     * @note (2-7-3-2) : 从找到平面的点中筛选满足条件的点,用新的变量保存该点和对应平面(下标对齐)
                     *      后面进行Kalman等计算时,都使用这里得到的"laserCloudOri"和"coeffSel"
                     */ 
                    for ( int i = 0; i < coeffSel_tmpt->points.size(); i++ )    // 遍历找到了对应平面的特征点
                    {
                        // 平面有效 && 点到面的残差小于2    (这里的残差条件按理都满足,因为前面存入的残差值小于1.0)
                        if ( point_selected_surf[ i ] && ( res_last[ i ] <= 2.0 ) )
                        {                                                           // 下面重新放入容器是为了对齐点-面的索引
                            laserCloudOri->push_back( feats_down->points[ i ] );    // 将找到最近平面的点放入laserCloudOri中
                            coeffSel->push_back( coeffSel_tmpt->points[ i ] );      // 将最近平面容器放入coeffSel中
                            total_residual += res_last[ i ];        // 总残差 - 从最小二乘的角度,优化的就是让这个总残差最小
                            laserCloudSelNum++;                     // 找到平面的点的数量
                        }
                    }
                    res_mean_last = total_residual / laserCloudSelNum;  // 均值-期望, 后面没用此变量

                    match_time += omp_get_wtime() - match_start;
                    solve_start = omp_get_wtime();

                    /**
                     * @note (2-7-3-3) Computation of Measuremnt Jacobian matrix H and measurents vector
                     *               计算测量Jacobian矩阵和测量向量. 猜测对应FAST-LIO的公式(14)(15)(16)(17)
                     */  
                    Eigen::MatrixXd Hsub( laserCloudSelNum, 6 );    // Hsub(n x 6)
                    Eigen::VectorXd meas_vec( laserCloudSelNum );   // meas_vec(n x 1)
                    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;  // Lidar和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 ***/
                        // A = 当前点的反对称矩阵 * (lidar帧最后时刻时的旋转)转置 * 最近平面法向量
                        // ?TODO : 这里可能又是一个近似,猜测是因为直接计算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;// row(i)=A[0],A[1],A[2],norm_p.x, norm_p.y, norm_p.z

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

                    Eigen::Vector3d                           rot_add, t_add, v_add, bg_add, ba_add, g_add; //更新量:旋转,平移,速度,偏置等
                    Eigen::Matrix< double, DIM_OF_STATES, 1 > solution; // 最终解 : 29维
                    Eigen::MatrixXd                           K( DIM_OF_STATES, laserCloudSelNum );// kalman增益

                    /**
                     * @note (2-7-3-4) : Iterative Kalman Filter Update 
                     *       对应(有出入)FAST-LIO中公式(18)(19)(20)
                     */
                    if ( !flg_EKF_inited )  // 未初始化时初始化 - 前面已经初始化了
                    {
                        cout << ANSI_COLOR_RED_BOLD << "Run EKF init" << ANSI_COLOR_RESET << endl;
                        /*** only run in initialization period ***/
                        set_initial_state_cov( g_lio_state );
                    } 
                    else{
                        // cout << ANSI_COLOR_RED_BOLD << "Run EKF uph" << ANSI_COLOR_RESET << endl;
                        //1>:求公式中需要用到的 H 和 H^T*T
                        auto &&Hsub_T = Hsub.transpose();   // H转置 : 6xn = (nx6)^T
                        H_T_H.block< 6, 6 >( 0, 0 ) = Hsub_T * Hsub;//(0,0)处6x6块.H^T*T
                        //2>:求公式(20)中的Kalman增益的前面部分(省略R) : (H^T * R^-1 * H + P^-1)^-1
                        Eigen::Matrix< double, DIM_OF_STATES, DIM_OF_STATES > &&K_1 =
                            ( H_T_H + ( g_lio_state.cov / LASER_POINT_COV ).inverse() ).inverse();
                        //3>:结合2>求的前面部分,求公式(20)Kalman增益(省略R)
                        K = K_1.block< DIM_OF_STATES, 6 >( 0, 0 ) * Hsub_T;// K = (29x6) * (6xn) = (29xn)

                        //4>:求公式(18)中的最右边部分 : x^kk-x^k : Kalman迭代时的传播状态-预估(也是更新)状态
                        auto vec = state_propagate - g_lio_state;//state_propagate初始=g_lio_state
                        //5>:求公式(18)的中间和右边部分(有出入:I什么的都省略了)
                        solution = K * ( meas_vec - Hsub * vec.block< 6, 1 >( 0, 0 ) ); // kalman增益
                        // double speed_delta = solution.block( 0, 6, 3, 1 ).norm();
                        // if(solution.block( 0, 6, 3, 1 ).norm() > 0.05 )
                        // {
                        //     solution.block( 0, 6, 3, 1 ) = solution.block( 0, 6, 3, 1 ) / speed_delta * 0.05;
                        // }
                        //6>:结合5>中结果,求公式18计算结果,得到k+1次kalman的迭代更新值
                        g_lio_state = state_propagate + solution;   // kalman增益后的状态结果
                        print_dash_board();
                        // cout << ANSI_COLOR_RED_BOLD << "Run EKF uph, vec = " << vec.head<9>().transpose() << ANSI_COLOR_RESET << endl;
                        rot_add = solution.block< 3, 1 >( 0, 0 );   // 旋转增量
                        t_add = solution.block< 3, 1 >( 3, 0 );     // 平移增量
                        flg_EKF_converged = false;                  // 收敛标识
                        //7>:判断是否收敛
                        if ( ( ( rot_add.norm() * 57.3 - deltaR ) < 0.01 ) && ( ( t_add.norm() * 100 - deltaT ) < 0.015 ) )
                        {
                            flg_EKF_converged = true;   // 通过旋转和平移增量与上一次迭代的差值,判断是否收敛
                                                        // ? : 收敛了为啥不加break,而是继续进行迭代
                        }
                        //8>:旋转和平移增量转换单位
                        deltaR = rot_add.norm() * 57.3; // 角度单位
                        deltaT = t_add.norm() * 100;    // 厘米单位
                    }

                    // printf_line;
                    g_lio_state.last_update_time = Measures.lidar_end_time;
                    euler_cur = RotMtoEuler( g_lio_state.rot_end ); // 获得当前lidar的里程计信息,最后这个需要发布到ros中去
                    dump_lio_state_to_log( m_lio_state_fp );

                    /*** Rematch Judgement 重匹配判断 ***/
                    rematch_en = false;
                    if ( flg_EKF_converged || ( ( rematch_num == 0 ) && ( iterCount == ( NUM_MAX_ITERATIONS - 2 ) ) ) )
                    {
                        rematch_en = true;
                        rematch_num++;
                    }

                    /**
                     * @note (2-7-3-5) Convergence Judgements and Covariance Update 
                     *       收敛判断和协方差更新 : 对应FAST-LIO公式(19)
                     */
                    // if (rematch_num >= 10 || (iterCount == NUM_MAX_ITERATIONS - 1))
                    if ( rematch_num >= 2 || ( iterCount == NUM_MAX_ITERATIONS - 1 ) ) // Fast lio ori version.
                    {
                        if ( flg_EKF_inited )
                        {
                            /*** Covariance Update ***/
                            G.block< DIM_OF_STATES, 6 >( 0, 0 ) = K * Hsub; // 对应公式(19)中 : K * H
                            g_lio_state.cov = ( I_STATE - G ) * g_lio_state.cov;//公式(19): (单位阵-K*H)*Cur_协方差
                            total_distance += ( g_lio_state.pos_end - position_last ).norm();// 两次state间的距离
                            position_last = g_lio_state.pos_end;
                            // std::cout << "position: " << g_lio_state.pos_end.transpose() << " total distance: " << total_distance << std::endl;
                        }
                        solve_time += omp_get_wtime() - solve_start;
                        break;
                    }
                    solve_time += omp_get_wtime() - solve_start;
                    // cout << "Match cost time: " << match_time * 1000.0
                    //      << ", search cost time: " << kdtree_search_time*1000.0
                    //      << ", PCA cost time: " << pca_time*1000.0
                    //      << ", solver_cost: " << solve_time * 1000.0 << endl;
                    // cout <<"Iter cost time: " << tim.toc("Iter") << endl;
                } // (2-7-3) : Kalman滤波迭代完成

                t3 = omp_get_wtime();

                /**
                 * @note (2-7-4):add new frame points to map ikdtree 
                 *   1> : 更新维持的固定大小的map立方体 (参考FAST-LIO2:V.A地图管理)
                 *   2> : 将Kalman更新后的新lidar帧特征点先转世界坐标系,再加入ikd树
                 */
                PointVector points_history; // 将ikd树中需要移除的点放入points_history中
                ikdtree.acquire_removed_points( points_history );// 从Points_deleted和Multithread_Points_deleted获取点
                memset( cube_updated, 0, sizeof( cube_updated ) );
                //1> : 更新维持的固定大小的map立方体 (参考FAST-LIO2:V.A地图管理)
                for ( int i = 0; i < points_history.size(); i++ )
                {
                    PointType &pointSel = points_history[ i ];

                    int cubeI = int( ( pointSel.x + 0.5 * cube_len ) / cube_len ) + laserCloudCenWidth;
                    int cubeJ = int( ( pointSel.y + 0.5 * cube_len ) / cube_len ) + laserCloudCenHeight;
                    int cubeK = int( ( pointSel.z + 0.5 * cube_len ) / cube_len ) + laserCloudCenDepth;

                    if ( pointSel.x + 0.5 * cube_len < 0 )
                        cubeI--;
                    if ( pointSel.y + 0.5 * cube_len < 0 )
                        cubeJ--;
                    if ( pointSel.z + 0.5 * cube_len < 0 )
                        cubeK--;

                    if ( cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 && cubeK < laserCloudDepth )
                    {
                        int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
                        featsArray[ cubeInd ]->push_back( pointSel );
                    }
                }

                //2-1> : 将Kalman更新后的新lidar帧特征点先转世界坐标系
                for ( int i = 0; i < feats_down_size; i++ )
                {
                    /* transform to world frame */
                    pointBodyToWorld( &( feats_down->points[ i ] ), &( feats_down_updated->points[ i ] ) );
                }
                t4 = omp_get_wtime();
                //2-2> : 将特征点加入世界坐标中
                ikdtree.Add_Points( feats_down_updated->points, true ); // 存入ikd树中
                
                kdtree_incremental_time = omp_get_wtime() - t4 + readd_time + readd_box_time + delete_box_time;
                t5 = omp_get_wtime();
            }   // (2-7) ICP迭代+Kalman更新完成

最后就是向rgb点云地图中加入新点,并发布laserCloudFullResColor、Maps、Odometry、Path等信息。

/**
             * @note (2-9) : append point cloud to global map.
             *               将点云添加至世界地图中 
             */ 
            if ( 1 )
            {
                static std::vector< double > stastic_cost_time;
                Common_tools::Timer          tim;
                // tim.tic();
                // ANCHOR - RGB maps update
                wait_render_thread_finish();
                if ( m_if_record_mvs )
                {
                    std::vector< std::shared_ptr< RGB_pts > > pts_last_hitted;
                    pts_last_hitted.reserve( 1e6 );
                    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 );
                    m_map_rgb_pts.m_mutex_pts_last_visited->lock();
                    m_map_rgb_pts.m_pts_last_hitted = pts_last_hitted;
                    m_map_rgb_pts.m_mutex_pts_last_visited->unlock();
                }
                else
                {
                    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, nullptr,
                        m_append_global_map_point_step );
                }
                stastic_cost_time.push_back( tim.toc( " ", 0 ) );
            }
            /**
             * @note (2-10) : 仅发布找到了最近平面的有效点
             */ 
            if(0) // Uncomment this code scope to enable the publish of effective points. 
            {
                /******* Publish effective points *******/
                laserCloudFullResColor->clear();
                pcl::PointXYZI temp_point;
                for ( int i = 0; i < laserCloudSelNum; i++ )
                {
                    RGBpointBodyToWorld( &laserCloudOri->points[ i ], &temp_point );
                    laserCloudFullResColor->push_back( temp_point );
                }
                sensor_msgs::PointCloud2 laserCloudFullRes3;
                pcl::toROSMsg( *laserCloudFullResColor, laserCloudFullRes3 );
                // laserCloudFullRes3.header.stamp = ros::Time::now(); //.fromSec(last_timestamp_lidar);
                laserCloudFullRes3.header.stamp.fromSec( Measures.lidar_end_time ); //.fromSec(last_timestamp_lidar);
                laserCloudFullRes3.header.frame_id = "world";
                pubLaserCloudEffect.publish( laserCloudFullRes3 );
            }

            /**
             * @note (2-11)***** Publish Maps:  发布地图******
             */
            sensor_msgs::PointCloud2 laserCloudMap;
            pcl::toROSMsg( *featsFromMap, laserCloudMap );
            laserCloudMap.header.stamp.fromSec( Measures.lidar_end_time ); // ros::Time().fromSec(last_timestamp_lidar);
            laserCloudMap.header.frame_id = "world";
            pubLaserCloudMap.publish( laserCloudMap );

            /**
             * @note (2-12)***** Publish Odometry 发布里程计*****
             */
            geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw( euler_cur( 0 ), euler_cur( 1 ), euler_cur( 2 ) );
            odomAftMapped.header.frame_id = "world";
            odomAftMapped.child_frame_id = "/aft_mapped";
            odomAftMapped.header.stamp = ros::Time::now(); // ros::Time().fromSec(last_timestamp_lidar);
            odomAftMapped.pose.pose.orientation.x = geoQuat.x;
            odomAftMapped.pose.pose.orientation.y = geoQuat.y;
            odomAftMapped.pose.pose.orientation.z = geoQuat.z;
            odomAftMapped.pose.pose.orientation.w = geoQuat.w;
            odomAftMapped.pose.pose.position.x = g_lio_state.pos_end( 0 );
            odomAftMapped.pose.pose.position.y = g_lio_state.pos_end( 1 );
            odomAftMapped.pose.pose.position.z = g_lio_state.pos_end( 2 );
            pubOdomAftMapped.publish( odomAftMapped );

            static tf::TransformBroadcaster br;
            tf::Transform                   transform;
            tf::Quaternion                  q;
            transform.setOrigin(
                tf::Vector3( odomAftMapped.pose.pose.position.x, odomAftMapped.pose.pose.position.y, odomAftMapped.pose.pose.position.z ) );
            q.setW( odomAftMapped.pose.pose.orientation.w );
            q.setX( odomAftMapped.pose.pose.orientation.x );
            q.setY( odomAftMapped.pose.pose.orientation.y );
            q.setZ( odomAftMapped.pose.pose.orientation.z );
            transform.setRotation( q );
            br.sendTransform( tf::StampedTransform( transform, ros::Time().fromSec( Measures.lidar_end_time ), "world", "/aft_mapped" ) );

            msg_body_pose.header.stamp = ros::Time::now();
            msg_body_pose.header.frame_id = "/camera_odom_frame";
            msg_body_pose.pose.position.x = g_lio_state.pos_end( 0 );
            msg_body_pose.pose.position.y = g_lio_state.pos_end( 1 );
            msg_body_pose.pose.position.z = g_lio_state.pos_end( 2 );
            msg_body_pose.pose.orientation.x = geoQuat.x;
            msg_body_pose.pose.orientation.y = geoQuat.y;
            msg_body_pose.pose.orientation.z = geoQuat.z;
            msg_body_pose.pose.orientation.w = geoQuat.w;

            /**
             * @note (2-13)***** Publish Path 发布路径*******
             */
            msg_body_pose.header.frame_id = "world";
            if ( frame_num > 10 )
            {
                path.poses.push_back( msg_body_pose );
            }
            pubPath.publish( path );
            
            /**
             * @note (2-14)* save debug variables 保存debug变量**
             */
            frame_num++;
            aver_time_consu = aver_time_consu * ( frame_num - 1 ) / frame_num + ( t5 - t0 ) / frame_num;
            // aver_time_consu = aver_time_consu * 0.8 + (t5 - t0) * 0.2;
            T1[ time_log_counter ] = Measures.lidar_beg_time;
            s_plot[ time_log_counter ] = aver_time_consu;
            s_plot2[ time_log_counter ] = kdtree_incremental_time;
            s_plot3[ time_log_counter ] = kdtree_search_time;
            s_plot4[ time_log_counter ] = fov_check_time;
            s_plot5[ time_log_counter ] = t5 - t0;
            s_plot6[ time_log_counter ] = readd_box_time;
            time_log_counter++;
            fprintf( m_lio_costtime_fp, "%.5f %.5f\r\n", g_lio_state.last_update_time - g_camera_lidar_queue.m_first_imu_time, t5 - t0 );
            fflush( m_lio_costtime_fp );

你可能感兴趣的:(R3LIVE项目实战,R3LIVE,源码解析)