高精地图地图匹配定位算法（二）

一、 方案

结合新的地图匹配方面论文，进一步完善《高精地图地图匹配定位算法》。

1. Point-Line(PL) ICP
   文献1所提点-线ICP步骤与ICP步骤一样，使用迭代点-线距离方法计算两帧点云相对位姿，文献1中根据目标点云和相对位姿初值查找与之最近的源点云中2个点，然后最小化点-线距离优化两帧点云相对位姿：
   
   式中，n_i为线的法向量
   

2. Point-Plane ICP
   文献2所提点-面ICP骤与ICP步骤一样，使用迭代点-面距离方法计算两帧点云相对位姿，文献1中根据目标点云和相对位姿初值查找与之最近的源点云中2个点，然后最小化点-面距离优化两帧点云相对位姿：
   
   式中，n_i为面的法向量

3. GICP
   文献3所提GICP根据最近点匹配，目标点云和源点云对应的点a_i和b_i，根据点周围点(20个)计算点的分布，使用其均值和方差C构成高斯分布，两个分布之间的距离概率分布如下：
   
   最大化距离概率分布，可求得相对位姿T：
   
   

• 当方差满足如下条件时，该方法与点-点ICP一样
  

• 当方差满足如下条件时，该方法与点-面ICP一样
  

• 当方差满足如下条件时，该方法与面-面ICP一样：这里对方差进行SVD分解，最大两个特征值取1,最小特征值取一个较小的数，这样使得迭代优化时，梯度主要沿面的法向量方向
  

从实验结果来看，GICP结果明显优于点-点和点-面ICP方法

4. VGICP
   文献3所提VGICP计算源点云a_i与目标点云中半径r内多个点b_j的距离，通过同时计算局部的多个点，能达到平滑局部特征的作用。
   

最小化距离计算相对位姿：

算法步骤如下，在一次优化中，只需要对目标点云b_j进行一次体素化，计算其均值和方差，在迭代优化过程中不需要使用KD树进行最近点匹配：

5. Fast-LIO2
   文献4所提Fast-LIO使用迭代EKF估计点云帧的位姿，IMU积分预测点云帧位姿，使用点-面ICP更新点云帧位姿，观测方程如下式。
   

式中，u_j为地图中与点p_j相邻多个(5个)点组成面的法向量，q_j为面的中心点。

该论文使用Ikd树[5]表示局部地图，相对KD树插入、删除更快，迭代EKF的步骤如下：

该论文团队相关论文Fast-LIO[6]和R3LIVE[7]-[9]系列点云匹配时，也是使用点-线或点-面ICP，以及IMU位姿相对约束
以ouster64线为例
1）standard_pcl_cbk：提取特征点加入lidar_buffer、时间加入time_buffer
Preprocess::oust64_handler：对点云进行预处理，固定间隔降采样或提取面特征点
2)imu_cbk-:IMU数据加入imu_buffer
3）sync_packages：获取lidar_buffer第1帧数据
4）ImuProcess::Process：
ImuProcess::IMU_init：静止初始化init_state,计算初始重力向量grav、角速度偏置bg，但初始位姿没有对齐重力向量
UndistortPcl：IMU预测及点云运动补偿
5）体素滤波降采样及初始化ikdtree
6）update_iterated_dyn_share_modified：更新状态，
h_share_model：h_dyn_share(x_, dyn_share)获取ikdtre降采样点的最近邻点，计算H_x
更新
7）map_incremental：将降采样点加入ikdtree地图

6. MULLS
   文献10所提MULLS提取特征流程如左图，首先进行降采样和运动补偿，然后提取地面，再从非地面点提取点、线、面特征，并将线面特征分为与地面平行和垂直的线面，其中，点线面特征根据点及其周围K个点的计算，公式如下式。
   
   

点云匹配时，根据两帧点云点的属性构建点-点、点-线或点-面约束，如下图所示， 所有点构建的目标方程如下式，其中权重根据特征之间类别、距离和强度计算：

二、总结

三、参考文献

1. An ICP variant using a point-to-line metric
2. Generalized-ICP
3. Voxelized GICP for Fast and Accurate 3D Point Cloud Registration
4. FAST-LIO2: Fast Direct LiDAR-inertial Odometry
5. ikd-Tree: An Incremental K-D Tree for Robotic Applications
6. FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter
7. R3LIVE++: A Robust, Real-time, Radiance reconstruction package with a tightly-coupled LiDAR-Inertial-Visual state Estimator
8. R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package
9. R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping
10. MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square