FAST-LIO2源码学习_fastlio2源码解读

学习计划

因为fast-lio2的框架将各个线程都放到了一个节点中，因此也就只有一个主函数cpp文件，从工程结构来看更加简约，代码风格更加集中化。

主函数结构

主要梳理清楚各个模块在主函数中的分布，实现的功能以及之间的联系。

各模块函数具体实现

涉及各个模块中数学原理和代码的实现，较为复杂。

尚未理解的地方

主要涉及编程语言的使用技巧、数学原理的实现等没有理解的内容。

可以借鉴的优点

工程上对传感器信息进行处理的操作；
框架布局的构思；
项目文件的分布等。

关于这篇文章的阅读，我通过对比框架发现fast-lio2与fast-lio的里程计部分是一样的，不一样的地方就在于IKD-TREE的增量式建图。所以公式的推导我都放到了博客fast-lio 滤波学习，以及 迭代扩展卡尔曼滤波学习中。

主函数结构

结构图

IKDTREE结构理解（结合论文理解起来更方便）

惰性标签是我从论文中的Lazy label翻译过来的，能力有限，不知道更精确的译法是什么样的，就按自己理解的来了。意思是，将点或者树节点赋予deleted/treedeleted后并不是立即删除该点或树节点，而是等到re-build的时候再执行，在re-build之前这些标签可能会取消掉，就是有点拖延症的感觉。

各个结构所需参数获取

主要是从yaml及launch文件中读取各个结构需要的固定参数

nh.param<bool>("publish/path_en",path_en, true);                                // 是否发布路径的topic
    nh.param<bool>("publish/scan_publish_en",scan_pub_en, true);                    // 是否发布当前正在扫描的点云的topic
    nh.param<bool>("publish/dense_publish_en",dense_pub_en, true);                  // 是否在全局帧点云扫描中降低点数
    nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);         // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic
    nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);                            // 卡尔曼滤波的最大迭代次数
    nh.param<string>("map_file_path",map_file_path,"");                             // 地图保存路径
    nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");                  // 激光雷达点云topic名称
    nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");                   // IMU的topic名称
    nh.param<bool>("common/time_sync_en", time_sync_en, false);                     // 是否需要时间同步，只有当外部未进行时间同步时设为true
    nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);              // VoxelGrid降采样时的体素大小//角点
    nh.param<double>("filter_size_surf",filter_size_surf_min,0.5);                  // VoxelGrid降采样时的体素大小//平面点
    nh.param<double>("filter_size_map",filter_size_map_min,0.5);                    // VoxelGrid降采样时的体素大小//地图
    nh.param<double>("cube_side_length",cube_len,200);                              // 地图的局部区域的长度（FastLio2论文中有解释）
    nh.param<float>("mapping/det_range",DET_RANGE,300.f);                           // 激光雷达的最大探测范围
    nh.param<double>("mapping/fov_degree",fov_deg,180);                             // 激光雷达的视场角
    nh.param<double>("mapping/gyr_cov",gyr_cov,0.1);                                // IMU陀螺仪的协方差
    nh.param<double>("mapping/acc_cov",acc_cov,0.1);                                // IMU加速度计的协方差
    nh.param<double>("mapping/b_gyr_cov",b_gyr_cov,0.0001);                         // IMU陀螺仪偏置的协方差
    nh.param<double>("mapping/b_acc_cov",b_acc_cov,0.0001);                         // IMU加速度计偏置的协方差
    nh.param<double>("preprocess/blind", p_pre->blind, 0.01);                       // 最小距离阈值，即过滤掉0～blind范围内的点云
    nh.param<int>("preprocess/lidar_type", p_pre->lidar_type, AVIA);                // 激光雷达的类型
    nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);                      // 激光雷达扫描的线数（livox avia为6线）
    nh.param<int>("preprocess/scan_rate", p_pre->SCAN_RATE, 10);                    // 激光雷达的扫描频率（livox avia为10hz）
    nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);                  // 采样间隔，即每隔point_filter_num个点取1个点
    nh.param<bool>("feature_extract_enable", p_pre->feature_enabled, false);        // 是否提取特征点（FAST_LIO2默认不进行特征点提取）
    nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);                   // 是否输出调试log信息
    nh.param<bool>("mapping/extrinsic_est_en", extrinsic_est_en, true);             // 是否进行在线的外参估计
    nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);                     // 是否将点云地图保存到PCD文件
    nh.param<int>("pcd_save/interval", pcd_save_interval, -1);                      // 每一个PCD文件保存多少个雷达帧（-1表示所有雷达帧都保存在一个PCD文件中）
    nh.param<vector<double>>("mapping/extrinsic_T", extrinT, vector<double>());     // 雷达相对于IMU的外参T（即雷达在IMU坐标系中的坐标）
    nh.param<vector<double>>("mapping/extrinsic_R", extrinR, vector<double>());     // 雷达相对于IMU的外参R
    cout<<"p_pre->lidar_type "<<p_pre->lidar_type<<endl;
    
    // nav_msgs::Path 消息类型，初始化path的header（包括时间戳和帧id），path：用于保存odemetry的路径
    path.header.stamp    = ros::Time::now();
    path.header.frame_id ="camera_init";
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

日志所需变量的定义

    int effect_feat_num = 0, frame_num = 0;
    double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0;
    bool flg_EKF_converged, EKF_stop_flg = 0;
    
    FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0);//不太明白这里这样处理的意义（虽然没有用到）
    HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0);

    _featsArray.reset(new PointCloudXYZI());
1
2
3
4
5
6
7
8

/** 变量定义
 * effect_feat_num          有效的特征点数量（未用到该变量）
 * frame_num                雷达总帧数
 * deltaT                 平移增量  （未用到该变量）
 * deltaR                   旋转增量（未用到该变量）
 * aver_time_consu          每帧平均的处理总时间
 * aver_time_icp            每帧中icp的平均时间
 * aver_time_match          每帧中匹配的平均时间
 * aver_time_incre          每帧中ikd-tree增量处理的平均时间
 * aver_time_solve          每帧中计算的平均时间
 * aver_time_const_H_time   每帧中计算的平均时间（当H恒定时）
 * flg_EKF_converged        扩展卡尔曼滤波收敛标志（未用到该变量）
 * EKF_stop_flg             扩展卡尔曼滤波停止标志（未用到该变量）
 * FOV_DEG                  视场角度（未用到该变量）
 * HALF_FOV_COS             视场角度半值的cos值（未用到该变量）
 * _featsArray              特征点数组（未用到该变量）
 */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

对容器或函数设置上面获取的参数

bool型数组point_selected_surf表示点云中某个位置处的点是不是挑选出来的平面点
该部分代码中数组初始化的操作似乎存在重复//尚未验证过注释后的影响

    // 将从点云中挑选的平面点数组point_selected_surf内元素的值全部设为true，数组point_selected_surf用于平面特征点
    memset(point_selected_surf, true, sizeof(point_selected_surf));
    // 将数组res_last内元素的值全部设置为-1000.0f，数组res_last用于点与平面间的残差距离
    memset(res_last, -1000.0f, sizeof(res_last));

    // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_surf_min
    downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min);
    // VoxelGrid滤波器参数，即进行滤波时的创建的体素边长为filter_size_map_min
    downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min);

    // 感觉重复了
    memset(point_selected_surf, true, sizeof(point_selected_surf));
    memset(res_last, -1000.0f, sizeof(res_last));

    // 输出雷达相对于IMU的外参R和T
    Lidar_T_wrt_IMU<<VEC_FROM_ARRAY(extrinT);
    Lidar_R_wrt_IMU<<MAT_FROM_ARRAY(extrinR);

    // 设置IMU的参数，对p_imu进行初始化，其中p_imu为ImuProcess的智能指针（ImuProcess是进行IMU处理的类）
    p_imu->set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU);
    p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov));
    p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov));
    p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov));
    p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov));

    // TODO
    double epsi[23] = {0.001};//收敛阈值
    fill(epsi, epsi+23, 0.001);//epsi初始化重复？
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

扩展卡尔曼滤波器的初始化

不明白get_f、df_dx、df_dw、h_share_model这些函数的参数是如何传递的

 kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
1

调试记录的日志文件开启

只是打开相应文件，是否输出可以在后面的代码中进行设置

    FILE *fp;
    string pos_log_dir = root_dir + "/Log/pos_log.txt";
    fp = fopen(pos_log_dir.c_str(),"w");
    ofstream fout_pre, fout_out, fout_dbg;
    fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out);
    fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out);
    fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out);
    if (fout_pre && fout_out)
        cout << "~~~~"<<ROOT_DIR<<" file opened" << endl;
    else
        cout << "~~~~"<<ROOT_DIR<<" doesn't exist" << endl;
1
2
3
4
5
6
7
8
9
10
11

订阅&发布话题

订阅激光雷达、IMU驱动或数据包发布的话题，并将数据缓存起来。
发布当前点云以及经过处理后得到的结果。

    /*** ROS subscribe initialization ***/
    // ROS订阅器和发布器的定义和初始化
     // 雷达点云的订阅器sub_pcl，订阅点云的topic//放入缓存器中
    ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
        nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
        nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
    // IMU的订阅器sub_imu，订阅IMU的topic//放入缓存器中
    ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
    // 发布当前正在扫描的点云，topic名字为/cloud_registered
    ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered", 100000);
    // 发布经过运动畸变校正注册到IMU坐标系的点云，topic名字为/cloud_registered_body
    ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_registered_body", 100000);
    // 后面的代码中没有用到
    ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
            ("/cloud_effected", 100000);
    // 后面的代码中没有用到
    ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
            ("/Laser_map", 100000);
    // 发布当前里程计信息，topic名字为/Odometry
    ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
            ("/Odometry", 100000);
    // 发布里程计总的路径，topic名字为/path
    ros::Publisher pubPath          = nh.advertise<nav_msgs::Path> 
            ("/path", 100000);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

数据处理部分

包括激光雷达点云处理，IMU数据处理，卡尔曼滤波，IKD树建图。

主循环如下

    // 中断处理函数，如果有中断信号（比如Ctrl+C），则执行第二个参数里面的SigHandle函数
    signal(SIGINT, SigHandle);

    // 设置ROS程序主循环每次运行的时间至少为0.0002秒（5000Hz）
    ros::Rate rate(5000);
    bool status = ros::ok();

    // 程序主循环
    while (status)
    {
    ......
    }
1
2
3
4
5
6
7
8
9
10
11
12

以下为主循环中的各个部分
/**************************************************************************************************************/

###激光点云、IMU、EKF的初始化操作

        // 如果有中断产生，则结束主循环
        if (flg_exit) break;

        // ROS消息回调处理函数，放在ROS的主循环中
        ros::spinOnce();

        // 将激光雷达点云数据和IMU数据从缓存队列中取出，进行时间对齐，并保存到Measures中
        if(sync_packages(Measures)) 
        {
            // 激光雷达第一次扫描
            if (flg_first_scan)
            {
                // 记录激光雷达第一次扫描的时间
                first_lidar_time = Measures.lidar_beg_time;
                p_imu->first_lidar_time = first_lidar_time;
                flg_first_scan = false;
                continue;
            }

            double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;//IKD树建图以及相关算法求解所用时间记录//用于后续输出到日志文件中//看来是记录各个模块的处理时间的

            match_time = 0;
            kdtree_search_time = 0.0;
            solve_time = 0;
            solve_const_H_time = 0;
            svd_time   = 0;
            t0 = omp_get_wtime();

            // 对IMU数据进行预处理（初始化），其中包含了点云畸变处理
            p_imu->Process(Measures, kf, feats_undistort);

            state_point = kf.get_x();
            pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;//激光雷达的位姿

            if (feats_undistort->empty() || (feats_undistort == NULL))
            {
                ROS_WARN("No point, skip this scan!\n");
                continue;
            }

            flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \
                            false : true;//EKF初始化完成
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

###对激光雷达视场中的地图进行分割

            /*** Segment the map in lidar FOV ***/
            lasermap_fov_segment();//可以参考论文
1
2

###对一个扫描中的特征点进行降采样操作

            /*** downsample the feature points in a scan ***/
            downSizeFilterSurf.setInputCloud(feats_undistort);
            downSizeFilterSurf.filter(*feats_down_body);
            t1 = omp_get_wtime();
            feats_down_size = feats_down_body->points.size();
1
2
3
4
5

###初始化建图的KD树结构
ikdtree.size()与 ikdtree.validnum()的区别不明白

            /*** initialize the map kdtree ***/
            if(ikdtree.Root_Node == nullptr)//是否还没有根节点
            {
                if(feats_down_size > 5)//降采样后的特征点树是否多于5个
                {
                    ikdtree.set_downsample_param(filter_size_map_min);//设置IKDtree的降采样参数//0.5米的方块
                    feats_down_world->resize(feats_down_size);//世界系下降采样点数空间大小重置
                    for(int i = 0; i < feats_down_size; i++)
                    {
                        pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));//将载体系中降采样特征点转到世界系下
                    }
                    ikdtree.Build(feats_down_world->points);//根据世界系下的降采样特征点构建ikdtree
                }
                continue;
            }
            int featsFromMapNum = ikdtree.validnum();//获取ikdtree中有效点的数目
            kdtree_size_st = ikdtree.size();//获取ikdtree的大小？它的大小和有效点数目的区别是什么呢？
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

###ICP以及迭代EKF的更新并输出相应状态值到文件中
transpose()的作用不明白

            if (feats_down_size < 5)
            {
                ROS_WARN("No point, skip this scan!\n");
                continue;
            }
            
            normvec->resize(feats_down_size);
            feats_down_world->resize(feats_down_size);

            V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);//状态中Imu与雷达的旋转偏差转换为欧拉角
            //计算并输出状态处理结果到日志文件中
            fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \
            <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl;
1
2
3
4
5
6
7
8
9
10
11
12
13

###是否显示ikdtree中的地图点

            if(0) // If you need to see map point, change to "if(1)"
            {
                PointVector ().swap(ikdtree.PCL_Storage);
                ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
                featsFromMap->clear();
                featsFromMap->points = ikdtree.PCL_Storage;
            }
       //对搜索点索引数组以及最近点数组大小进行更新
            pointSearchInd_surf.resize(feats_down_size);
            Nearest_Points.resize(feats_down_size);
            int  rematch_num = 0;
            bool nearest_search_en = true; //
                 t2 = omp_get_wtime();
1
2
3
4
5
6
7
8
9
10
11
12
13

###开始迭代状态估计

            /*** iterated state estimation ***/
            double t_update_start = omp_get_wtime();
            double solve_H_time = 0;
            kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);//卡尔曼迭代函数前者针对一个特定系统修改的迭代误差状态EKF更新，后者求解时间
            state_point = kf.get_x();//更新后的状态
            euler_cur = SO3ToEuler(state_point.rot);
            pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;//激光雷达位姿
            geoQuat.x = state_point.rot.coeffs()[0];
            geoQuat.y = state_point.rot.coeffs()[1];
            geoQuat.z = state_point.rot.coeffs()[2];
            geoQuat.w = state_point.rot.coeffs()[3];//相应的四元数

            double t_update_end = omp_get_wtime();
1
2
3
4
5
6
7
8
9
10
11
12
13

###发布里程计
pubOdomAftMapped是一个发布器,odomAftMapped是里程计数据，它在哪里更新呢？

            /******* Publish odometry *******/
            publish_odometry(pubOdomAftMapped);
1
2

###将特征点添加到地图kdtree中

            t3 = omp_get_wtime();
            map_incremental();//ikdtree的增量式建图
            t5 = omp_get_wtime();
1
2
3

###发布点云

            /******* Publish points *******/
            if (path_en)                         publish_path(pubPath);//发布路径
            if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFull);//当前扫描点
            if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);//去畸变后载体系的点云
1
2
3
4

###调试变量输出
aver_time_solve是h_share_model函数加上kf.update_iterated_dyn_share_modified所用平均时间时间
aver_time_const_H_time 是h_share_model所用平均时间

            /*** Debug variables ***/
            if (runtime_pos_log)//在《各个结构所需参数获取》部分进行设置
            {
                frame_num ++;//统计帧数
                kdtree_size_end = ikdtree.size();//统计ikdtree的大小
                aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;//每帧点云在框架中计算所用平均时间
                aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num;//每帧点云ICP迭代所用平均时间
                aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num;//每帧点云进行匹配所用平均时间
                aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num;//每帧点云进行ikdtree增量操作所用平均时间
                aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num;//总的求解时间平均值
                aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num;
                T1[time_log_counter] = Measures.lidar_beg_time;
                s_plot[time_log_counter] = t5 - t0;//当前帧所用总时间
                s_plot2[time_log_counter] = feats_undistort->points.size();//去畸变后特征点数量
                s_plot3[time_log_counter] = kdtree_incremental_time;//当前帧Ikdtree增量时间
                s_plot4[time_log_counter] = kdtree_search_time;//当前帧KDtree搜索所用时间
                s_plot5[time_log_counter] = kdtree_delete_counter;//当前帧kdtree删减计数器
                s_plot6[time_log_counter] = kdtree_delete_time;//当前帧kdtree删减所用时间
                s_plot7[time_log_counter] = kdtree_size_st;//当前帧添加特征点到地图之前KDTREE的大小
                s_plot8[time_log_counter] = kdtree_size_end;//当前帧添加特征点到地图之后KDTREE的大小
                s_plot9[time_log_counter] = aver_time_consu;//上面计算出的平均计算时间
                s_plot10[time_log_counter] = add_point_size;//kdtree中插入的点数量
                time_log_counter ++;
                printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f  ave ICP: %0.6f  map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time);
                ext_euler = SO3ToEuler(state_point.offset_R_L_I);
                fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<<" "<< state_point.vel.transpose() \
                <<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<<" "<<feats_undistort->points.size()<<endl;
                dump_lio_state_to_log(fp);
            }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

至此主循环中数据处理流程就结束了

/*******************************************************************************************************/
###地图保存

    /**************** save map ****************/
    /* 1. make sure you have enough memories
    /* 2. pcd save will largely influence the real-time performences **/
    if (pcl_wait_save->size() > 0 && pcd_save_en)
    {
        string file_name = string("scans.pcd");
        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
        pcl::PCDWriter pcd_writer;
        cout << "current scan saved to /PCD/" << file_name<<endl;
        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
    }
1
2
3
4
5
6
7
8
9
10
11

/********************************************************************************************************/

各模块函数解析

扩展卡尔曼滤波器的初始化

 kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);
1

get_f：获得状态转移阵
df_dx，df_dw 分别代表测量值与估计值之差对状态和噪声偏差值的偏导

h_share_model：构造优化函数并求解该函数的雅克比矩阵

通过函数h_share_model（h_dyn_share_in）同时计算残差（z）、估计测量（h）、偏微分矩阵（h_x，h_v）和噪声协方差（R）

NUM_MAX_ITERATIONS：最大的迭代数。
epsi：要满足的迭代误差阈值。

IMU初始化

这部分主要有两个函数实现
IMU_init(meas, kf_state, init_iter_num)
主要是获得IMU中加速度值和陀螺仪角速度值，激光帧起始时间，平均加速度，平均角速度，以及相应的协方差，KF的初始状态，以及变化量，误差状态协方差初始值及变化量，以及最后一个Imu数据
UndistortPcl(meas, kf_state, #cur_pcl_un_)
包括了IMU的前向传播和反向传播，利用IMU的数据将点云数据都投影到当前帧最后时刻。

            // 对IMU数据进行预处理（初始化），其中包含了点云畸变处理
            p_imu->Process(Measures, kf, feats_undistort);
1
2

void ImuProcess::Process(const MeasureGroup &meas,  esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
  double t1,t2,t3;
  t1 = omp_get_wtime();

  if(meas.imu.empty()) {return;};
  ROS_ASSERT(meas.lidar != nullptr);

  if (imu_need_init_)
  {
    /// The very first lidar frame
    IMU_init(meas, kf_state, init_iter_num);**//这里进行IMU的初始化工作**

    imu_need_init_ = true;
    
    last_imu_   = meas.imu.back();

    state_ikfom imu_state = kf_state.get_x();//将KF初始化后得到的状态给IMU状态
    if (init_iter_num > MAX_INI_COUNT)//如果迭代帧数多于20便不需要初始化
    {
      cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
      imu_need_init_ = false;

      cov_acc = cov_acc_scale;
      cov_gyr = cov_gyr_scale;
      ROS_INFO("IMU Initial Done");
      // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
      //          imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
      fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
    }

    return;
  }

  UndistortPcl(meas, kf_state, *cur_pcl_un_);**//点云去畸变**

  t2 = omp_get_wtime();
  t3 = omp_get_wtime();
  
  // cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

IMU初始化

void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
  /** 1.初始化重力，加速度、陀螺仪偏差协方差。
   ** 2. 将加速度归一化到单位重力 **/
  
  V3D cur_acc, cur_gyr;
  
  if (b_first_frame_)//如果是第一帧数据
  {
    Reset();
    N = 1;
    b_first_frame_ = false;
    const auto &imu_acc = meas.imu.front()->linear_acceleration;//这里都是IMU数据//
    const auto &gyr_acc = meas.imu.front()->angular_velocity;
    mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
    mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;//用于后面计算平均加速度和角速度
    first_lidar_time = meas.lidar_beg_time;//获得第一帧激光雷达开始的时间
  }

  for (const auto &imu : meas.imu)//遍历测量信息中存放的imu信息
  {
    const auto &imu_acc = imu->linear_acceleration;
    const auto &gyr_acc = imu->angular_velocity;
    cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
    cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;

    mean_acc      += (cur_acc - mean_acc) / N;
    mean_gyr      += (cur_gyr - mean_gyr) / N;//求已有数据的平均值
    /*简单介绍一下这个求平均//正推倒推均可
  N = 1时，mean = cur1 ，即mean1
  N = 2时 mean2 = (cur2 + cur1)/2 = (cur2 + mean1)/2 =  mean1 + (cur2 - mean1)/2 //这里实现了将代码简约化
 类推
 mean_n = mean_n-1 + (cur_n - mean_n-1)/n
*/

    cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
    cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);//计算加速度和角速度的协方差值//套协方差的公式

    // cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;

    N ++;
  }
  state_ikfom init_state = kf_state.get_x();//KF的初始状态
  init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
  
  //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
  init_state.bg  = mean_gyr;
  init_state.offset_T_L_I = Lidar_T_wrt_IMU;
  init_state.offset_R_L_I = Lidar_R_wrt_IMU;
  kf_state.change_x(init_state);//KF的变化量

  esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();//误差状态协方差初始值
  init_P.setIdentity();
  init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;
  init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
  init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;
  init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;
  init_P(21,21) = init_P(22,22) = 0.00001; 
  kf_state.change_P(init_P);//误差状态协方差变化量
  last_imu_ = meas.imu.back();//最后一个Imu数据

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62

点云去畸变
kf的输入量：

前向传播模型：

？IMU和激光雷达结束时刻的时间指的是相对于这一帧的时间还是相对于最开始的时间

状态的反向传播

位姿及速度的反向传播

以及中间某时刻相对于扫描结束时刻的位姿


然后就能够将局部某个点投影到帧尾处了
在点云去畸变时，需求得lidar周期的多段离散时间完毕后的状态预测，将点云按该状态量转换到imu坐标系下处理，再转换回lidar坐标系
这个是激光雷达到IMU的外参阵

这个是投影公式

void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
  /*** 将上一帧结尾的IMU数据作为这一帧的开始，并获得当前帧激光雷达开始和结束的时间 ***/
  auto v_imu = meas.imu;
  v_imu.push_front(last_imu_);
  const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
  const double &imu_end_time = v_imu.back()->header.stamp.toSec();
  const double &pcl_beg_time = meas.lidar_beg_time;
  const double &pcl_end_time = meas.lidar_end_time;
  
  /*** 通过时间先后点云进行排序 按之前已设定的曲率（单点时间戳）对点云进行排列***/
  pcl_out = *(meas.lidar);
  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);//
  // cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
  //          <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;

  /***初始化IMU的位姿---设置好IMU位姿的数据格式***/
  state_ikfom imu_state = kf_state.get_x();//获取kf的状态
  IMUpose.clear();
  IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));

  /*** 对每一个IMU数据进行前向传播***/
  V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;//角速度平均值，加速度平均值，IMU加速度，IMU速度，IMU的位姿//后三个为推断值
  M3D R_imu;//误差

  double dt = 0;//时间增量

  input_ikfom in;//kf的输入

  for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)//imu数据迭代
  {
    auto &&head = *(it_imu);
    auto &&tail = *(it_imu + 1);
    
    if (tail->header.stamp.toSec() < last_lidar_end_time_)    continue;//只选用在这一帧激光雷达时间里的imu数据
    
    angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
    acc_avr   <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);//获得当前帧下一帧两帧IMU数据的平均值

    // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
    //加速度去除重力影响
    acc_avr     = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;//加速度平均值*重力加速度9.8/归一化后的加速度平均值

    if(head->header.stamp.toSec() < last_lidar_end_time_)//帧头的时间还在上一帧激光雷达时间区域中
    {
      dt = tail->header.stamp.toSec() - last_lidar_end_time_;//获得帧尾到上一帧激光雷达结束时间的时间差
      // dt = tail->header.stamp.toSec() - pcl_beg_time;
    }
    else
    {
      dt = tail->header.stamp.toSec() - head->header.stamp.toSec();//否则就IMU帧尾到帧头的时间差
    }
    
    in.acc = acc_avr;
    in.gyro = angvel_avr;//获得输入的 加速度平均值，角速度平均值
    Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
    Q.block<3, 3>(3, 3).diagonal() = cov_acc;
    Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
    Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;//获得角速度，加速度以及相应偏差值的协方差
        //每收到一次imu数据帧则进行一次前向更新（先验）
    kf_state.predict(dt, Q, in);//获得一个预测的状态值//前向传播后的状态

    /*保存IMU观测值的位姿 */
    imu_state = kf_state.get_x();
    angvel_last = angvel_avr - imu_state.bg;
    acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba);
    for(int i=0; i<3; i++)
    {
      acc_s_last[i] += imu_state.grav[i];
    }
    double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
    IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
  }

  /*** 计算帧尾位姿和姿态的预测值 ***/
  double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
  dt = note * (pcl_end_time - imu_end_time);
    //确保lidar与imu的处理时间对齐
  kf_state.predict(dt, Q, in);//这里的预测和上面的预测有什么区别？//在时间差上有区别//这里的时间差计算的是点云帧的结尾与IMU最后数据的时间//一帧激光雷达，中间的预测值时间差用IMU自己的，结束时预测值时间差用IMU和激光雷达的//
  
  
  imu_state = kf_state.get_x();//此时的状态是一帧结束时刻的
  last_imu_ = meas.imu.back();
  last_lidar_end_time_ = pcl_end_time;//记录IMU和激光雷达结束时刻的时间

  /*** 对每一个激光雷达进行去畸变处理(反向传播) ***/
  // 在点云去畸变时，需求得lidar周期的多段离散时间完毕后的状态预测，将点云按该状态量转换到imu坐标系下处理，再转换回lidar坐标系。
  if (pcl_out.points.begin() == pcl_out.points.end()) return;
  auto it_pcl = pcl_out.points.end() - 1;
  for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)//从当前帧激光雷达中IMU数据从后往前迭代
  {
    auto head = it_kp - 1;//从倒数第二帧IMU开始
    auto tail = it_kp;
    R_imu<<MAT_FROM_ARRAY(head->rot);//旋转阵
    // cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
    vel_imu<<VEC_FROM_ARRAY(head->vel);//速度
    pos_imu<<VEC_FROM_ARRAY(head->pos);//位姿
    acc_imu<<VEC_FROM_ARRAY(tail->acc);//加速度
    angvel_avr<<VEC_FROM_ARRAY(tail->gyr);//角速度//加速度、角速度使用下一帧的数据

    for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)//这里的曲率是什么？除以1000与IMU的偏移时间比较？
    {
      dt = it_pcl->curvature / double(1000) - head->offset_time;//获得时间差//这个时间差是

      /* Transform to the 'end' frame, using only the rotation
       * Note: Compensation direction is INVERSE of Frame's moving direction
       * So if we want to compensate a point at timestamp-i to the frame-e
       * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
       //按时间戳的差值进行插值
      //本时刻的imu位姿
      M3D R_i(R_imu * Exp(angvel_avr, dt));//旋转
      
      V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);//平移
      V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
      //lidar点通过外参(Rp+t)转换到imu坐标系，在imu坐标系下向前一帧做反向变换，再转换回到lidar坐标系 
      //非常简便，但这样的方式并不是严格的线性插值
      V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!//去畸变后的位姿
      
      // 保存去畸变的点和旋转
      it_pcl->x = P_compensate(0);
      it_pcl->y = P_compensate(1);
      it_pcl->z = P_compensate(2);

      if (it_pcl == pcl_out.points.begin()) break;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131