参考文章

前言

文章：LOAM: Lidar Odometry and Mapping in Real-time

作者：Ji Zhang and Sanjiv Singh

编译：robinvista

来源：https://blog.csdn.net/robinvista/article/details/104379087?spm=1001.2014.3001.5501

目的

​ LOAM是KITTI测试中排名第一的状态估计和激光建图方法，知名度很高，在它的基础上衍生出了很多改进版本，例如LEGO-LOAM、LLOAM、ALOAM、Inertial-LOAM等等。
　　本文对论文和代码的细节进行分析，试图弄明白这个方法的特点以及为何有如此优秀的性能。

特征点提取

​ 既然LOAM是个激光建图和状态估计方法，那就离不开对激光这种传感器特点的分析讨论。机械旋转式激光雷达虽然是连续旋转的，但是它的输出却是一帧一帧的。通过匹配每帧激光点云与上一帧点云，可以估计得到两帧之间机器人的相对位移，这就是激光里程计的工作方式。这里用估计，是因为我们不能精确的得到相对位移。传统的估计方法是直接在原始的点云上操作（例如大名鼎鼎的ICP算法）。但是LOAM没有直接使用ICP，而是采用了更巧妙的方法，它不是直接对大量的点云进行变换，而是在点云的基础上提取出相对较少的特征点，然后再用特征点进行匹配。

​ 如何得到特征点呢？特征点是具有一定特征的点，对于点云中任意一个点X ，给它定义一个特征值，称为c cc，如下式所示。其中，$X_i$是X 相邻的几个点，在程序中选取的是X前5个和后5个。这里暗含着一个前提还没说，就是点云中的点都是按照一定顺序排列的（激光雷达返回的点就是这样的），如果点的排列顺序是杂乱无章的，再使用这个公式就没什么用了。原作者考虑激光雷达的每帧点云数据由很多独立的线组成。因此，在定义特征点时考虑的是单个线上的点与相邻点的关系。点也可看成一个向量， ||X||表示向量的范数，也就是向量的长度，或者说点到雷达坐标系原点的距离。公式中除以 ||X||的原因是考虑到不同距离下得到的激光点的分布疏密程度不一样，为了归一化。
$$
c=\frac{1}{||x||}||\sum_i(X-X_i)||
$$
​ 这篇论文中的公式不多，我们挨个仔细探讨，尽量做到充分理解无死角。有人将上式定义的c 称为曲率，这么叫确实有道理。我们可以想一想，如果点X在一条直线上而且直线上的点都是均匀分布的，那么不管这条直线方向如何，X的前5个点和后5个点的平均值刚好就是X （此时c = 0 ）。上面公式就是在计算平均点与中间点X 的差距，点的排列越平直差距越小，反之点排列越弯曲差距越大c cc的值就越大。下面给出两个例子来更好地展示曲率的含义。左图为平面点云的例子，其中有的点在直线上有的位于边角上，我们计算各点处的曲率。为了直观地展示曲率的大小，我用直线表示在各个点上，直线高度与曲率c cc成正比，如右图所示。越尖锐的点曲率越大，在直线上的点曲率则是0。

第二个例子是由光滑的曲线轮廓生成的点云，如下图所示，这时计算的曲率如右图所示，同样是曲率越大的地方直线越高。这两个例子证明我们对上面公式的理解是正确的。作者提出用曲率信息来区分激光点，这显然是个非常大的创新点。但是我们扪心自问，这个公式并不复杂，高中生都可以理解，而且特征点的概念也并不是什么开天辟地的创新。因为在跟点云处理很相近的另一个领域——图像处理中，特征点的概念几乎是尽人皆知，而且定义是五花八门，随便拿出来一个都比这个公式要复杂的多。但是将特征点的概念推广到激光点云领域可能是作者的一大贡献。

​ 根据曲率值大小，可以对点进行分类。论文中分成两类：曲率大的是角点，曲率小的是平面点。其实这里叫平面点我觉得不太合适，因为提取特征是通过对每条扫描线进行计算得到的，因此应该叫“线上的点”，但是在特征匹配的时候却是用来和平面进行匹配的，这是矛盾的，但是这里不纠结这些无关紧要的细节了。下面我用实际的激光点云数据来展示特征点提取的效果。下图中的绿色点是velodyne 16线激光雷达的原始点云，扫描环境是笔者的卧室，大概就是一个长方体，能够看到点云在垂直方向大致分成了16条线。红色的小圆球是提取出来的角点，蓝色的是平面点。可见，角点基本上位于房间的墙角和过渡较大的地方，例如物体（窗帘）的边缘。但是由于程序中对特征点的数量有限制，不是所有的角点都被提取出来加以利用，可以看到四个墙角只有一个被保存下来，其它三个墙角基本上没有提取。平面点也散落在了墙壁上，同样不是所有平面点都被利用了，只有一部分。

在程序中，对特征点分的更细一些，除了角点和平面点，还使用了曲率c cc不太大的点（称为less sharp point），和曲率不太小的点（称为less flat point），下图中红色的小圆就是less sharp point，蓝色的小圆是less flat point。在右侧的图中我把门打开了，可以清晰地看到门口边界有更多的less sharp point。

对于多线激光雷达来说，计算曲率c 是分别针对单个线进行的。但是velodyne 16线激光雷达返回的点云数据是按照先上下，后左右的顺序，如下面的动画所示。而且上下的顺序也是错杂的，至于为什么是这样的，就要问激光雷达的厂家了，我也不知道。因此在程序中需要先将其按照线号重新排列。velodyne 16雷达每次返回的数据称为一帧（sweep），一帧由16条线组成（每条线称为一个scan），每个scan有很多点。如果将velodyne 16雷达的扫描频率设置为10Hz，那么一秒就返回10帧数据。工作在10Hz的频率下，这个雷达的水平扫描角度的分辨率是0.2°，我们可以算出来理论上一帧有360 / 0.2 × 16 = 28800个点，但是实际上你可以试试，每次的点数不是完全一样的，有时多一点有时少一点，在程序中将存储点的数组定义为40000个元素，这是选了一个保守的上限.

运动估计

​ 提取出特征点，下一步就是借助特征点计算两帧点云之间的相对位移，即点云匹配（或者叫配准、对齐，反正都是一个意思）。在ICP等传统匹配方法中，用点与点之间的距离评价匹配的效果，因此每个点只需要找一个对应的点，但是在LOAM采用了更好匹配标准。提取出特征点的目的是利用特征点表示环境特征，这是一种数据压缩的思想。特征点分为两类，对于角点，一般在转折线上（例如卧室里的墙角），要计算它到折线的距离；对于平面点，一般在比较平坦的表面上（例如墙面），要计算它到平面的距离。我不知道作者是怎么想到这一步的，但是应该也不会很难，拿爷爷辈的ICP方法来说，对它的改进非常多， 其中很多改进都是针对距离度量的，原始ICP使用点到点的距离度量，改进的使用点到线或者点到面（例如这篇文章[1]）。只要你看论文足够多并且善于思考、勤于联想，总能提炼出一些创新点的。

　　计算角点到折线的距离要用到点到直线的距离公式，如下式所示。其中，X0 是角点，直线用两个点X1 , X2 表示

​ 计算平面点到平面的距离要用到点到面的距离公式，如下式所示。其中，X0是平面点，平面用三个不共线的点X1 , X2 , X3表示

​ LOAM使用了点到直线和点到平面来计算距离，这就意味着他适用于结构化比较好的室内环境，因为在室内环境中充满了平直的墙壁、地面、家具、平直的转折角。对于室外环境就没这么理想了，可能充满了树叶、石块、车辆等等不规则的物体，其直线和平面的假设就不太准确了。

　　LOAM还解决了运动畸变问题（叫demotion或者distortion correction）。运动畸变产生的原因就是激光雷达在采集数据的过程中是处于运动状态的。如果激光雷达的扫描频率很高，比自身的运动快得多，我们可以假设畸变很小从而忽略。但是，大多数雷达的频率都不是非常高，以velodyne 16线雷达为例，常用的频率是10Hz（最快也不过20Hz），假如机器人或者无人车的运动速度是1m/s（这算慢的了），在完成一次360°扫描的过程中雷达的位置已经改变了10cm，这就不能再忽略了。LOAM解决运动畸变的方法比较简单，就是根据每个点的相对时间进行补偿。无独有偶，我最近看百度Apollo的激光雷达运动补偿源代码的时候，发现百度也是这么简单处理的。雷达扫描一帧的时间是固定的，可以得到每个点的采集时刻，将所有点都统一到同一时刻，这里选择的是每完成一帧扫描的末尾时刻，如下图所示。t_k就是一帧扫描开始的时刻， t_{k+1}就是完成一次扫描的时刻，水平的箭头表示将所有点都投影到 t_{k+1}t时刻。P_k就是这一帧扫描生成的点云，显然不同的点具有不同的时间戳。

论文中假设雷达做最简单的运动，即匀速运动，也就是说一次扫描过程中雷达的线速度和角速度都是不变的。在时间间隔不大时，例如100ms，可以认为这样的假设比较合理。当然如果是无人机这种做剧烈运动的，这个假设就不太合理了，此时可以用IMU补偿。将 $t_{k+1}时刻相对于 t_k时刻的雷达相对位姿记为T^L_{k+1}$，对每个特征点 i 计算它的补偿变换矩阵 $T^L_{(k+1,i)}$，如下式所示。这就是简单的线性插值，当然是根据匀速运动的假设来的，而且是一种近似，真正的插值可没这么简单。注意是对点云中所有的点都补偿，不仅仅是特征点，因为后面会用所有的点来建图。原公式有个小错误。

用变换矩阵$T^L_{(k+1,i)}$对特征点 i 进行变换即可完成补偿。当然具体实现还要考虑旋转变换的参数化方式，论文中使用了轴角度表示，但是在程序中却使用了欧拉角表示。

优化

​ 别看激光点很多，上面唯一的未知量就是相对位姿$T^L_{k+1}$，我们将它视为变量，求使距离d dd最小的$ T^L_{k+1}$，这可以转化成一个优化问题，然后就可以利用成熟的优化理论和方法解决了。由于特征点的变换中包含了角度的三角函数，显然这个问题是个非线性优化问题。优化目标涉及距离，因此是一个最小二乘问题。

　　最小二乘问题是一种比较常见的问题，不用害怕，有微积分的知识就能理解。我们是机器人学家不是数学家，所以不去深入探讨最小二乘方法背后的理论，会用即可。但是作为使用者，我们应该知道最小二乘问题的特点。很多从事机器人研究的年轻学者数学并不好（笔者也并不避讳这个），对问题的理解停留在人云亦云的表面，或者是知其然而不知其所以然。最小二乘问题最著名的缺点是受离群点的影响很大，下图是一个简单的例子。图中的5个黑点是数据点，假设我们想对这些数据拟合一条直线。如果采用误差的平方作为优化目标，这就是一个标准的最小二乘问题。拟合结果为图中的红色直线。如果我改变其中一个点的纵坐标，可以看到整条直线发生了严重的偏移。我移动的点就相当于实际中可能出现的离群点，这些点可能不是很多，但是仅仅几个就能严重影响解的质量。这是因为每个点的误差以平方项的形式进入方程，不难猜到，误差越大其平方项被放大的更大，为了弥补这一个很大的偏差，最小二乘法不得不牺牲其它的点。图中还展示了另一种拟合方法，这种方法使用了误差的绝对值作为优化目标，又被称为最小一乘解。可以看到，最小一乘解对离群点的包容性要好，它没有出现严重偏移。

　　为了防止结果被离群点污染，通常的做法是在求解前先剔除掉这些害群之马。

​ 求解非线性最小二乘问题的常用方法有LM（ Levenberg-Marquardt）方法和高斯牛顿法。作者在论文中声称采用了LM方法，但是在程序中却使用了高斯牛顿法。不管用哪种方法，都需要计算目标函数的雅克比矩阵，这个是最繁琐的一步。雅克比矩阵由一阶导数构成，求导数可以采用数值法，也可以用解析法。数值法就是用很小的差分近似表示导数，一般用在函数复杂或者根本无法得到解析解的情况；解析法就是直接用初等函数的求导公式求出复合函数的导数，这样比较精确，作者在程序中就是直接求出导数并组装成雅克比矩阵的，他没有调用第三方的函数库。至于作者是手工求解还是用数学软件我就不知道了。

　　程序中用OpenCV自带的solve函数求解得到增量matX，不断迭代transform[i] += matX.at(i, 0);得到最优解。

cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

在改进版本ALOAM中，后人就图省事直接用了Ceres库，省去了计算雅克比矩阵的工作。Ceres库是Google开发的解最小二乘问题的开源库，被用于自家的SLAM项目Cartographer。

　　求最小二乘问题的时候还有个小细节笔者一开始不太放心，就是在目标函数中涉及对应点的选择。因为对应点是根据特征点搜索得到的，也就是说依赖于特征点。如果我们改变了特征点（即使只是微小的改变），都有可能导致对应点改变，那么在求导时距离函数就不能视为不变的了。当然这只是我的理论设想 ，实际情况中大多数对应点肯定是不会特别敏感的，或者即使改变了也无所谓反正不会差太多，因此可以忽略这个问题。

建图

　LOAM缺少闭环检测，它建图的主要目的还是为状态估计服务的。论文中说建图的频率与状态估计不同，比它要慢。扫描一帧就对地图做一次更新，如果用velodyne 16雷达就是10Hz，而论文中使用了简陋的自制激光雷达，建图频率只能做到1Hz。

　　建图的过程就是不断地把匹配好的点云堆积在一起的过程，其中的思路与状态估计有些类似，但是有很多地方不一样。特征点的定义和使用与前面状态估计的一样，但是数量更多了，多了10倍。为什么多了10倍大概是因为建图的频率慢了10倍吧。在寻找对应特征点时，将地图中已有的点云$（Q_{k}）$按照10立方米的格子存储。至于为什么是10立方米我也不知道。使用已经粗略估计出来的单帧点云，它是相对于世界全局坐标系的。凡是与$Q_{k + 1}$ 有交集的方格，从$Q_{k}$中取出位于这些方格中的点，再存入一个KDtree中。然后，再针对不同的特征点找它同类的那些点，**$Q_{k + 1} $ 中的角点找$Q_{k}$中对应的角点，平面点同理，各找各妈。在寻找时限制搜索范围，只找一定半径范围内的。由于现在特征点的数量增加了十倍，再挨个计算距离太慢了，此时作者采用了计算特征向量的方法。为什么用特征向量了呢？这让我们想起了另一种点云匹配方法——NDT方法。**

　　特征向量的几何意义可以从下图中看出来。我生成了一些随机点，这些点都在一个椭球里面（当然不必非得是椭球，你也可以试试长方体等等）。然后再计算随机点的协方差矩阵，最后计算这个矩阵的特征值和特征向量。图中三个红色的箭头就是特征向量（更准确的说是乘以特征值后的特征向量）。可以清楚地看到，特征向量的长度反应了点的分布，也就是说我们根据特征值和特征向量就能计算出直线的方向。平面也是同理，我们可以根据两个较长的特征向量计算平面的法向量，三个向量相交于几何中心，这样平面就确定了。

然后在直线上取两个点利用前面的公式计算角点到直线的距离，在平面上取三个点计算平面点到平面的距离。后面的操作跟前面的一样，也是利用LM方法得到最优的变换，利用新得到的最优变换，将最近的一帧单帧点云叠加到地图点云之上，就完成了建图。新得到的最优变换与之前粗略估计出来的变换一般是不同的，新的变换由于使用了更多点参与计算，应该更接近真实，所以用新的变换作为以后变换的基础。这样LOAM就完成了整个状态估计和建图。整个过程没有特别复杂或者难以理解的地方，作者没有使用传统的ICP或者NDT点云匹配方法，而是自己赤手空拳发明了一种新方法。在理论上，整篇论文没有什么太难理解的东西，甚至可以说都是基础知识，既没有吓人的数学公式、也没有复杂的逻辑。

LOAM论文梳理

LOAM原理分析与实践

参考博客解读

原文链接：https://blog.csdn.net/weixin_41469272/article/details/106432491

作者使用一个单线二维激光雷达，将其旋转起来之后，则该设备能扫描的范围为雷达前方的半球形，实现三维激光雷达效果。

主要技术点：

• 二维雷达固定在一个转轴上，实现3维雷达；一次完整的三维扫描为sweep，雷达平面的一次扫描为scan；
• 将点云分为两类：边线点(edge point角点)和平面点(planar point)，分别对应采取点到线及点到面的约束；在实际的测试中，平面点占总点云的80%。
• 位姿变化估计采用高频粗估计+低频优化。两套算法来实现定位。(laser odometry & mapping odometry)其中，laser odometry高频粗估计是在雷达坐标系下，通过点到线及点到面距离，利用LM得到雷达位姿估计；
• mapping odometry低频位姿精估计，是在世界坐标系下，通过点到线，点到面的距离，得到世界坐标系下的雷达位姿估计优化。
  低频位姿精估计是1HZ，与sweep频率相同。也就是一次完整的sweep会进行一次完整的位姿估计。

LOAM整体框架

将定位与建图分开运行，高频位姿估计+低频优化建图->实现实时，低计算量，低漂移。

LOAM主要包括四个节点：点云提取(scan registration)、雷达里程计(lidar odometry)、雷达建图(lidar mapping) 和位姿集成(transform integration)

点云提取及处理（scan registration）

根据点的曲率c来将点划分为不同的类别（边/面特征或不是特征），在每一个sweep中，根据曲率对点进行排序，来作为评价局部表面平滑性的标准。
一个sweep指完成一次完整的扫描，一次sweep分为多个scan，每一次sweep的雷达位姿定义为这一sweep起始时的状态。

特征点提取及处理
LOAM中选用特征点来进行运动估计，特征点选取(edge point角点)和**平面点(planar point)**，如下黄点为edge point，红点为planar point。试验数据表明，大部分点云都是平面点。边线点起到的约束作用较小。

判断公式：

$P_k$表示第k此整体sweep中得到的点云。$X_{(k,i)}^L$表示第k个sweep中的i点在雷达坐标系下的坐标。

设$i\in P_k$，表示点i是第k次整体sweep中的点。选取i点在同一个scan中相邻的前后5个点$X_{(k,j)}^L,j\in S,j!=i$,计算$\Sigma_j X_{(k,i)}^L-X_{(k,j)}^L$.注意，论文中使用的是平面雷达，每个scan扫描区域是半平面，即空间的平面，雷达依次scan会得到空间平面与雷达扫描平面交线上的点，会得到一条线的点；如果雷达扫过角点即边缘点，就是类似平面V字型的点。

即：

如果是平面，前后的5个点与i的距离相加=0，$c\rightarrow0$

如果是边线角点，则c>0

在上面c的计算公式中，c的计算方程的分母，用于归一化c值，防止因为雷达的距离带来的c值过大或小。

相关代码文件scanRegistration.cpp:

接收雷达驱动发布的点云，解析驱动点云，计算曲率，并按序排布，按照曲率由高到低，将点云分为:

• sharp
• less_sharp
• flat
• less_flat

四种点云并发布对应的消息。

雷达驱动发布的点云topic为：**/velodyne_points*，其中点云的位置是以x,y,z存储的。根据x,y,z计算点云的仰角(angle)，及偏角(ori)，并对应的计算出点云对应的线号，存放在$point.intensity=scanID+scanPeriodrelTime$。其中scanID是整数，为点的线数(0-15)。relTime代表水平偏转度(0-1)scanPeriod=0.1，即将偏转度存为归一化的小数，线束为整数。从而每个点包含的数据包括$x,y,intensity。intensity$则存放的是竖直上的线束及水平上的偏转度。也就是包含位置信息的同时，包含了仰角及偏转角信息。

2.位姿粗估计(雷达里程计lidar odometry):高频

符号定义：

$P_k$是第k次整体sweep中得到的点云；

根据插值将$P_k$映射到k时刻sweep起始坐标系下（点云去掉运动漂移），记为$\tilde{P_k}$，则根据插值将$P_{k+1}$映射到k+1时刻sweep起始坐标系下(点云去掉运动漂移)，记为$\tilde{P_{k+1}}$。将第k帧扫描到的特征点$P_k$映射到第k+1帧的雷达坐标系下，记为$\tilde{P_{k+1}}$。

点云匹配：

LOAM提出的方法是将$\tilde{P_{k}}和\tilde{P_{k+1}}$进行匹配。即将第k个sweep的点云映射到第k+1个sweep的起始，将第k+1个sweep的点云映射到第k+1个sweep的起始。匹配$\tilde{P_{k+1}}$和$\tilde{P_{k+1}}$的点云点面关系，计算得到第k+1个sweep中产生的运动。

点云特征点被分为：边线点和平面点。

边线约束点的两个点确定：

设想两个墙面的竖直墙角线，设雷达偏移较小，相邻的的scan，会出现类似图a的情况，相邻scan会测量到墙角线上的点。因为可以在上一帧中找到两个相邻帧的墙角线上的点，约束当前帧打到墙角上的点到这两个点构成的直线上的距离最小，得到雷达运动T。如下图a所示。

平面约束点三个点确定：

​ 论文中的雷达是二维平面雷达加旋转得到，每一次scan都是一次平面半圆，一个sweep由加在雷达上的电机转动180度，构成半球。一次scan打到对面一个平面上，就是一排直线排列的点。

​ 橙色线表示点 j 所在的scan，蓝色线表示与橙色线相邻的两次scan的线。使用velonedy雷达，每个FOV对应的多线构成一个竖直scan，但是点云约束类似。使用kdtree存储点云信息。

将第k帧扫描得到的特征点$P_K$映射到第k+1帧的雷达坐标系下，记为$\bar{P_K}$；将$\bar{P_K}$与第k+1帧的扫描点云$P_{k+1}$进行点云匹配。在LOAM代码中，计算$\bar{P_k}$是使用$TransformToEnd()$将$P_k$映射到本k时刻sweep的结束，即k+1时刻sweep的开始来得到的。而当前时刻k+1时刻的点云映射到k+1时刻的初始是使用TransfromToStart()实现的。假设k+1时刻的sweep中与前一k时刻的运动一致$T_{k+1}^L$。使用位置插值方法得到整个sweep点云对应的$T_{k+1}$，从而利用TransformToStart()，TransformToEnd()可将点云映射到初始和结束时的坐标系下。

位姿插值

论文中，作者采用的是二维雷达加了一个电机旋转，每一次scan得到的点云的xyz是基于雷达的自身的坐标系，就是已经旋转后的雷达坐标系。

雷达建图(lidar mapping): (低频) 一次完整sweep执行一次

《机器人SLAM导航——核心技术与实践》

LOAM算法

激光SLAM算法中的Gmapping和Cartographer，通常采用单线激光雷达作为输入，且只能在室内环境运行。CartoGrapher支持2D建图与3D建图模式，但是CartoGrapher在3D建图模式下构建出来的地图格式仍然是2D形式的地图。

原理分析

LOAM的核心思想是将SLAM问题拆分为独立的定位和建图分别来处理，其过程如图8-25所示。

首先，特征提取模块(point cloud registeration)从雷达点云中提取特征点(corner和surface)。然后，定位模块(lidar odometry)利用Scan-to-scan方法对相邻两帧雷达点云中的特征点进行匹配，这种帧间特征匹配能得到较低精度的里程计，用该里程计来校正雷达点云的运动畸变。

​ 接着，建图模块(Lidar mapping)利用Scan-to-map方法进行高精度定位，该方法以前面低精度的里程计作为位姿初始值，将校正后的雷达特征点云与地图进行匹配，这种扫描帧到地图的匹配能得到较高精度的里程计(1hz里程计)。基于该高精度的里程计所提供的的位姿将校正后的雷达特征点云加入已有地图，最后将低精度里程计和高精度里程计融合，输出更新速度和精度都较高的里程计。

特征提取模块

​ 从雷达点云中提取特征点。特征提取过程很简单，即对当前帧点云中的每个点计算平滑度，将平滑度小于某阈值(min)的点判断为corner特征点，而平滑度大于某阈值(max)的点判断为surface特征点。所有的corner特征点被存放在corner点云中发布，所有的surface特征点被存放在surface点云中发布，也就是说特征提取结果将发布到两个点云中。

定位模块

​ 利用Scan-to-scan方法对相邻两帧雷达点云中的特征点进行匹配。这里的匹配属于帧间匹配，利用前后两帧配对的特征点，很容易计算出其位姿转移关系，在低速运动场景，直接利用帧间特征匹配就能得到低精度的里程计(10hz里程计)，可利用该里程计在匀速模型假设下对雷达运动畸变做校正。在高速运动场景，就需要借助IMU、VO、轮式里程计等提供的外部定位信息来加快帧间特征匹配速度，以响应高速运动场景下位姿的变换，同时这些外部定位信息可以用于雷达运动畸变校正。

建图模块

​ 利用Scan-to-map方法进行高精度定位，该方法以前面低精度的里程计作为位姿初始值，将校正后的雷达特征点云与地图进行匹配，这种扫描帧到地图的匹配能得到较高精度的里程计(1hz里程计)，基于该高精度的里程计所提供的位姿可将校正后的雷达特征点云加入已有地图。

总结

​ 定位模块输出的里程计虽然精度较低，但是更新速度高。而建图模块输出的里程计虽然精度较高，但是更新速度低。将二者融合可以得到更新速度和精度都较高的里程计，融合通过插值过程实现。以1Hz的高精度里程计为基准，利用10hz的低精度里程计对其进行插值，那么1Hz的高精度里程计就能以10Hz的速度输出了。

​ 如果激光雷达本身帧率很高或者有IMU、VO、轮式里程计等外部定位辅助，且建图模块中的里程计更新速度很高，里程计融合模块中的插值过程也就没有必要了。