slam入门学习笔记

       SLAM是Simultaneous localization and mapping缩写，意为“同步定位与建图”，主要用于解决机器人在未知环境运动时的定位与地图构建问题，目前广泛用于机器人定位导航领域，VR/AR方面，无人机领域，无人驾驶领域等。

（1）slam框架流程

SLAM系统框架如图所示，一般分为五个模块，包括传感器数据、视觉里程计、后端、建图及回环检测。

• 传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。
• 视觉里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。Vo（又称为前端）能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构，称它为里程计。被称为里程计是因为它只计算相邻时刻的运动，而和再往前的过去信息没有关联。相邻时刻运动串联起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，根据每一时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。
• 后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。后端优化主要是处理slam过程中噪声的问题。任何传感器都有噪声，所以除了要处理“如何从图像中估计出相机运动”，还要关心这个估计带有多大的噪声。前端给后端提供待优化的数据，以及这些数据的初始值，而后端负责整体的优化过程，它往往面对的只有数据，不必关系这些数据来自哪里。在视觉slam中，前端和计算接视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波和非线性优化算法。
• 建图：主要是根据估计的轨迹建立与任务要求对应的地图，在机器人学中，地图的表示主要有栅格地图、直接表征法、拓扑地图以及特征点地图这4种。而特征点地图是用有关的几何特征（如点、直线、面）表示环境，常见于视觉SLAM技术中。这种地图一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。
• 回环检测：主要用于空间累积误差消除，是指机器人识别曾到达场景的能力。如果检测成功，可以显著地减小累积误差。回环检测实质上是一种检测观测数据相似性的算法。对于视觉SLAM，多数系统采用目前较为成熟的词袋模型（Bag-of-Words, BoW）。词袋模型把图像中的视觉特征（SIFT, SURF等）聚类，然后建立词典，进而寻找每个图中含有哪些“单词”（word）。也有研究者使用传统模式识别的方法，把回环检测建构成一个分类问题，训练分类器进行分类。

其工作流程大致为：

传感器读取数据后，视觉里程计估计两个时刻的相对运动（Ego-motion），后端处理视觉里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。

（2）slam类型特性

下面根据常见的激光slam和视觉slam特性进行如下总结：

• 激光slam：

激光SLAM采用2D或3D激光雷达（也叫单线或多线激光雷达），2D激光雷达一般用于室内机器人上（如扫地机器人），而3D激光雷达一般使用于无人驾驶领域。激光雷达的出现和普及使得测量更快更准，信息更丰富。激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。激光雷达测距比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。激光SLAM理论研究也相对成熟，落地产品更丰富。

• 视觉slam

早期的视觉SLAM基于滤波理论，其非线性的误差模型和巨大的计算量成为了它实用落地的障碍。近年来，随着具有稀疏性的非线性优化理论(Bundle Adjustment)以及相机技术、计算性能的进步，实时运行的视觉SLAM已经不再是梦想。视觉SLAM的优点是它所利用的丰富纹理信息。例如两块尺寸相同内容却不同的广告牌，基于点云的激光SLAM算法无法区别他们，而视觉则可以轻易分辨。这带来了重定位、场景分类上无可比拟的巨大优势。同时，视觉信息可以较为容易的被用来跟踪和预测场景中的动态目标，如行人、车辆等，对于在复杂动态场景中的应用这是至关重要的。

（3）slam涉及的知识点

无论是视觉slam还是激光slam,其中都涉及到了相邻帧之间的匹配，然后根据匹配结果估算其相对位姿。

》相邻帧图像

拿到相邻帧的图像（保证存在一定的共视区域即可），通过特征提取算法（比如sift,suft，orb,甚至superpoint等）提取图像特征点，然后结合特征描述子和ransac完成图像特征匹配或者直接借助superglue等匹配网络完成特征匹配，通过匹配结果计算得到相机的位姿信息。具体计算位姿的过程如下：

通过相邻两张图像计算相机的位姿，指的就是求解旋转矩阵和平移向量，它们一起描述了从第一张图像到第二张图像的相机的相对运动关系（也就是位姿）。这可以通过计算图像对之间的几何关系实现，通常涉及以下矩阵：

如果是基于假设场景中的点都在同一个平面上或接近于一个平面，还可以计算单应矩阵，它描述了从一个平面在一个视角投影到另一个视角的映射关系。因此，如果我们假设场景满足这个条件，可以通过单应性矩阵提取相机的位姿（旋转和平移）。

这样拿到本质矩阵后就可以通过奇异值分解和几何约束计算得到位姿信息了。但需要注意这里的单应矩阵H和基础矩阵F不是一回事，两者通过下面方式计算

当获取到相机的位姿信息后，可以通过三角测量法（Triangulation）来恢复三维空间中的点，从而构建空间地图。下面是详细步骤，包括相机的位姿（旋转和平移）如何用于恢复空间中的三维点。

由于噪声等存在上述计算等式不一定成立，所以更常见的做法是求最小二乘解而不是零解。通过上述三角测量步骤，可以得到每个匹配点的三维坐标。随着多个图像帧的处理，逐渐重建出整个场景的三维点云。具体过程如下：

1. 逐帧三角测量：每处理一对或多对图像帧，将其中匹配的特征点通过三角测量恢复三维位置。
2. 点云累积：将每一帧计算出的三维点云累积到一个整体点云地图中，构建出场景的空间地图。
3. 稀疏点云到稠密点云：通常初步得到的是稀疏的三维点云，可以通过进一步的密集匹配、视差估计或深度图技术将稀疏点云扩展为稠密点云，从而生成更完整的三维场景。

在完成初步三角测量并构建稀疏点云后，可以使用Bundle Adjustment（捆绑调整）来优化相机位姿和三维点的位置。它通过最小化所有视角下的重投影误差，进一步调整和优化三维点云的精度和相机的姿态。至此整个slam构建过程结束。

》相邻帧点云

针对相邻的两帧点云数据通过滤波去燥等预处理后，利用3D SIFT特征点提取与匹配或者NDT或基于随机采样一致性（RANSAC）的全局配准方法得到一个粗匹配结果计算得到初始的位姿，然后在此基础上利用ICP等局部优化方法得到一个较好的位姿变换，同时在多帧点云配准的场景下，通常会通过图优化方法（如g2o、Ceres等）来对多个帧的位姿进行全局优化，以减少累积误差。在较大的场景中，回环检测可以识别已经经过的场景，并进行全局优化，进一步减少漂移。

在相邻帧匹配的基础上，逐帧累积相对位姿变换，得到每一帧的全局位姿，即当前帧相对于初始帧（或世界坐标系）的位姿。将每一帧转换到全局坐标系下的点云逐帧融合，生成整体的点云地图。常见的地图构建方式有以下几种：

>直接累积点云

• 点云拼接：将每一帧的点云直接累加到全局地图中。这种方式简单直接，每一帧点云转换到全局坐标系后，直接将所有点加入到地图中。
• 问题：这种方式会逐渐增加地图的大小，且由于每帧点云可能包含一定的噪声或位姿误差，累积误差可能会导致地图的失真。

>体素网格（Voxel Grid）

• 滤波融合：在每帧点云加入到全局地图前，先对其进行体素滤波（Voxel Grid Filter），将点云稀疏化，减少点的密度，从而降低存储空间。

• 优点：可以显著减少数据量，同时保持足够的地图精度。

• 步骤：

>OctoMap

• 八叉树地图：OctoMap 是一种基于八叉树的数据结构，它能够高效地构建和存储三维环境地图。通过对点云进行体素化处理，OctoMap 可以生成占据图，标记出环境中的占据空间和空闲空间。

• 适用场景：OctoMap适合需要构建稀疏占据图的场景，尤其在无人机、机器人导航等应用中非常常用。

在SLAM中，由于位姿计算中不可避免的累积误差，长时间运行后可能会导致地图的漂移和不准确。通过回环检测可以识别出已经经过的场景，并通过全局优化减少累积误差。同时将每一帧的位姿作为图中的节点，相邻帧之间的相对位姿作为图的边，通过图优化算法（如g2o、Ceres等），对整个地图的位姿进行全局优化。通过检测已访问的相同位置，调整全局位姿，消除长时间运行产生的漂移误差。如果需要构建动态环境下的地图，可能需要定期更新点云地图。可以使用类似 OctoMap 的更新策略，定期清除过时的点云数据，并融合新的点云帧。