FAST-LIO2代码解析(四)

0. 简介

在讲完前面三节后我们将开始以主函数触发，并分析ESKF和IKD-Tree中对应的算法。其中ESKF作为主要的部分来进行展开介绍

1. 主函数

作为主函数，内部主要存放了一系列的参数传入，这部分没什么好说的基本都已经注释完成。

// FAST_LIO2主函数
int main(int argc, char **argv)
{
    /*****************************初始化：读取参数、定义变量以及赋值*************************/
    // 初始化ros节点，节点名为laserMapping
    ros::init(argc, argv, "laserMapping");
    ros::NodeHandle nh;
    // 从参数服务器读取参数值赋给变量（包括launch文件和launch读取的yaml文件中的参数）

    nh.param<bool>("publish/scan_publish_en", scan_pub_en, true);               // 是否发布当前正在扫描的点云的topic
    nh.param<bool>("publish/dense_publish_en", dense_pub_en, true);             // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，
    nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, true);    // 是否发布经过运动畸变校正注册到IMU坐标系的点云的topic，需要该变量和上一个变量同时为true才发布
    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>("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>("pcd_save/pcd_save_en", pcd_save_en, false);                 // 是否将点云地图保存到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;

    // 初始化path的header（包括时间戳和帧id），path用于保存odemetry的路径
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera_init";

    /*** variables definition ***/
    /** 变量定义
     * 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             （后面的代码中没有用到该变量）
     * _featsArray              （后面的代码中没有用到该变量）
     */
    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());
    // 将数组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); // 相对IMU的外参
    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));

    double epsi[23] = {0.001};
    fill(epsi, epsi + 23, 0.001); //从epsi填充到epsi+22 也就是全部数组置0.001
    // 将函数地址传入kf对象中，用于接收特定于系统的模型及其差异
    // 作为一个维数变化的特征矩阵进行测量。
    // 通过一个函数（h_dyn_share_in）同时计算测量（z）、估计测量（h）、偏微分矩阵（h_x，h_v）和噪声协方差（R）。
    kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);

    /*** debug record ***/
    // 将调试log输出到文件中
    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;

    /*** 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);
    // 发布雷达的点云，topic名字为/cloud_registered_lidar
    ros::Publisher pubLaserCloudFull_lidar = nh.advertise<sensor_msgs::PointCloud2>("/cloud_registered_lidar", 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);
    //------------------------------------------------------------------------------------------------------
    // 中断处理函数，如果有中断信号（比如Ctrl+C），则执行第二个参数里面的SigHandle函数
    signal(SIGINT, SigHandle);

    // 设置ROS程序主循环每次运行的时间至少为0.0002秒（5000Hz）
    ros::Rate rate(5000);
    bool status = ros::ok();
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
132
133
134
135
136
137
138
139
140
141
142
143

其中有一个函数我们需要特地关心一下，就是kf.init_dyn_share，这部分传入了不同的函数用于初始化

2. get_f函数

这部分主要对应的是fast_lio2论文公式(2-3)，这里列一下原文的解释：
取第一个IMU框架(记为$I$)作为全局框架(记为$G$)，记为ITL = (${}^I{R}_L$,${}^I{p}_L$)为LiDAR和IMU之间的未知外部属性，运动学模型为：

式中，${}^G{p}_I{,}^G{R}_I$ 表示IMU在全局框架中的位置和姿态，$G_g$是全局坐标系中的重力矢量，$a_m$和$w_m$是IMU的测量值，$n_a$和$n_w$表示$a_m$和$w_m$的测量噪声，$b_a$和$b_w$表示$n_{ba}$和$n_{bw}$驱动下的随机游动过程模型的IMU偏差，符号$|a|$^表示一个映射了叉乘运算的向量$a\in R3$的斜对称矩阵。

设i为IMU测量值的索引。根据FAST-LIO中定义的运算符$⊞$，可以在IMU采样周期$∆t$处对连续运动学模型(1)进行离散化：

函数f、状态x、输入u和噪声w，定义如下:

// double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
// vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
// fast_lio2论文公式(2), 起始这里的f就是将imu的积分方程组成矩阵形式然后再去计算
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
{
	Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero(); // 将imu积分方程矩阵初始化为0,这里的24个对应了速度(3)，角速度(3),外参偏置T(3),外参偏置R(3)，加速度(3),角速度偏置(3),加速度偏置(3),位置(3)，与论文公式不一致
	vect3 omega;
	in.gyro.boxminus(omega, s.bg); // 得到imu的角速度
	// 加速度转到世界坐标系
	vect3 a_inertial = s.rot * (in.acc - s.ba);
	for (int i = 0; i < 3; i++)
	{
		res(i) = s.vel[i];						 //更新的速度
		res(i + 3) = omega[i];					 //更新的角速度
		res(i + 12) = a_inertial[i] + s.grav[i]; //更新的加速度
	}
	return res;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

3. df_dx函数和df_dw函数

矩阵$F_{\widetilde{x_i}}$~和$F_{w_i}$的计算如下(更抽象的推导见论文[55]，更具体的推导见论文[22])：

…详情请参照古月居