高精地图的匹配定位算法

这里主要分析GICP与NDT系列、LOAM系列、FAST_LIO系列算法，其他算法见附件pdf文件。

一、基于体素内点云分布的 GICP与NDT系列

GICP和NDT系列算法都是将当前帧或点云地图相邻点的点集点云分布进行匹配，如将点云划分为体素，计算体素的分布，NDT算法见附件。GICP[19]、NDT和VGICP[20]的主要区别见下图，分布是分布-分布、点-分布和分布-多分布间的匹配，这里主要介绍文献[21]中GICP和文献[22]中VGICP算法,VGICP代码中除了VGICP实现，还有GICP实现。

GICP

由源点云点a(如当前帧)及其k邻近点(k一般取20)组成源点集A，搜索点a对应目标点云(如地图)最近邻点b，由其相邻k个点组成目标点集B，分布分布如下：

根据点云之间相对位姿T，可以得到误差分布：

优化下式可以求得T

通过调整方差矩阵C可实现不同配准方法，文中推荐面-面方式
1）点-点

2）点-面

3）面-面
理论上，面的方差矩阵的特征值有2个比较大，1个为0，通过SVD分解，可将其约束为接近0的值，可以减小这个特征影响，也能让非平面产生一定影响

VDICP代码中通过继承PCL库实现GICP，PCL点云匹配步骤如下，以ICP为例：
1）新建ICP实例：pcl::IterativeClosestPoint icp_tmp;
2）设置参数：
rotation_epsilon_两次迭代相对变换各元素与单位矩阵差值绝对值相加的和；
transformation_epsilon_两次迭代相对位移各元素绝对值和；
max_iterations_：最大迭代次数

3）设置源点云和目标点云：icp_tmp.setInputSource(src_pts_ptr);icp_tmp.setInputTarget(target_pts_ptr);
4）配准：pcl::PointCloud Final;icp_tmp.align(Final);final为输入点云通过相对位姿变换后结果

GICP继承关系如下FastGICP <- LsqRegistration <- pcl::Registration <- pcl::PCLBase，代码见fast_gicp.hpp和fast_gicp_impl.hpp（等价于源文件，方便include）实现步骤如下：
1)新建实例：重写构造函数，新建source_kdtree_和target_kdtree_
2）设置参数：1）中设定最近邻数量为20，匹配方式为PLANE
3）设置源点云和目标点云：重写setInputSource和setInputSource，调用原set函数，以及构造KDTree
4）配准：调用pcl::Registration中align，align中主要函数是computeTransformation，FastGICP重写computeTransformation先计算源和目标点云每个点的方差FastGICP->calculate_covariances，然后再调用LsqRegistration->computeTransformation计算相对位姿，主要优化步骤如下图，其中step_optimize可选用LevenbergMarquardt和GaussNewton，LM方法首先使用FastGICP->linearize(x0, &H, &b)线性化，遍历源点云搜索目标点云最近点，构建误差函数，使用李代数求导，得到线性方程(H+lambda*I)x=b,使用Eigen的Cholesky分解方法LDLT求解线性方程,当FastGICP->compute_error()结果变好则认为二次线性化较好，单次优化成功；变差时，说明二次线性化不好，需要增大lambda，使LM接近一阶梯度下降。

VGICP

文献[20]所提VGICP计算源点云a_i与目标点云中半径r内N_i个点b_j(1个或多个体素内)的距离，通过同时计算局部的多个点，体素和体素之间计算均值和方差时，使用过相同的点，故优化时能达到平滑局部特征的作用。

根据点云之间相对位姿T，可以得到误差分布：

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

为了计算更方便，可简化为

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

VGICP继承关系如下FastVGICP <- FastGICP <- LsqRegistration <- pcl::Registration <- pcl::PCLBase，代码见fast_gicp.hpp和fast_gicp_impl.hpp（等价于源文件，方便include）实现步骤如下：
1)新建实例：重写构造函数，新建source_kdtree_和target_kdtree_
2）设置参数：1）中设定最近邻数量为20，匹配方式为PLANE，voxel_resolution_设为1m，search_method_设为DIRECT1，voxel_mode_设为ADDITIVE
3）设置源点云和目标点云：重写setInputSource和setInputSource，调用原set函数，以及构造KDTree
4）配准：调用pcl::Registration中align，align中主要函数是computeTransformation，FastVGICP重写computeTransformation先重置voxelmap_，再调用FastGICP->calculate_covariances计算源和目标点云每个点的方差，然后再调用LsqRegistration->computeTransformation计算相对位姿，主要优化步骤如下图，其中step_optimize可选用LevenbergMarquardt和GaussNewton，LM方法首先使用FastVGICP->linearize(x0, &H, &b)线性化，根据上式构建体素地图，遍历源点云搜索目标点云最近的1个(默认)或多个体素，分布构建误差函数，使用李代数求导，得到线性方程(H+lambda*I)x=b,使用Eigen的Cholesky分解方法LDLT求解线性方程,当FastVGICP->compute_error()结果变好则认为二次线性化较好，单次优化成功；变差时，说明二次线性化不好，需要增大lambda，使LM接近一阶梯度下降。

二、基于特征的 LOAM系列方法

1、LOAM

LOAM 系列是一种基于特征的 3D 激光 SLAM 算法，文献[10]-[11]提出最初版本的 LOAM，之后出现了 A-LOAM、LeGO-LOAM、LIOM 和 LIO-SAM 等开源 LOAM 系列算法。这里主要介绍文献[12]所提 LeGO-LOAM 算法。

由于LOAM已经闭源，可以参考A-LOAM代码。LOAM算法流程图如下。LOAM基于估计lidar位姿使用匀速模型进行运动补偿。

1、LIDAR ODOMETRY
里程计只计算帧与帧之间位姿，只使用两帧点云。对应ALOAM中scanRegistration.cpp和laserOdometry.cpp代码，前者用于提取点特征，未看见剔除质量较差线点和面点代码，后者用于特征匹配和配准。
1)特征点提取
使用单帧稀疏的激光雷达点云中点i及其周围点集S(通过同一scan线束排序得到最近点)计算点的粗糙度，判断点是否为线点或面点，**线点的相邻点与其距离较远，c较大，面点的相邻点与其距离较近，c较小；故c越大越粗糙，更可能是线点，c越小越平滑，更可能是面点。**为了使点的分布比较均匀和减少计算量，作者将scan分为4个固定的区域(ALOAM每个SCAN分成6个区域)，每个区域选取2个线点和4个面点，c值超过某个阈值(ALOAM-0.1)的点才会被选为线点和面点。选取的原则如下：
a.线点、面点数量不超过设定最大值2、4；
b.避免选取距离相近的点，尽量使点均匀分布；
c.避免选取图a中点B类型面点，其所在平面与激光束光线平行(可通过点B相邻点集S计算面法向量，比较面法向量和激光束方向检测)，这种情况雷达移动时，相邻帧采集的物理点相差较大，且这种点反射较少；避免选取图b中易被遮挡的点A（若点A相邻点集S中存在接近点A激光束路径的，则认为点A易被遮挡），当雷达移动时，A点可能会被遮挡发生闭塞


2）特征点匹配
提取相邻帧中特征点，分为线点集和面点集，使用KD树组织第k帧所有的点。如图(a),对于第k+1帧中线点i，选取第k帧相邻scan中与之最近的2个点(l，j)，稀疏的单帧点云可以保证两个点距离不会太近，同时需根据c判断两个点是否为线点，之所以选择相邻scan，是因为同一scan较难观测到同一条线，被同一scan观测到的线会出现退化情况，不应被选择；如图（b）,对于第k+1帧中面点i，选取第k帧相邻scan中与之最近的3个点(m，l，j)，同时需根据c判断两个点是否为面点，之所以选择相邻scan，是为了避免3个点共线。

根据下式计算点线和点面距离，点线距离三角形面积(线ji和线li叉乘)除以底边长，等于i到线lj的高（距离），点在线上有差距，高如何优化？；假设3个点(m，l，j)在同一个面上，由线lj和线kj叉乘得到面法向量，面法向量和线ji点乘得到点i到面的距离。其使用匀速运动模型估计第k+1帧位姿，将2帧点变换到同一参考坐标系（ALOAM根据上一次里程计估计相对，将第k+1帧变换到第k帧进行搜索）。

2、lidar mapping
对应ALOAM中laserMapping.cpp代码
当前帧（第k+1）点云与地图匹配，过程与里程计相近。其选取当前帧比里程计更多的特征点（10倍），将地图分为10m的体素，使用与当前帧点云相交的体素构建KD树（ALOAM分别用不同KD树保存线点和面点，不需再判断线点和面点），通过KD树选取当前帧最近的点，及其周围线点或面点集S（得到相邻点集后，需要判断点是线点或面点，ALOAM两者都是5个点），计算S的方差、特征值和特征向量，通过PCA方法计算线方向向量和面的法向量，线的一个特征值较大，面的两个特征值较大，将点集几何中心作为线和面的位置，计算点线和点面距离，进而优化。

2、LeGO-LOAM

LeGO-LOAM 的算法框架如下图所示，这里主要介绍算法的 Lidar Odometry 模块定位算法。

1.特征提取
imageProjection.cpp中实现，地面只是简单相邻点z和r的角度判断，聚类结果参考对应论文实现。
Segmentation 模块首先将水平角度分辨率 0.2°的 16 线 Velodyne VLP-16 激光雷达扫描 360°的点云投影到 1800×16 的图像上，图像的像素值是点到激光雷达中心的距离 ri。再从图像中分割出地面。然后使用图像分割方法对图像剩余非地面部分进行聚类，将相同类中点云打上相同标签；将类中点的数量少于 30 个的类视为噪声并剔除，这种类一般与环境中的树叶等较小物体对应。如下图所示，图(a)为一帧原始点云，图(b)为模块处理后的点云，红色点是地面点，其他颜色为不同聚类上的点。

Feature Extraction模块采用与文献[10]-[11]相似公式计算图(b)中各点的粗糙度 c(roughness),只是其将文献 [10]-[11]点位置换成了点的距离。对于某个点 p ，i取其周围点集 S，点的数量|S|设为 10，计算公式如下：

若 c 大于 cth的点作为边缘点，若 c 小于 cth的点作为平面点。将 1800× 16 的图像分成 300×16 的图像的 6 幅子图，每幅子图每行取 c 最大且不属于地面的nFe个边缘点，取 c 最小的nFp 个平面点,可以是地面或其他类别。所有边缘点和平面点集合分别用F e 和 Fp表示，由图 (b)得到的边缘点和平面点分别如图 (d)绿色和粉色点所示，用于上一帧，构建KD树，以及地图匹配和构建。再从每幅子图每行取c最大且不属于地面的nFe1个边缘点，取c最小且属于地面的nFp1个平面点。提取的所有边缘点和平面点集合分别用Fe1和Fp1表示，提取的的边缘点和平面点分别如图 ©蓝色和黄色点所示，用于当前帧。算法中， nFe 、 nFp 、 nFe1和 nFp2分别取 40、80、2 和 4。

2.特征关联
featureAssociation.cpp实现特征关联和里程计估计，不依赖第三方优化库实现两步优化，未发现考虑类别关联匹配的方式。
Lidar odometry 模块首先寻找当前帧 Fe1k 中点 X(k,i)与上一帧 Fek-1共线的 2 个点
X(k-1, j) X(k-1,l) , 寻找当前帧 Fe1k 中点 X(k,i)与上一帧 Fek-1共面的 3 个点
X(k-1, j) X(k-1,l) X(k-1,l) 。除了按文献 [10]-[11]方法寻找匹配点，文献 [12]还利用聚类的类别确定共线和共面的点，剔除一些误匹配，当前帧面点只有地面点，但线点需要搜索上一帧同一类别线点，需要目标跟踪？。
得到匹配点后，构造与LOAM相同点-线的误差项，不同的是使用两步 LM(Levenberg Marquardt)优化方法最小化由点-线和点-面误差项构造的目标函数，求解当前帧的位姿。文献[12]与文献[10]-[11]不同的是其分两步优化求解。首先由于面点都是地面点，使用点 -面误差项优化求解 [t_z,q_roll ,q_pitch]T ,然后使用 [t_z,q_roll ,q_pitch]T作为约束，用点 -线误差项优化求解 [t_x,t_y ,t_yaw]T。该方法精度与一起优化相近，但速度加快35%

3.mapping
mapOptmization.cpp中实现，特征点匹配方式和LOAM一样，寻找最近的5个点，计算线和面参数，不依赖第三方优化库实现当前帧和地图匹配，但未使用两步优化，一起优化所有点，然后使用GTSAM构建相对位姿因子图，进行位姿优化。
mapping过程与LOAM相近，不同的是其点云地图，使用帧点云和位姿形式保存Fe和Fp点；以及使用GTSAM构建相对位姿因子图，进行位姿优化。该形式支持两种不同访问方式，还能利用ICP等方法检测回环，进行闭环优化。

4.闭环优化
mapOptmization.cpp中实现。首先使用KD树保存历史位姿，再搜索距离当前帧7m范围的点云帧，选取时间差超过30s最近的帧作为闭环候选帧，使用ICP进行匹配，若收敛且匹配分数大于0.3这认为是匹配的，将匹配的位姿加入GTSAM因子图进行优化。
论文简单提及闭环优化，代码已实现闭环优化。

3、 MULLS

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

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

二、FAST_LIO系列算法

1、 Fast-LIO2

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

式中，u_j为地图中与点p_j相邻多个(5个)点组成面的法向量，q_j为面的中心点。
该论文使用Ikd树[5]表示局部地图，相对KD树插入、删除更快，迭代EKF的步骤如下：

该论文团队相关论文Fast-LIO[6]和R3LIVE[7]-[9]系列点云匹配时，也是使用点-线或点-面ICP

三、总结与对比

通过上述文献发现，设计高精地图的匹配定位算法，主要需要考虑以下问题：

1. 选用什么方法？

匹配定位算法主要分为基于滤波和非线性优化的方法，一般情况下，基于滤波的方法鲁棒性和实时性较好，基于非线性优化方法精度较高。自动驾驶一般需要融合多传感器数据，融合过程中常会用到滤波和非线性优化两种方法，如文献

[18]，使用 ML-AMCL 计算比较准确的初值，再用 NDT 或 ICP 方法进一步优化结果，提高算法精度。

2.原始点云运动补偿和去畸变

基于 IMU 或编码器：两者频率一般高于激光雷达
2）插值：对估计轨迹进行线性插值、B-spline等插值方法
匀速运动模型：假设机体短时间是匀速运动的，基于匀速运动模型估计激光点对应位姿

3. 如何计算当前帧位姿初值? 对于滤波方法，预测步需要计算当前帧位姿的初始概率分布；对于非线性优化方法，进行非线性优化前，需要给定当前帧位姿的初值。计算当前帧位姿初值主要有以下几种方式：

GPS
使用 GPS 可以得到无累积误差的当前帧位姿，但 GPS 在有遮挡、多路径反射和强电磁干扰的环境中定位不准确，如城市、停车场、桥底和隧道等场景。
基于 IMU 或编码器的惯导系统
IMU 和编码器是感知自身运动的内部传感器，不受外部环境的影响，鲁棒性比较强，短时间测量结果比较准确，长时间测量结果漂移较大。

3）插值：线性插值、B-spline等插值方法
4) 匀速运动模型在没有前两种估计结果的情况下，可认为机体短时间是匀速运动的，基于匀速运动模型估计当前帧位姿。

**4. 已知当前帧的位姿，用什么方法计算当前帧与地图的匹配度？ **
对于滤波方法，在更新步需要使用测量模型更新当前帧位姿的概率分布，常用文献[15]第 6 章的测量模型。对于非线性优化方法，NDT、ICP、LOAM 系列基于特征的方法和 Cartographer 使用的方法都有所不同。

5. 是否考虑动态物体？以上介绍的方法都是未考虑动态物体。如果考虑动态物体，需要将全局和局部地图中动态和静态物体分开，文献[1]中提到一种标准的表示方法，如下图所示。
第一层是静态(static)物体，如道路、建筑物等。
第二层是准静态(quasi-static)物体，如路边基础设施、指示牌等。
第三层是动态(dynamic)物体，如天气信息、交通灯时间和拥堵程度。第四层是高动态物体(highly dynamic)物体,如车辆、行人等。

**6. 用激光雷达的什么信息？ **
激光雷达的原始数据包括点云和反射值数据。多数方法是直接使用点云数据进行定位。但是，文献[9]和文献[12]都将点云数据变换成了像素值为点到激光雷达距离的图像进行处理，提取特征区域；文献[16]使用反射值和高度值地图进行定位。
7. 使用当前帧哪部分数据估计其位姿？
选择当前帧有特征的部分数据进行定位，可以减少计算时间和滤除噪声数据。如：文献[9]选取点云中特征质量比较好的区域进行定位；文献[12]只使用当前帧部分平面点和边缘点进行定位。
8. 建立怎样的全局地图用于定位？
确定方案后，需要根据 3、4、5、6 点，建立相应全局地图用于定位。
目前在自动驾驶领域比较常用的主要是 NDT 和 ICP 演变的方法。NDT 和 ICP 的精度都比较高，文献[18]结果如下图所示，可达到 0.2m 左右。文献[6]中实验表明，NDT 存储概率分布比 ICP 存储点云更节省存储；在 3D 特征较少和当前帧与地图重合度比较低的场景，如高速公路和隧道等场景，NDT 表现比 ICP 好； ICP 对给定的初值比较敏感；NDT 的实时性较好。在不同文献中，由于使用了不同的实现方法、数据集和参数等，NDT 和 ICP 的精度有所不同，但精度相近。

9.使用什么样闭环检测方法
一般分为基于位姿匹配和基于特征的方法。
1）基于位姿搜索候选帧，使用CSM算法暴力匹配点云帧，可使用cartographer中分支定界方法加速匹配；
2）基于位姿搜索候选帧，使用ICP等算法匹配点云帧
3）基于点云DBOW3等特征作闭环检测

八、参考文献**

综述：

Kuutti S , Fallah S , Katsaros K , et al. A Survey of the State-of-the-Art Localization Techniques and Their Potentials for Autonomous Vehicle Applications[J]. IEEE Internet of Things Journal, 2018:829-846.
Badue C , Guidolini R , Carneiro R V , et al. Self-driving cars: A survey[J]. Expert Systems with Applications, 2020:113816.
Elhousni M , Huang X . A Survey on 3D LiDAR Localization for Autonomous Vehicles[J]. IEEE, 2020.
Jonnavithula N , Lyu Y , Zhang Z . LiDAR Odometry Methodologies for Autonomous Driving: A Survey[J]. 2021.

NDT：

Biber P . The normal distributions transform: a new approach to laser scan matching[C]// 2003.
Magnusson M , Lilienthal A , Duckett T . Scan registration for autonomous mining vehicles using 3D-NDT. Wiley Subscription Services, Inc. A Wiley Company, 2007.
Magnusson M . The three-dimensional normal-distributions transform : an efficient representation for registration, surface analysis, and loop detection[J]. renewable energy, 2009.(博士学位论文)

ICP：

Besl P J , Mckay H D . A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 1992, 14(2):239-256.
Yoneda K , Tehrani H , Ogawa T , et al. Lidar scan feature for localization with highly precise 3-D map[C]// Intelligent Vehicles Symposium. IEEE, 2014.

LOAM 系列基于特征的方法

Ji Z , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014.
Zhang J , Singh S . Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2):401-416.
Shan T , Englot B . LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain[C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019.

CSM：

Olson E B . Real-time correlative scan matching[C]// IEEE International Conference on Robotics & Automation. IEEE, 2009.
Hess W , Kohler D , Rapp H , et al. Real-time loop closure in 2D LIDAR SLAM[C]// 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.

直方图：

Sebastian Thrun 等著，曹红玉等译. 概率机器人[M]. 2005.
Wan G , Yang X , Cai R , et al. Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse City Scenes[J].2017.
https://developer.apollo.auto/devcenter/idpcourse_cn.html

粒子滤波：

Rohde J , Jatzkowski I , Mielenz H , et al. Vehicle pose estimation in cluttered urban environments using multilayer adaptive Monte Carlo localization[C]// 19th International Conference on Information Fusion (FUSION). IEEE, 2016.

https://developer.apollo.auto/devcenter/idpcourse_cn.html