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();
其中有一个函数我们需要特地关心一下，就是kf.init_dyn_share，这部分传入了不同的函数用于初始化
2. get_f函数
这部分主要对应的是fast_lio2论文公式(2-3)，这里列一下原文的解释：
取第一个IMU框架(记为
式中，
函数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;
}3. df_dx函数和df_dw函数
矩阵
…详情请参照古月居