大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群Q:951026257,欢迎大家加入讨论。
Fast-lio2原理解析见链接
从零入门激光SLAM(二十一)——看不懂FAST-LIO?进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
GitHub - huashu996/Fast-lio2-Supernote: Fast-lio2 code with note
本代码解析以算法流程的逻辑解析代码,一些简单的函数忽略讲解。
一、误差迭代更新
1.1 整体框架
迭代更新流程:输出修正结果、更新状态、发布里程计信息以及将特征点添加到地图
/*** iterated state estimation ***/
//计算欧拉角并输出预测结果
V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);
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; //输出预测的结果
//记录更新开始时间
double t_update_start = omp_get_wtime();
//初始化观测矩阵协方差
double solve_H_time = 0;
//迭代卡尔曼滤波更新,更新地图信息
kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);
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();
/******* Publish odometry 发布里程计信息*******/
publish_odometry(pubOdomAftMapped);
/*** add the feature points to map kdtree将特征点添加到地图 KD 树 ***/
t3 = omp_get_wtime();
map_incremental();
t5 = omp_get_wtime();
/******* Publish points发布点云数据 *******/
if (1) 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 迭代更新
初始化动态共享数据结构,用于在迭代过程中共享数据和控制迭代过程。 初始化传播后的状态和协方差,以准备在后续的滤波步骤中使用。
定义并初始化卡尔曼增益矩阵,以用于后续的卡尔曼滤波计算。
初始化状态增量向量,用于存储新的状态增量。
void update_iterated_dyn_share_modified(double R, double &solve_time) {
1.初始化
//动态共享数据结构的初始化:
dyn_share_datastruct<scalar_type> dyn_share; // 定义一个名为 dyn_share 的动态共享数据结构
dyn_share.valid = true; //表示数据有效
dyn_share.converge = true; //表示迭代过程已经收敛
//变量初始化
int t = 0; //用于记录迭代次数
state x_propagated = x_; //初始化当前状态 x_,表示传播后的状态
cov P_propagated = P_; //初始化当前的协方差 P_,表示传播后的协方差
int dof_Measurement; //用于表示测量的自由度
//卡尔曼增益矩阵的初始化
Matrix<scalar_type, n, 1> K_h;
Matrix<scalar_type, n, n> K_x;
//定义 dx_new 并初始化为零向量,用于存储新的状态增量
vectorized_state dx_new = vectorized_state::Zero();
//2.迭代循环
for(int i=-1; i<maximum_iter; i++)
{
//共享数据结构的初始化和检查
dyn_share.valid = true;
//观测模型
h_dyn_share(x_, dyn_share);
if(! dyn_share.valid)
{
continue;
}
//获取观测矩阵 h_x_ 矩阵用于存储观测模型的雅可比矩阵
//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
//记录时间起始点和测量自由度
double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows();
//状态增量计算
vectorized_state dx;
//前状态 x_ 与传播状态 x_propagated 之间的增量 dx
x_.boxminus(dx, x_propagated);
dx_new = dx;
//初始化协方差矩阵 P_ 为传播后的协方差 P_propagated
P_ = P_propagated;
//SO3状态更新
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
int idx = (*it).first;
int dim = (*it).second;
for(int i = 0; i < 3; i++){
seg_SO3(i) = dx(idx+i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for(int i = 0; i < n; i++){
P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));
}
for(int i = 0; i < n; i++){
P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
//S2状态更新
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
int idx = (*it).first;
int dim = (*it).second;
for(int i = 0; i < 2; i++){
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for(int i = 0; i < n; i++){
P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));
}
for(int i = 0; i < n; i++){
P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
//状态维数大于测量自由度时的计算卡尔曼增益
if(n > dof_Measurement)
{
//创建一个大小为dof_Measurement×n 的零矩阵h_x_cur
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
//将 h_x_ 矩阵的前 12 列复制到 h_x_cur 中的左上角
h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
//计算卡尔曼增益矩阵K
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
//计算增益矩阵 K_h 和 K_x
//dyn_share.h 表示测量残差
K_h = K_ * dyn_share.h;
K_x = K_ * h_x_cur;
}
//状态维数小于测量自由度时的计算卡尔曼增益
详情请见。。。
从零入门激光SLAM(二十二)——Fast-lio2代码详解(三) 迭代误差更新 - 古月居 (guyuehome.com)