当前位置：首页 > news >正文

Point-LIO代码阅读与解析

news 2025/7/3 16:37:48

前言

Point-LIO是既FAST-LIO之后HKU的MARS实验室又一力作，这个LIO最令我惊艳的创新有两点：

逐点更新：Point-LIO的思想不是像大多数LIO一样，认为100ms激光雷达同时收集一次数据，然后以10Hz的频率去处理并输出定位结果；Point-LIO是收到一个点就进行一次point-to-plane更新，因此它可以实现更高的带宽和输出频率，因此可以说Point-LIO真正认识到并利用了点云不是同时收集的事实。
IMU作为输出：在建模观测时，将IMU测量也进行建模，并进行了饱和检测，这似的系统在IMU饱和时也有较好的精度和稳定性。

Point-LIO github上最近发布了最新的分支是使用iVOX作为地图管理结构，本文就以这个分支进行解析/

1. 参数管理器

Point-LIO是在FAST-LIO基础上进行的改进，可以说对代码结构也进行了优化，使用了一个参数管理器来实现对参数的加载，相比于FAST-LIO中在main函数中将所有参数加载进来要简洁很多。

在参数管理器中，定义了一些常用的参数，iVOX也在这里进行了配置：

IVoxType::Options ivox_options_;
nh.param<float>("mapping/ivox_grid_resolution", ivox_options_.resolution_, 0.2);nh.param<int>("ivox_nearby_type", ivox_nearby_type, 18);if (ivox_nearby_type == 0) {ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;} else if (ivox_nearby_type == 6) {ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6;} else if (ivox_nearby_type == 18) {ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;} else if (ivox_nearby_type == 26) {ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26;} else {// LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18";ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;}

2. 主函数

Point-LIO可以在代码中设置IMU作为输入还是输出，不同设置代码中的状态量维度也不一样：

esekfom::esekf<state_input, 24, input_ikfom> kf_input;
esekfom::esekf<state_output, 30, input_ikfom> kf_output;MTK_BUILD_MANIFOLD(state_input,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((vect3, gravity))
);MTK_BUILD_MANIFOLD(state_output,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, omg))
((vect3, acc))
((vect3, gravity))
((vect3, bg))
((vect3, ba))
);

可以看到state_output将测量值的acc和omg也放到状态量里面了。

接下来是为ESKF设置运动模型和观测模型：

kf_input.init_dyn_share_modified_2h(get_f_input, df_dx_input, h_model_input);
kf_output.init_dyn_share_modified_3h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output);

分别对应于IMU作为输入还是输出的两个方法，里面的具体内容留到ESKF代码部分再进行解析。

接下来设置了初始的协方差矩阵和噪声的协方差矩阵：

// 设置协方差并对ESKF进行设置
reset_cov(P_init);
kf_input.change_P(P_init);
Eigen::Matrix<double, 30, 30> P_init_output; // = MD(24, 24)::Identity() * 0.01;
reset_cov_output(P_init_output);
kf_output.change_P(P_init_output);
// 根据IMU协方差和偏置项，设置噪声的协方差矩阵，对应于(10)
Eigen::Matrix<double, 24, 24> Q_input = process_noise_cov_input();
Eigen::Matrix<double, 30, 30> Q_output = process_noise_cov_output();

然后，进行了回调函数的声明，先介绍对于激光的回调函数。

2.1 雷达回调函数

对于机械式雷达，进入回调函数后会先进行预处理：

p_pre->process(msg, ptr);

预处理逻辑很简单，主要就是判断一下雷达本身有没有提供时间偏移，如果没有就按照yaw角算一个。

由于Point-LIO2之后就没有进行特征提取了，因此这里提取平面点的思路很简单，就是隔point_filter_num个点提取一个作为面点。

对于固态雷达，使用了数据格式中的.tag标签对点是否有效进行了判断，然后同样是跳点添加为面点。

Point-LIO2代码还支持帧合并，可以在config中设置是否启用帧合并，合并多少帧：

con_frame: false # true: if you need to combine several LiDAR frames into one
con_frame_num: 1 # the number of frames combined

如果设置为true，会在callback中将连续几帧进行合并后再处理，实际发布的代码中没有使用con_frame_num变量，而是直接设置为10，也就是合并1s的点云，可以include一下parameters.hpp文件，然后将10改为con_frame_num。

2.2 IMU回调函数

收到IMU消息后，根据时间偏移对IMU消息时间戳进行修正，然后将IMU加入队列。

2.3 同步IMU和雷达数据

处理完上述过程就进入了主循环，首先进入的就是这个函数：

if(sync_packages(Measures))

这个函数主要是将需要处理的雷达和IMU数据对齐，也就是将相邻两个雷达帧之间的IMU数据放入meas.imu，同时还考虑了丢帧的情况，如果丢帧就按照固定扫描时间间隔(lidar_time_inte)选择IMU。

主要对应了这几个判断逻辑：

if(!lidar_pushed)：推入雷达帧并设置起始终止时间，判断是否丢帧
if (!lose_lid && (last_timestamp_imu < lidar_end_time))：如果没丢帧，但是IMU的最新时间戳早于雷达点的终止时间戳，返回false(说明IMU数据没有覆盖LiDAR数据)
if (lose_lid && last_timestamp_imu < meas.lidar_beg_time + lidar_time_inte)：如果丢帧，但是IMU的最新时间戳早于雷达点的起始时间戳+雷达扫描间隔，返回false(说明IMU数据没有覆盖LiDAR数据)
if (!lose_lid && !imu_pushed)：将两帧雷达之间的IMU数据全部放入meas.imu
if (lose_lid && !imu_pushed)：如果丢帧了就把雷达时间设置为起始时间+扫描周期

2.4 初始化

初始化包括两个方面，一个是初始化IMU，一个是初始化地图。

对于IMU初始化，就是在前几帧时累积IMU的数据，计算平均值，和绝对重力方向进行比较，如果差别角比较大，就更新初始化矩阵，计算方式大概是这样：

void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot)
{/** 1. initializing the gravity, gyro bias, acc and gyro covariance** 2. normalize the acceleration measurenments to unit gravity **/// V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity;M3D hat_grav;hat_grav << 0.0, gravity_(2), -gravity_(1),-gravity_(2), 0.0, gravity_(0),gravity_(1), -gravity_(0), 0.0;// 绝对重力方向 叉乘 平均重力方向 归一化 = sindouble align_norm = (hat_grav * tmp_gravity).norm() / gravity_.norm() / tmp_gravity.norm();double align_cos = gravity_.transpose() * tmp_gravity;align_cos = align_cos / gravity_.norm() / tmp_gravity.norm(); // 绝对重力方向 和 平均重力方向 的夹角cosif (align_norm < 1e-6) // 接近于0，说明绝对重力方向和IMU估计的重力方向为0或180度{// 用cos判断正负if (align_cos > 1e-6){rot = Eye3d;}else{rot = -Eye3d;}}else{// 叉积 * 角度 = 轴角表示V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); rot = Exp(align_angle(0), align_angle(1), align_angle(2)); // 转为旋转矩阵}
}

对于地图初始化，先对当前帧进行降采样，变换到世界坐标系后，然后调用ivox的AddPoints接口，ivox内部实现了LRU缓存机制，如果在某个vexol中添加了点，这个voxel会插入到列表最前面，插入新体素时，如果超出哈希表大小，会pop出最不常用的体素。

2.5 ESIKF状态估计

Point-LIO代码中实现了两种不同的ESIKF框架，分别是使用IMU作为输入和输出，先来讲解IMU作为输出的情况，这也是Point-LIO创新点之一。

(1) IMU作为输出

虽然激光点确实是在不同时刻采样得到的，但由于很多雷达制造商在发布激光点之前会进行一次封装，收集一定时间内的点统一发布，因此代码无法实现真正的逐点更新，作者使用了对激光点和IMU数据进行了排序模拟了这种逐点实现。

那么根据论文，如果激光点的时间大于一个IMU测量的时间，就只进行IMU预测，并使用IMU测量值进行更新，而不使用激光测量进行更新。

while (imu_comes) 
{
imu_upda_cov = true;
angvel_avr<<imu_next.angular_velocity.x, imu_next.angular_velocity.y, imu_next.angular_velocity.z;
acc_avr   <<imu_next.linear_acceleration.x, imu_next.linear_acceleration.y, imu_next.linear_acceleration.z;/*** covariance update ***/
double dt = imu_next.header.stamp.toSec() - time_predict_last_const;
kf_output.predict(dt, Q_output, input_in, true, false); // 只预测状态，不传播协方差
time_predict_last_const = imu_next.header.stamp.toSec(); // big problem{double dt_cov = imu_next.header.stamp.toSec() - time_update_last; if (dt_cov > 0.0){time_update_last = imu_next.header.stamp.toSec();double propag_imu_start = omp_get_wtime();kf_output.predict(dt_cov, Q_output, input_in, false, true);propag_time += omp_get_wtime() - propag_imu_start;double solve_imu_start = omp_get_wtime();kf_output.update_iterated_dyn_share_IMU();  // 使用IMU测量进行更新solve_time += omp_get_wtime() - solve_imu_start;}
}
imu_deque.pop_front();
if (imu_deque.empty()) break;
imu_last = imu_next;
imu_next = *(imu_deque.front());
imu_comes = time_current > imu_next.header.stamp.toSec();
}

其中预测部分的代码实现，作者们进行了很好的封装，在前面初始化ESIKF时就设置了函数地址，因此代码中就可以用很简洁和优雅的方式完成代码，例如这里的状态转移：

if (predict_state)
{flatted_state f_ = f(x_, i_in);x_.oplus(f_, dt);
}

而实际上它的实现为Estimato.cpp中的实现：

Eigen::Matrix<double, 30, 1> get_f_output(state_output &s, const input_ikfom &in)
{Eigen::Matrix<double, 30, 1> res = Eigen::Matrix<double, 30, 1>::Zero();vect3 a_inertial = s.rot * s.acc; // .normalized()for(int i = 0; i < 3; i++ ){res(i) = s.vel[i];res(i + 3) = s.omg[i]; res(i + 12) = a_inertial[i] + s.gravity[i]; }return res;
}

使用IMU测量值进行更新update_iterated_dyn_share_IMU的核心代码也在Estimato.cpp中，如论文所述，这部分代码进行了IMU饱和的判断：

void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified<double> &ekfom_data)
{std::memset(ekfom_data.satu_check, false, 6);ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg;ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba;ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov;if(check_satu){if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro){ekfom_data.satu_check[0] = true; ekfom_data.z_IMU(0) = 0.0;}if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro) {ekfom_data.satu_check[1] = true;ekfom_data.z_IMU(1) = 0.0;}if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro){ekfom_data.satu_check[2] = true;ekfom_data.z_IMU(2) = 0.0;}if(fabs(acc_avr(0)) >= 0.99 * satu_acc){ekfom_data.satu_check[3] = true;ekfom_data.z_IMU(3) = 0.0;}if(fabs(acc_avr(1)) >= 0.99 * satu_acc) {ekfom_data.satu_check[4] = true;ekfom_data.z_IMU(4) = 0.0;}if(fabs(acc_avr(2)) >= 0.99 * satu_acc) {ekfom_data.satu_check[5] = true;ekfom_data.z_IMU(5) = 0.0;}}
}

然后又在update_iterated_dyn_share_IMU中进行了卡尔曼增益的计算和状态更新：

Matrix<scalar_type, 6, 1> z = dyn_share.z_IMU;Matrix<double, 30, 6> PHT;
Matrix<double, 6, 30> HP;
Matrix<double, 6, 6> HPHT;
PHT.setZero();
HP.setZero();
HPHT.setZero();
for (int l_ = 0; l_ < 6; l_++)
{if (!dyn_share.satu_check[l_]){PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_);HP.row(l_) = P_.row(15+l_) + P_.row(24+l_);}
}
for (int l_ = 0; l_ < 6; l_++)
{if (!dyn_share.satu_check[l_]){HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_);}HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l);
}
Eigen::Matrix<double, 30, 6> K = PHT * HPHT.inverse(); Matrix<scalar_type, n, 1> dx_ = K * z; P_ -= K * HP;
x_.boxplus(dx_);

代码继续往后，这时激光之间的IMU更新完了，轮到激光点进行更新了，它实际上调用了update_iterated_dyn_share_modified函数，每轮迭代中先调用Estimato.cpp中的h_model_output函数进行观测方程构建。

在这里面会调用ivox的最近邻搜索函数进行搜索：

ivox_->GetClosestPoint(point_world_j, points_near, NUM_MATCH_POINTS);

搜索5个最近点后进行平面拟合，若拟合后的点到平面距离较小则归类为有效特征点，构建观测方程的H矩阵。

for (int j = 0; j < time_seq[k]; j++)
{// ekfom_data.converge = false;if(point_selected_surf[idx+j+1]){V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z);if (extrinsic_est_en){V3D p_body = pbody_list[idx+j+1];M3D p_crossmat, p_imu_crossmat;p_crossmat << SKEW_SYM_MATRX(p_body);V3D point_imu = s.offset_R_L_I * p_body + s.offset_T_L_I;p_imu_crossmat << SKEW_SYM_MATRX(point_imu);V3D C(s.rot.transpose() * norm_vec);V3D A(p_imu_crossmat * C);V3D B(p_crossmat * s.offset_R_L_I.transpose() * C);ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);}else{   M3D point_crossmat = crossmat_list[idx+j+1];V3D C(s.rot.transpose() * norm_vec); // conjugate().normalized()V3D A(point_crossmat * C);ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;}ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity;m++;}
}

下面同样会计算卡尔曼增益并进行更新

Matrix<scalar_type, n, Eigen::Dynamic> PHT;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> HPHT;
Matrix<scalar_type, n, Eigen::Dynamic> K_;
if(n > dof_Measurement)
{PHT = P_. template block<n, 12>(0, 0) * h_x.transpose();HPHT = h_x * PHT.topRows(12);for (int m = 0; m < dof_Measurement; m++){HPHT(m, m) += m_noise;}K_= PHT*HPHT.inverse();
}
else
{Matrix<scalar_type, 12, 12> HTH = m_noise * h_x.transpose() * h_x;Matrix<scalar_type, n, n> P_inv = P_.inverse();P_inv.template block<12, 12>(0, 0) += HTH;P_inv = P_inv.inverse();K_ = P_inv.template block<n, 12>(0, 0) * h_x.transpose() * m_noise;
}
Matrix<scalar_type, n, 1> dx_ = K_ * z; // - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; 
// state x_before = x_;x_.boxplus(dx_);
{P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0);
}

通过一个点更新完状态量后，就会用这个新的坐标变换将这个点转换到世界坐标系，用于后续点到平面的残差构建。

for (int j = 0; j < time_seq[k]; j++)
{// 更新一个点就把这个点放到世界坐标系中PointType &point_body_j  = feats_down_body->points[idx+j+1];PointType &point_world_j = feats_down_world->points[idx+j+1];pointBodyToWorld(&point_body_j, &point_world_j);
}

(2) IMU作为输入

如果选择使用IMU作为输入，就会提前将IMU测量值放入input_in变量中：

input_in.gyro<<imu_last.angular_velocity.x, imu_last.angular_velocity.y, imu_last.angular_velocity.z;                 
input_in.acc<<imu_last.linear_acceleration.x, imu_last.linear_acceleration.y, imu_last.linear_acceleration.z;
input_in.acc = input_in.acc * G_m_s2 / acc_norm;

然后执行IMU的预测，而不执行IMU的更新：

if (dt_cov > 0.0)
{kf_input.predict(dt_cov, Q_input, input_in, false, true); time_update_last = imu_last.header.stamp.toSec(); //time_current;
}
kf_input.predict(dt, Q_input, input_in, true, false);

然后同样构建点到平面残差进行更新：

if (!kf_input.update_iterated_dyn_share_modified()) 
{idx = idx+time_seq[k];continue;
}

至此，就完成了状态估计的过程，下面就是一些收尾工作，包括地图更新和话题发布。

2.6 地图增量更新

MapIncremental()函数会将feats_down_world中的点添加到ivox体素地图中，但这里有一个额外的判断逻辑，用于控制地图中点的数量，保持地图尺寸在可控范围内：

如果存在最近邻点，会判断一下最近邻点与降采样体素中心的距离，如果小于一半的降采样的尺寸，则不添加
如果周围点为空，直接加入这个点

void MapIncremental() {PointVector points_to_add;int cur_pts = feats_down_world->size();points_to_add.reserve(cur_pts);for (size_t i = 0; i < cur_pts; ++i) {/* decide if need add to map */PointType &point_world = feats_down_world->points[i];if (!Nearest_Points[i].empty()) {// 如果存在最近邻点，会判断一下最近邻点与降采样体素中心的距离，如果小于一半的降采样的尺寸，则不添加const PointVector &points_near = Nearest_Points[i];Eigen::Vector3f center =((point_world.getVector3fMap() / filter_size_map_min).array().floor() + 0.5) * filter_size_map_min;bool need_add = true;for (int readd_i = 0; readd_i < points_near.size(); readd_i++) {Eigen::Vector3f dis_2_center = points_near[readd_i].getVector3fMap() - center;if (fabs(dis_2_center.x()) < 0.5 * filter_size_map_min &&fabs(dis_2_center.y()) < 0.5 * filter_size_map_min &&fabs(dis_2_center.z()) < 0.5 * filter_size_map_min) {need_add = false;break;}}if (need_add) {points_to_add.emplace_back(point_world);}} else {// 如果周围点为空，直接加入这个点points_to_add.emplace_back(point_world);}}ivox_->AddPoints(points_to_add);
}

2.7 话题发布

最后就是发布一些话题，包括里程计/点云/轨迹。

if (path_en)                         publish_path(pubPath);
if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFullRes);
if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);