学习笔记之——激光雷达SLAM（LOAM系列的复现与学习）

For cooperaive inquiries, please visit the website：guanweipeng.com

文章共15,399字 · 阅读需要大约52分钟

一键AI生成摘要，助你高效阅读

问答

gwpscut

23242人浏览 · 2021-08-17 16:58:52

gwpscut · 2021-08-17 16:58:52 发布

概述

激光雷达SLAM

激光雷达从2D到3D（Cartographer）

LOAM (Lidar Odometry and Mapping in Real-time)

KITTI数据集

A-LOAM

LeGO-LOAM (Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain)

LIO-SAM (Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping)

SC-LIO-SAM

LVI-SAM (Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping)

LOAM_livox（固态激光雷达）

CCM- SLAM

参考资料：

概述

激光雷达SLAM

SLAM问题被认为是真正意义上实现机器人自主移动的关键。其问题可以理解为如下的生活问题。

当你来到一个陌生的环境时，为了迅速熟悉环境并完成自己的任务（比如找饭馆，找旅馆），你应当做以下事情：

a.用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）
b.在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）
c.当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）
d.根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）
e.当无意中走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）。实际这一步可有可无。

以上五步是同时进行的，因此是simultaneous localization and mapping。

目前用在SLAM上的Sensor主要分两大类，激光雷达和摄像头。

由于Sensor和需求的不同，SLAM的呈现形式略有差异。大致可以分为激光SLAM（也分2D和3D）和视觉SLAM（也分Sparse、semiDense、Dense）两类，但其主要思路大同小异。

SLAM算法在实现的时候主要要考虑以下4个方面吧：

地图表示问题，比如dense和sparse都是它的不同表达方式，这个需要根据实际场景需求去抉择
信息感知问题，需要考虑如何全面的感知这个环境，RGBD摄像头FOV通常比较小，但激光雷达比较大
数据关联问题，不同的sensor的数据类型、时间戳、坐标系表达方式各有不同，需要统一处理
定位与构图问题，就是指怎么实现位姿估计和建模，这里面涉及到很多数学问题，物理模型建立，状态估计和优化

其他的还有回环检测问题，探索问题（exploration），以及绑架问题（kidnapping）。

本博文主要学习激光雷达SLAM，激光雷达传感器利用光原理进行工作，激光雷达代表光探测和测距。它们可以探测到300米以内的障碍物，并准确估计它们的位置。在自动驾驶汽车中，这是用于位置估计的最精确的传感器。

激光雷达传感器由两部分组成：激光发射（顶部）和激光接收（底部）。发射系统的工作原理是利用多层激光束，层数越多，激光雷达就越精确。层数越多，传感器就越大。激光被发射到障碍物并反射，当这些激光击中障碍物时，它们会产生一组点云，传感器与飞行时间（TOF）进行工作，从本质上说，它测量的是每束激光反射回来所需的时间。如下图：

当激光雷达的质量和价格非常高时，激光雷达是可以创建丰富的三维环境，并且每秒最多可以发射200万个点。点云表示三维世界激光雷达传感器获得每个撞击点的精确（X，Y，Z）位置。

激光雷达传感器可以是固态的，也可以是旋转的，固态激光雷达将把检测的重点放在一个位置上，并提供一个覆盖范围（比如FOV为90° ）。在后一种情况下，它将围绕自身旋转，并提供360度旋转。在这种情况下，一般把它放在设备顶上，以提高能见度。

激光雷达很少用作独立传感器。它们通常与相机或雷达结合在一起，这一过程称为传感器融合。融合过程可分为早期融合和后期融合。早期融合是指点云与图像像素融合，后期融合是指单个检测物的融合。

激光雷达进行障碍物的步骤通常分为4个步骤：

点云处理
点云分割
障碍聚类
边界框拟合

为了处理点云，我们可以使用最流行的库PCL（point cloud library）。它在Python中可用，但是在C++中使用它更为合理，因为语言更适合机器人学。它也符合ROS（机器人操作系统）。PCL库可以完成探测障碍物所需的大部分计算，从加载点到执行算法。这个库相当于OpenCV的计算机视觉。因为激光雷达的输出很容易达到每秒100000个点，所以我们需要使用一种称为体素网格的方法来对点云进行下采样。体素网格是一个三维立方体，通过每个立方体只留下一个点来过滤点云。立方体越大，点云的最终分辨率越低。最终，我们可以将点云的采样从几万点减少到几千点。

滤波完成后我们可以进行的第二个操作是ROI（感兴趣区域）的提取，我们只需删除不属于特定区域的每一些点云数据，例如左右距离10米以上的点云，前后超过100米的点云都通过滤波器滤除。现在我们有了降采样并滤波后的点云了，此时可以继续进行点云的分割、聚类和边界框实现。

点云分割任务是将场景与其中的障碍物分离开来，其实就是地面的分割。一种非常流行的分割方法称为RANSAC（RANdom Sample consenses）。该算法的目标是识别一组点中的异常值。点云的输出通常表示一些形状。有些形状表示障碍物，有些只是表示地面上的反射。RANSAC的目标是识别这些点，并通过拟合平面或直线将它们与其他点分开。

为了拟合直线，我们可以考虑线性回归。但是有这么多的异常值，线性回归会试图平均结果，而得出错误的拟合结果，与线性回归相反，这里的ransac算法将识别这些异常值，且不会拟合它们。

如上图所示我们可以将这条线视为场景的目标路径（即道路），而孤立点则是障碍物。它是如何工作的？

过程如下：

随机选取2个点
将线性模型拟合到这些点计算每隔一点到拟合线的距离。如果距离在定义的阈值距离公差范围内，则将该点添加到内联线列表中。

因此需要算法一个参数：距离阈值。

最后选择内点最多的迭代作为模型；其余的都是离群值。这样，我们就可以把每一个内点视为道路的一部分，把每一个外点视为障碍的一部分。RANSAC应用在3D点云中。在这种情况下，3个点之间的构成的平面是算法的基础。然后计算点到平面的距离。

下面点云上RANSAC算法的结果。紫色区域代表车辆。

RANSAC是一个非常强大和简单的点云分割算法。它试图找到属于同一形状的点云和不属于同一形状的点云，然后将其分开。

激光雷达从2D到3D（Cartographer）

之前博客《ROS实验笔记之——基于cartographer方法的SLAM》基于Cartographer的2D SLAM。实际上，Cartographer支持2D和3D激光雷达的输入，实现机器人定位，并构建栅格地图。

Cartographer是由谷歌于2016年开源的一个支持ROS的室内SLAM库，并在截至目前为止，仍然处于不断的更新维护之中。
特点：代码极为工程，多态、继承、层层封装的十分完善。提供了方便的接口，便于接入IMU、（单/多线）雷达、里程计、甚至为二维码辅助等视觉识别方式也预留了接口（Landmark）。

2D-SLAM：基于2D栅格地图，可以直接用于导航。

3D-SLAM：基于hybridGrid，译为混合概率地图，可以理解为3D栅格地图。明确：RViz 仅显示3D混合概率网格的2D 投影(以灰度形式)。该地图难以直接使用。

在Cartographer前端中，不断维护的是scan和submap之间的位姿。整个的map是由一个个submap组成的。

Cartographer的匹配方法

Cartographer采用的是scan-map的匹配方法。一般情况的SLAM有如下三种：

scan-scan：这个意味着利用两帧激光数据（每帧激光束的数目相同），计算二者之间的变换。典型方法：ICP。
scan-map：利用一帧激光数据和地图数据，找到激光数据在地图中的位置。
map-map：利用一个子地图数据，在一个更大的地图中找到它合适的位置。

不管是2D还是3D，首先要有一个初始的位姿，在此基础上进行优化：

有IMU，则采纳其角速度积分作为初始姿态。不信任IMU任何加速度信息。
有里程计，则采纳里程计的线速度积分作为初始平移。
二者都没有，根据之前的运动做一个匀速的假设。

注意，cartographer的多传感器融合是一个松耦合，主要依赖激光来定位。IMU和里程计数据并没有被构建到真正优化的目标函数中。

在得到了初始位姿以后，初始位姿要经过第一阶段解算：CSM（Correlation Scan Match 相关扫描匹配）——构建似然场。即对原先的地图map进行一个高斯模糊，让它膨胀一些，然后把激光scan在一个搜索窗口内暴力匹配，计算得分。注意，这里有两个问题：

1.得分怎么算？

如果scan的点落在障碍物模糊区域内，落的越多，得分越高。

2.地图不是无限大的吗，你怎么保证在搜索窗口里就能找到位姿呢？

因为有初始位姿。误差肯定在一个范围内而不会马上发散到很远，所以可以在一个位姿的窗口内，对位姿进行暴力匹配搜索。（初始位姿估计中，里程计数据不会突然激增；imu的加速度信息会漂移，但是算法对于imu加速度数据选择直接丢弃不看；而根据之前位姿匀速假设也不会飘走）

这时候我们就要考虑，什么是位姿？位置+姿态。对于2Dslam而言，有三个变量，x，y，yaw角。对于3Dslam而言，有x，y，z，roll，pitch，yaw六个变量。

2dslam中，采用三层循环，（最外层为θ，减小sin和cos的频繁计算），对x，y，θ在给定大小的搜索窗口内进行穷举，计算最高得分的x，y，θ作为一阶段解算的输出位姿。
3dslam中，采用六层循环，对x，y，z，roll，pitch，yaw六个变量在搜索窗口内穷举，计算得分最高的作为一阶段解算输出位姿。很显然，3d-slam的这种方式对于计算资源依赖较大，复杂度达到O(n^6)级别。因此3d-slam的CSM方法，作为一个配置选项，默认是不开启的。当然如果用户机器比较牛逼，也可以选择开启。

第一阶段CSM解算中，位姿在其中是一个离散的变量，通过暴力枚举获得输出结果；但是暴力枚举也是存在分辨率的，例如：如果角度步长设为1度，但如果刚好真正的角度是5.5度，那么CSM只能搜索到5或6度，而无法进一步细化，逐步累积将会造成误差。因此，引入第二阶段位姿解算：非线性优化。

Si(T)表示把激光数据S用位姿T进行转换，M(x)表示得到坐标x的地图占用概率。思路：S代表了激光击中障碍物，将激光点在机器人坐标系下的位置，经过T转换到世界坐标系下以后，应该尽可能的落在已有地图的障碍物上。

第二阶段的位姿求解，显然位姿在其中是一个连续的变量，通过梯度下降的方法求解目标函数。由于地图是离散的，因此需要对地图进行插值处理，使地图也变成一个可以求导的连续变量，这样才能优化前述目标函数。

线性插值：已知数据 (x0, y0) 与 (x1, y1)，要计算 [x0, x1] 区间内某一位置 x 在直线上的y值；

双线性插值本质上就是在两个方向上做线性插值。双三次插值：更加复杂的插值方式，它能创造出比双线性插值更平滑的图像边缘。使用最近16个点插值。

Cartographer的建图方法

Cartographer的地图（map）以子地图（submap）的形式组成。分为前端和后端。前端：根据帧间匹配算法（scan-match），实时根据激光(scan)来推测累积的scan相对于submap的位姿。后端：检测回环（发现在已到达的位置附近），修正各个submap之间的位姿。根据代码可以判断，2D和3D基于的是同一套思路，但是在实现上有一定区别。接下来结合2D和3D部分，对比介绍实现定位和建图的方法。

在介绍定位和建图思路之前，先介绍一下地图的更新方式：

以方格代表地图块，内部存储数据用来表示被占据的概率。发出一束激光，打到一个障碍物点，被打到的称为hit点，中间连线上的空区域，称为miss点。 2d和3d都是存储的这样的地图。3d相当于是3维的栅格地图。宏观上：多次观测到，提升其概率。问题是，如何用公式表达这个“多次观测”来实现“概率提升”？如下所示

p表示占据概率，当p=0.5时，概率比值odds=1，表示占据和空闲各占一半。odds^-1表示函数逆运算。
p_hit=0.55代表该位置被激光打到一次的概率，第一次观测会被直接赋值。
M(x)表示地图中x位置处的概率值。

举例：

初始时刻，栅格未知状态，激光第一次打到了位置x处，M(x)概率更新为0.55。
随后，激光第二次重复打到了同一个位置： odd(p_hit)=0.55/（1-0.55）=1.22, odds(M_old(x))=odds(0.55)=1.22 odd(p_hit)和odds(M_old(x))相乘为1.484，再求函数逆运算，恢复出更新后的M_new(x)=0.597。

Cartographer的后端

Cartographer在后端主要寻找回环，并根据建立的约束对所有的sumap进行统一优化。回环检测目的是：检测当前位置是否曾经来过，即采用当前scan在历史中搜索，确认是否匹配。

为什么要有回环检测呢？原因有二：1. 已有地图时位姿初始化，不知道当前帧初始位姿，也就不清楚在地图中哪个位置，无法做定位。 2.有累积误差，仅靠前端递推，不进行修正的话，地图很容易变形。因此接下来我们探讨两个问题：1.如何检测回环。2.检测回环后该怎么做。

如何检测回环

检测回环和前端的思路也比较相似，先通过穷举暴力匹配，再通过优化精细修正。但是，前端的暴力穷举，是在有个初始位姿的基础上在一个小窗口内穷举。后端重定位，没有初始位姿了，暴力匹配的范围变成了整个地图。因此必须采用算法加速处理：多分辨率地图+分枝定界操作。

假设有一帧激光：

蓝色代表障碍物：

在高分辨率的地图上，四个点命中3个；在低分辨率的地图上，四个点全部命中。激光在低分辨率的地图上匹配情况： 代表得分的上界（再往精细展开，匹配得分只能更低，不能更高） 在高分辨率的地图上匹配情况： 代表得分的下界（再往粗略缩放，匹配得分只能更高，不会更低）

分支定界：

先把整个地图中的一个区域展开到底（最高分辨率），得到一个匹配分数（得分下界）；
然后把其他区域不展开，算匹配分数。（得分上界）
如果低分辨率区域的得分上界，还没有已展开到底的高分辨率区域的下界高，这个低分辨率区域就不再展开了，统统丢掉不要。

左图四个命中3个，得分75；右图四个命中2个，得分50；那么激光打在子区域A的可能性就要大于B，因此B就无需继续展开成更精细的地图了。

2D-slam的思路比较简单前端：小范围内穷举+非线性优化方法修正位姿。回环检测：大范围内穷举（利用分支定界加速） +非线性优化修正位姿。
3D-slam有所不同。前端的暴力匹配方法，是直接6层循环暴力枚举的，因此配置文件中默认不开启，而是在初始通过IMU、里程计等预测位姿基础上，直接非线性优化修正位姿。如果回环检测仍然是：大范围内6个循环穷举+分支定界的话，小范围都嫌慢，大范围更别提。直接对位姿非线性优化？ 1.没有初值，会算到猴年马月。2.会落入局部最优值。

回环检测后，就到优化了~而优化的本质，还是一个最小二乘问题。

LOAM (Lidar Odometry and Mapping in Real-time)

LOAM是Ji Zhang早期开源的多线LiDAR SLAM算法。该代码可读性很差，作者后来将其闭源。其效果如下：

上文介绍的cartographer主要解决室内问题，LOAM室内外都可以，但是没有回环检测。Cartographer的3D部分，更像是2D的扩展：即用2D的思路去做3D的事情。而LOAM则主要解决3D问题，其核心思路难以解决2D问题。虽然该算法发表于14年RSS，但是在KITTI上的Odometry模块（The KITTI Vision Benchmark Suite）的激光SLAM排行榜上，仍然霸占前列。

由于LOAM的工程已经闭源。本博文在此处采用下面两个github工程来查看LOAM的效果～

其框架如下图所示

Point Cloud Registration：点云不是同一时刻获取的，每一个帧点云，其中的每一个点，都是不同时刻获取的，因此把它进行运动补偿：获取每个点的时间戳，位姿插值，把点云先投影到同一时刻；提取特征点。
Lidar Odometry：估计两帧点云之间的位姿变换，获得两个时刻之间的相对位姿，频率较高 10Hz
Lidar Mapping：建图模块，把连续10帧的点云数据和整个地图匹配，获得世界坐标系下的位姿，频率较低 1Hz。
Transform Intergration：实时利用世界坐标系下的位姿和两个时刻之间的相对位姿，更新各个时刻世界坐标系下的位姿。

Cartographer使用栅格地图，地图中存储着占据概率，通过把点云投影到栅格地图，计算匹配得分，找到最合适的投影，作为位姿变换。但是，LOAM使用的是点云地图，那么点云投影后，进行匹配的就还是点云地图。两堆点云，如何匹配？

传统方法，使用ICP方法，即默认两堆点云中最近的点是匹配点，构建矩阵进行奇异值分解，得到变换矩阵后投影点云，然后再次寻找匹配点，重新计算投影……直至收敛。然而，对SLAM算法而言，要求同步定位和建图。这样随便根据距离选匹配点，计算太复杂，可能算来算去都不收敛，压根就不能实时，计算量实在是太大了。LOAM作者决定对点云提取特征，然后根据稀疏的特征来计算位姿变换。作者决定提取两种特征点：平面点和边缘点。

对于平面点与边缘点，作者引入曲率计算方法

计算曲率听起来是一个很麻烦的事情，在高等数学中，一条曲线的曲率以如下公式进行计算：

但事实上作者并不是这样算的。他直接利用激光的每条扫瞄线中，一个点前后各五个点，计算平均值到该点的距离。

直观理解如下图示

平面点：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
边缘点：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。

作者对整个扫描进行分段，分四段，每段各取2个边缘点和4个平面点。在确定了边缘点和平面点后，两帧点云将根据这两种特征点进行运动估计。

边缘点匹配方法：

红、绿、蓝代表多线激光雷达的扫描线；在第k+1帧中，边缘点i处在红色的扫瞄线上；在第k帧中，红线上的边缘点j和i更近，在相邻绿线上再找到一个最近的边缘点l，那么l和j构成一个边缘。因此，对边缘特征点来说，优化的目标就是，i到直线lj的距离最近。

向量叉乘的模长，代表平行四边形的面积；除以底，代表平行四边形的高；也就是点到直线的距离。

平面点匹配方法：

红、绿、蓝代表多线激光雷达的扫描线；在第k+1帧中，平面点i处在红色的扫瞄线上；在第k帧中，红线上的平面点j和i更近，在同线上再找到一个附近的平面点l，在相邻绿线上再找到一个最近的平面点m，那么lmj构成一个平面。因此，对平面特征点来说，优化的目标就是，i到平面mlj的距离最近。

分子：三维物体的体积；分母：地面构成的平行四边形的面积；高=体积/面积也就是点到平面的距离。

运动估计

在确定了两个优化目标：边缘点到边缘线的距离、平面点到平面的距离以后，就可以对特征点进行旋转，构建目标函数，通过非线性优化的方式进行梯度下降求解了。
作者论文中使用的求解方法是列文伯格-马尔夸特算法（LM），又叫阻尼牛顿法，可以避免求解中遇到的非奇异和病态问题。

LOAM虽然没有回环检测（不能发现自己到了相同的位置），但这并不代表它仅靠前端递推。 LOAM在建图部分，采用map-map的匹配方法，用连续10帧的激光点云数据，和10立方米之内的地图做一个匹配。也就是说，在第25帧的位姿，不是从0帧-1帧，1帧-2帧……一直递推到第25帧的。而是10～20帧之间构建的点云地图和全局地图匹配得到的第20帧位姿开始递推，从20～21，21～22……直到第25帧。

地图构建和全局位姿

建图部分采用map-map的匹配，仍然是基于边缘点和平面点，投影点云，计算点到直线和点到平面的距离。
里程计的匹配，是用两帧点云数据；
建图的匹配，是用10帧点云数据，和10立方米范围内的整个地图匹配。
特征点增加了10倍！

前端寻找边缘线和平面，使用的是最近临的方法。后端则不同，使用的是特征点周围的点云簇，通过主成分分析（求解协方差矩阵的特征值和特征向量）确定边缘线和平面。

LOAM的优点是：

1.提出了新颖的特征提取方式。

2.根据时间戳，对旋转的雷达采集时间不一致进行运动补偿。

3.融合了scan to scan（里程计部分） map to map（建图部分）的思想。

LOAM的缺点也很明显：

1.没有回环检测。（发表时间较早，比谷歌Cartographer要早两年）

2.不能处理大规模的旋转变换。

KITTI数据集

接下来看看什么是KITTI数据集。更多关于该数据集的，可以参考

KITTI数据采集平台：1个惯性导航系统，1个64线3D激光雷达， 2个灰度摄像机，2个彩色摄像机，以及4个光学镜头。

KITTI数据集主要有以下Benchmark：road（用于道路分割），semantics（用于语义分割），object（2D、3D和鸟瞰三种视角，用于目标检测），depth（用于视觉深度评估），stereo（用于双目立体视觉和三维重建），flow（用于光流预测），tracking（用于目标跟踪），odometry（里程计）。
另外还提供了velodyne64线激光雷达数据。评估标准：平移误差（百分比），旋转度数（度每米）。

A-LOAM

A-LOAM是港科大秦通博士（VINS-Mono一作）在LOAM原有代码基础上，使用Ceres-solver和Eigen库对其进行重构和优化，在保持原有算法原理的基础上，使其可读性大大增加，作为入门多线激光slam最好选择。

先基于作者的github工程（https://github.com/HKUST-Aerial-Robotics/A-LOAM）来构建环境进行数据集测试。

Ceres Solver的安装参考连接Installation — Ceres Solver

安装完后可以测试

bin/simple_bundle_adjuster ../ceres-solver/data/problem-16-22106-pre.txt

而PCL的安装可以参考：

# 安装依赖
sudo apt-get update  
sudo apt-get install git build-essential linux-libc-dev  
sudo apt-get install cmake cmake-gui   
sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev  
sudo apt-get install mpi-default-dev openmpi-bin openmpi-common    
sudo apt-get install libflann1.8 libflann-dev  
sudo apt-get install libeigen3-dev  
sudo apt-get install libboost-all-dev  
sudo apt-get install libvtk5.10-qt4 libvtk5.10 libvtk5-dev  
sudo apt-get install libqhull* libgtest-dev  
sudo apt-get install freeglut3-dev pkg-config  
sudo apt-get install libxmu-dev libxi-dev   
sudo apt-get install mono-complete  
sudo apt-get install qt-sdk openjdk-8-jdk openjdk-8-jre  
# 版本可能需要修改，比如libflann1.9

# 安装OpenNI
git clone https://github.com/OpenNI/OpenNI.git
cd OpenNI
cd Platform/Linux/CreateRedist
./RedistMaker
cd Platform/Linux/Redist/OpenNI-Bin-Dev-Linux-x64-v1.5.7.10
./install.sh

# 下载pcl
git clone `pcl`

# 编译
cd pcl
mkdir build && cd build
cmake BUILD_GPU ON \
	BUILD_CUDA ON \
	BUILD_OPENNI ON \
	BUILD_gpu_kinfu ON \
	BUILD_gpu_kinfu_large_scale ON ..
make -j8

# .so安装到/usr/local/lib中
make install

不过好像直接运行下面命令即可

sudo apt install libpcl-dev

首先测试NSH indoor outdoor数据集

    roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
    rosbag play YOUR_DATASET_FOLDER/nsh_indoor_outdoor.bag

效果如下所示

基于nsh

接下来看看KITTI数据集。感觉下载雷达的数据集测试就对了hh

    roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch
    roslaunch aloam_velodyne kitti_helper.launch

LeGO-LOAM (Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain)

LeGO-LOAM（lightweight and ground optimized lidar odometry and mapping）是Tixiao Shan在原有LOAM基础上，做了一些改进包括:1、对前端里程计的前量化改造，提取地面点更适配水平安装的LiDAR; 2、使用SLAM中的Keyframe概念以及回环检测位姿图优化的方式对后端进行重构。简而言之，其主要在于两点提升：轻量级和地面优化。其发表在IROS2018。

该算法是基于VLP-16激光雷达的，在进行特征提取前，会先进行语义分割（segmentation）。其框架如下所示。

Segmentation：对点云进行分割，分割为地面和非地面区域；
Feature Extraction：基于分割后的点，和LOAM类似的算法提取出边缘点和平面点；
Lidar Odometry：基于提取的特征点，scan-to-scan推算两帧激光之间的相对位姿变换（使用两次LM优化），频率较高（10Hz）；
Lidar Mapping： scan-to-map，构建全局地图，获得世界坐标系下的位姿，频率较低（2Hz）；
Transform Intergration：与LOAM相同，实时利用世界坐标系下的位姿和两个时刻之间的相对位姿，更新各个时刻世界坐标系下的位姿

在提取特征点之前，对点云进行一个聚类操作：低于30个数据点的类别就当成噪声处理，这样保存下来的点就是一些相对比较静态的物体了。

右图为聚类处理后的点云，红色部分被分割为地面点。对前述形成的16*1800的距离图像，水平分成若干个子图像（sub-image），对每一个子图像都进行特征提取操作：依旧是和LOAM中相同的边缘点和平面点评判标准，根据曲率c，区分边缘点和平面点。

之后，要划分两个大集合：

每一行中，选取有最大c值的N_Fme（40个）边缘点，要求不得属于地面点, 组成集合F_me;
每一行中，选取有最小c值的N_Fmp（80个）平面点，组成集合F_mp;

之后，再从F_me和F_mp中，划分两个小集合：

n_fe（2个）边缘点；
n_fp（4个）平面点，要求是地面点。

下图展示了大集合和小集合的区别。

运动估计

雷达里程计部分，要根据两帧点云，确定相对的位姿变换：

从t时刻的小集合中，选取边缘点，和t-1时刻的大集合中的边缘点，构建点到线的关系，构建方法和LOAM一致。
从t时刻的小集合中，选取平面点，和t-1时刻的大集合中的地面点，构建点到面的关系，构建方法和LOAM一致。

构建了目标函数以后，仍然采用LOAM中的列文伯格-马尔夸特优化算法，但是分了两阶段，第一阶段计算竖直维度的z，roll，pitch。（根据地面的平面点来优化）第二阶段计算水平维度的x，y，yaw，根据边缘点来优化。两次LM优化得到相同的精度，实验计算时间减少35%。

地图构建和全局位姿

与LOAM一致，Lego-LOAM同样也是采用了一快一慢两个部分，快的Lidar Odometry 10Hz来计算相对变换，慢的Lidar Mapping 2Hz来计算世界坐标系下的位姿。相当于10Hz在2Hz的基础上递推。

Lego-LOAM不再存储所有的传感器点云数据，而是只存储特征点集。

LOAM采取的是map-to-map的优化，最近的十帧点云与10立方米内的全局地图进行匹配，主成分分析，构建点到线、点到面的误差；

Lego-LOAM采取的是scan-to-map的优化，scan为当前帧点云中的特征点集；map有两种取法：第一种，和LOAM中一致，10立方米；第二种，选一组时间上相近的特征点云，构建图优化问题。当前时刻的特征点云作为观测数据，当前位姿作为优化节点。

使用的优化库：不再是谷歌公司的Ceres，而是因子图优化库gtstam。

回环检测

Lego-LOAM中加入了回环检测，是单独用一个线程运行的。

流程：

第一步，寻找历史位姿：1.与当前位姿的距离上最近。2.与当前位姿间隔时间较长。（这样才是回环）

第二步，把历史位姿作为候选，用ICP算法迭代，修正位姿。

我们可以看出：1.相比Cartographer（激光点云投影到栅格地图，暴力搜索全局地图，分支定界加速，确定初值以后用非线性优化精修），Lego-LOAM的回环检测就是一个简单的回环检测，其默认偏移较小。如果偏移较大，就不能修正了。2.不具备重定位功能。因为这种算法的前提是需要知道自己的大致位置，和历史中附近的位置进行匹配，修正位姿。

Lego-LOAM有一个显著的缺陷——依赖地面。

The system takes in point cloud from a Velodyne VLP-16 Lidar (palced horizontally) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.基于下面的原理来求出6D的pose

接下来根据github工程，配置一下该代码（https://github.com/RobustFieldAutonomyLab/LeGO-LOAM）

这里有个小知识，对于cm一开始采用

catkin_make -j1

When you compile the code for the first time, you need to add "-j1" behind "catkin_make" for generating some message types. "-j1" is not needed for future compiling.

运行方式

roslaunch lego_loam run.launch

Notes: The parameter "/use_sim_time" is set to "true" for simulation, "false" to real robot usage.

rosbag play *.bag --clock --topic /velodyne_points /imu/data

注意：Notes: Though /imu/data is optinal, it can improve estimation accuracy greatly if provided.

关于数据集（https://github.com/TixiaoShan/Stevens-VLP16-Dataset）是采用Velodyne VLP-16来收集的

一些样板数据集（https://github.com/RobustFieldAutonomyLab/jackal_dataset_20170608）

此处，先跑一下上面的A-LOAM的NSH indoor outdoor数据集

当报错[mapOptmization-7] process has died时，可以试试

sudo apt-get install libparmetis-dev

然后再运行即可，详见下面展示

基于nsh

LIO-SAM (Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping)

LIO-SAM 是Tixiao Shan在LeGO-LOAM的扩展，实际上也是Lego-LOAM的扩展版本。添加了IMU预积分因子和GPS因子：前端使用紧耦合的IMU融合方式，替代原有的帧间里程计，使得前端更轻量；后端沿用LeGO-LOAM，在此基础上融入了GPS观测。同时前端后端相互耦合，提高系统精度。LIO-SLAM是一个real-time lidar-inertial odometry package

其框架如下所示。

使用关键帧Keyframe，使用关键帧进行匹配，丢掉了关键帧之间的帧。（阈值设置为1m和10度）。四种因子：
橙色：IMU预积分因子（可参考之前的博客《学习笔记之——VIO与VINS-Mono》）；
绿色：雷达里程计因子，激光“关键帧” 和 “之前的N个关键帧构成的体素地图” 进行匹配

雷达里程计部分主要得到两帧之间的位姿变换；方法基本与LOAM或Lego-LOAM的一致。

区别：仅用关键帧和之前n+1个关键帧中的特征集合构成地图，进行匹配，构建点到线、点到面的约束；（原先是使用帧到帧的匹配）

关于这点，该方法应该没有使用Lego-LOAM的提取地面特征的方式，因为在实验部分，采用手持建图、轮船水面建图，可能不适合Lego-LOAM该方式。

黄色：GPS因子：当估计位姿的方差大于GPS位置方差时加入

收到一个GPS测量，即将其转换到笛卡尔坐标系下，构建一个新的约束；

时间戳不同步问题：插值解决；

当估计的位置的协方差大于GPS位置协方差时，才插入一个约束因子。

黑色：回环检测因子，由关键帧和候选关键帧相邻的2m+1个关键帧帧图匹配得到。该方法使用的回环检测方法，应该和Lego-LOAM中的一致；搜索当前位置15m内的最近历史位置，使用该历史位置的前后分别12个关键帧的特征，和当前匹配，构建约束。

接下来看看https://github.com/TixiaoShan/LIO-SAM按照github工程进行配置。大部分的设置都是前面的几个算法用过的。然后运行

roslaunch lio_sam run.launch
rosbag play your-bag.bag -r 3

对于要跑的数据集，需要设置对应的参数：

Download some sample datasets to test the functionality of the package. The datasets below is configured to run using the default settings:
- Walking dataset: [Google Drive]
- Park dataset: [Google Drive]
- Garden dataset: [Google Drive]
The datasets below need the parameters to be configured. In these datasets, the point cloud topic is "points_raw." The IMU topic is "imu_correct," which gives the IMU data in ROS REP105 standard. Because no IMU transformation is needed for this dataset, the following configurations need to be changed to run this dataset successfully:
- The "imuTopic" parameter in "config/params.yaml" needs to be set to "imu_correct".
- The "extrinsicRot" and "extrinsicRPY" in "config/params.yaml" needs to be set as identity matrices.
  - Rotation dataset: [Google Drive]
  - Campus dataset (large): [Google Drive]
  - Campus dataset (small): [Google Drive]

对应修改如下：

lio_sam:

  # Topics
  pointCloudTopic: "points_raw"               # Point cloud data
  # imuTopic: "imu_raw"                         # IMU data
  imuTopic: "imu_correct"                         # IMU data
  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file

  # Frames
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

  # GPS Settings
  useImuHeadingInitialization: true           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  savePCD: false                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

  # Sensor Settings
  sensor: velodyne                            # lidar sensor type, either 'velodyne' or 'ouster'
  N_SCAN: 16                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used

  # IMU Settings
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03
  imuAccBiasN: 6.4356659353532566e-05
  imuGyrBiasN: 3.5640318696367613e-05
  imuGravity: 9.80511
  imuRPYWeight: 0.01

  # Extrinsics (lidar -> IMU)
  extrinsicTrans: [0.0, 0.0, 0.0]
  # extrinsicRot: [-1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, -1]
  # extrinsicRPY: [0,  1, 0,
  #                -1, 0, 0,
  #                 0, 0, 1]
  extrinsicRot: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]
  extrinsicRPY: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]

  # LOAM feature threshold
  edgeThreshold: 1.0
  surfThreshold: 0.1
  edgeFeatureMinValidNum: 10
  surfFeatureMinValidNum: 100

  # voxel filter paprams
  odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
  mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
  mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor

  # robot motion constraint (in case you are using a 2D robot)
  z_tollerance: 1000                            # meters
  rotation_tollerance: 1000                     # radians

  # CPU Params
  numberOfCores: 4                              # number of cores for mapping optimization
  mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

  # Visualization
  globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density




# Navsat (convert GPS coordinates to Cartesian)
navsat:
  frequency: 50
  wait_for_datum: false
  delay: 0.0
  magnetic_declination_radians: 0
  yaw_offset: 0
  zero_altitude: true
  broadcast_utm_transform: false
  broadcast_utm_transform_as_parent_frame: false
  publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
  publish_tf: false
  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  frequency: 50
  two_d_mode: false
  sensor_timeout: 0.01
  # -------------------------------------
  # External IMU:
  # -------------------------------------
  imu0: imu_correct
  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, true,
                true,  true,  true]
  imu0_differential: false
  imu0_queue_size: 50 
  imu0_remove_gravitational_acceleration: true
  # -------------------------------------
  # Odometry (From Navsat):
  # -------------------------------------
  odom0: odometry/gps
  odom0_config: [true,  true,  true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_differential: false
  odom0_queue_size: 10

  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

demo视频如下：

LIO-SLAM复现（3D LiDAR SLAM）

其节点图如下所示

SC-LIO-SAM

https://github.com/gisbi-kim/SC-LIO-SAM

LVI-SAM (Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping)

LVI-SAM为Lego-LOAM和LIO-SAM作者Tixiao Shan的最新工作，发表在ICRA 2021上。
提出了一个基于图优化的多传感器融合框架，具有多个子系统：视觉惯性子系统（VIS）和雷达惯性子系统（LIS）；单目+雷达+imu融合
鲁棒性：任一子系统失效，不会导致整个系统挂掉。

这是一个基于视觉与激光雷达的系统。对于现有的方法而言，单纯视觉或者激光雷达都有各自的不足。

基于激光的方法，在结构比较简单的环境里，容易失效——因为特征只有一个简单的“距离”信息；
基于视觉的方法，容易受到光照改变、快速运动导致的图像模糊等问题；所以，一般会加入IMU传感器——但IMU具有bias，其估计的并不是很准。

其框架如下所示。LVI-SAM使用了两个子系统，视觉惯性系统（VIS）和雷达惯性系统（LIS）：

1.视觉惯性系统用雷达惯性系统来初始化；——用LIS计算的位姿，作为VIS的初始优化值；

2.VIS中需要根据图像计算特征的深度，可以利用雷达的测量数据来辅助优化；

3. LIS计算两个点云位姿变换的优化初值，同样也可以用VIS的视觉估计来计算；

4. 回环检测可以用视觉信息确定初值，再用雷达数据优化。

视觉-惯性框架

雷达-惯性框架

而对于回环检测部分。LVI所采用的回环检测，与大多数视觉SLAM所采取的回环检测方法相同，为词袋模型。

接下来复现该demo，参考github工程GitHub - TixiaoShan/LVI-SAM: LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

编译出错的时候换一下eigen库（ubuntu安装eigen3.3.7_reason的博客-CSDN博客）

查看已安装版本

gedit /usr/local/include/eigen3/Eigen/src/Core/util/Macros.h

发现是空的。。。好像这个解决办法不太靠谱，换一个（LOAM-Livox算法编译过程中出现的问题及解决方案_LacyExsale的博客-CSDN博客）。具体报错如下，应该是ceres的版本不对？

error: ‘ScalarBinaryOpTraits’ is not a class template
 struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> {
        ^~~~~~~~~~~~~~~~~~~~
/usr/local/include/ceres/jet.h:1027:58: error: type/value mismatch at argument 3 in template parameter list for ‘template<class BinaryOp, class T, int N> struct Eigen::ScalarBinaryOpTraits’
 struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> {
                                                          ^
/usr/local/include/ceres/jet.h:1027:58: note:   expected a constant of type ‘int’, got ‘BinaryOp’

之前安装的是2.0.0版本（查看/home/kwanwaipang/ceres-solver/package.xml里面的ersion）试试重新配置

sudo apt-get install -y libgoogle-glog-dev
sudo apt-get install -y libatlas-base-dev
wget -O ~/下载/ceres.zip https://github.com/ceres-solver/ceres-solver/archive/1.14.0.zip
cd ~/下载/ && unzip ceres.zip -d ~/下载/
cd ~/下载/ceres-solver-1.14.0
mkdir ceres-bin && cd ceres-bin
cmake ..
sudo make install -j4

然后再编译，老报错

c++: internal compiler error: 已杀死 (program cc1plus)

cm几次终于成功了hhhh

运行

roslaunch lvi_sam run.launch
rosbag play handheld.bag

demo如下所示：

LVI

其节点图如下所示

LOAM_livox（固态激光雷达）

其为2019年开源的工作。是基于固态激光雷达的。接下来先看看固态雷达和机械雷达的区别：

机械雷达：通过机械方式改变扫描方向；类似于一个激光笔，放在电机上旋转；十六线雷达，就是依次改变扫描的角度和高度，进行旋转。

固态雷达：通过阵列干涉改变扫描方向。类似于一个激光笔自己不动，改变激光射出的方向和角度。

这论文的理论部分先不看，先直接开始试验，后面补充博文研究~

接下来按照github工程进行配置：https://github.com/hku-mars/loam_livox

Loam-Livox is a robust, low drift, and real time odometry and mapping package for Livox LiDARs

下载好功能包后，编译，然后运行下面命令

roslaunch loam_livox rosbag.launch
rosbag play YOUR_DOWNLOADED.bag

此处，先跑一下上面的A-LOAM的NSH indoor outdoor数据集

好像不可以。。。。是因为本包是基于Livox LiDARs吧？

然后再看看该工程在hku以及hkust环境下运行的结果(注意前面的A-LOAM无法在此数据集下运行，估计是包的数据不一样或者是采集的数据结构不一样？)

基于HKUST数据集的LOAM

CCM- SLAM

除了激光雷达slam外，本博文也跑了一下CMM-SLAM，基于单目camera，多个机器人的collaboration SLAM。

CCM（centralized，collaborative，monocular cmaera）

首先编译两个库

进入ccm_slam/cslam/thirdparty/DBoW2/然后运行

mkdir build
cd build
cmake ..
make -j8

进入g2o然后运行

cd ~/ccmslam_ws/src/ccm_slam/cslam/thirdparty/g2o
mkdir build
cd build
cmake --cmake-args -DG2O_U14=0 ..
make -j8

Unzip Vocabulary:

cd ~/ccmslam_ws/src/ccm_slam/cslam/conf
unzip ORBvoc.txt.zip

直接在当前工作空间做好像不行。。。新建一个工作空间试试～

按照github里面的教程步骤走即可（注意若为ubuntu18.04需要将里面的ros版本改为melodic）

编译的时候如果出现报错，可以修改其中的cpp文件

/home/kwanwaipang/ccmslam_ws/src/ccm_slam/cslam/src/KeyFrame.cpp

// size_t KeyFrame::nNextId=0;
long unsigned int  KeyFrame::nNextId=0;

然后再编译一下就成功了～

注意每次运行都应该先source一下工作空间

source ~/ccmslam_ws/devel/setup.bash

通过下面命令开启服务

roslaunch ccmslam Server.launch

写一个sh文件如下

#! /bin/bash

###############################################################
gnome-terminal -t "server" -x bash -c "roslaunch ccmslam Server.launch;exec bash;"
sleep 15

gnome-terminal -t "agent1" -x bash -c "roslaunch ccmslam Client0_euroc.launch;exec bash;"
gnome-terminal -t "agent2" -x bash -c "roslaunch ccmslam Client1_euroc.launch;exec bash;"
gnome-terminal -t "agent3" -x bash -c "roslaunch ccmslam Client2_euroc.launch;exec bash;"
gnome-terminal -t "agent4" -x bash -c "roslaunch ccmslam Client3_euroc.launch;exec bash;"


sleep 15
gnome-terminal -t "rviz" -x bash -c "cd /home/kwanwaipang/ccmslam_ws/src/ccm_slam/cslam; rviz -d conf/rviz/ccmslam.rviz;exec bash;" 
#如果roscd报错的话就直接用cd

sleep 15
gnome-terminal -t "agentbag1" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_01_easy.bag --start 45;exec bash;"  
#cd 数据集包所在的位置
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_02_easy.bag --start 35 /cam0/image_raw:=/cam0/image_raw1;exec bash;" 
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_03_medium.bag --start 15 /cam0/image_raw:=/cam0/image_raw2;exec bash;" 
gnome-terminal -t "agentbag2" -x bash -c "cd /home/kwanwaipang/下载;rosbag play MH_04_difficult.bag --start 15 /cam0/image_raw:=/cam0/image_raw3;exec bash;"

EuRoC dataset数据集下载

kmavvisualinertialdatasets – ASL Datasets

实验视频结果如下：

(ccmslam)基于三个agents的单目SLAM地图融合