SfM、VO和SLAM介绍

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

前言

    在自动驾驶地图定位模块的功能开发中，主要研究方向分为三种：建图、里程计、定位。SfM（Structure From Motion）即 传统三维重建，它更注重的是建图；VO（Visual Odometry）或VIO（Visual-Inertial Odometry）要解决的问题就是里程计问题，与机器人领域不同，在实际自动驾驶项目中，里程计部分是由惯导，轮速计，视觉，激光等多传感器融合完成；SLAM（Simultaneous Localization And Mapping）同时建图与定位，它更注重的是定位。

SfM，运动恢复结构

    顾名思义，SfM是通过相机的运动来进行三维重建。这里，需要用到对极几何的理论知识，在我的博客视觉里程计-对极几何中，有简单的入门介绍。这里，简单讲述一下SfM的流程。

• 特征检测与匹配。在我的博客视觉SLAM–ORB特征简介中，介绍了多种特征点以及特征匹配的实例代码。
• 对每一对完成特征匹配的图像，估计基础矩阵F或本质矩阵E。
• 根据本质矩阵分解得出R，t，估计相机运动。
• 三维点云计算。得出两帧图像的相对运动后，利用三角化得出特征点的深度。
• 计算多帧图像的重投影误差。将同一特征点的三维信息坐标转换至图像坐标系下，确定该值与实际图像感知的点的距离差。
• BA优化。利用BA同时优化地标位置与相机位姿，解决非线性优化问题，可用ceres，类似方法可以参考我的博客Ceres解决实车SLAM后端优化问题

视觉里程计

    视觉里程计主要有两大方法：直接法和特征点法。
特征点法：

• 当相机为单目时，我们只知道 2D 的像素坐标，因而问题是根据两组 2D 点估计运
  动。该问题用对极几何来解决。
• 当相机为双目、RGB-D 时，或者我们通过某种方法得到了距离信息，那问题就是根
  据两组 3D 点估计运动。该问题通常用 ICP 来解决。
• 如果我们有 3D 点和它们在相机的投影位置，也能估计相机的运动。该问题通过 PnP
  求解。

    在实际自动驾驶项目中，作者用到的最多的方法是ICP，在我的博客多种形式ICP问题的ceres实例应用中，提供了多种作者在实际应用中的ICP方法的C++代码。
    虽然特征点法在视觉里程计中处于主流地位，但同时它也有一些不足：

• 关键点的提取与描述子的计算非常耗时。
• 使用特征点时，忽略了除特征点以外的所有信息。针对相邻帧的运动估计而言，这样做会丢失了大部分可能有用的图像信息。
• 相机有时会运动到特征缺失的地方，往往这些地方都没有什么明显的纹理信息。这里额外提一句，在实际项目中，在这样没有明显特征的地方，正如上文提到的，可以利用IMU与轮速计进行航位推算。

    针对上述问题，直接法直接根据像素亮度信息，估计相机的运动，可以完全不用计算关键点和描述子。于是，直接法既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像特征），直接法就能工作。
    直接法是从光流演变而来的。光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。在直接法中，有一个重要假设，灰度不变假设，同一个空间点的像素灰度值，在各个图像中是固定不变的。而这也是直接法的关键不足的地方。

SLAM

    SLAM问题的本质是对主体自身（位姿）和周围环境空间（点云）的不确定性的估计。它要求解决是同步建图与定位问题，重点是定位。

1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。在自动驾驶车辆中，还有轮速计、惯性传感器等信息的读取和同步。
2. 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。VO 又称为前端（Front End）。
3. 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后，又称为后端（Back End）。
4. 回环检测（Loop Closing）。回环检测判断车辆是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
5. 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。

在这里，会发现SLAM与SfM本质其实是一样的，都是有特征检测，运动估计，非线性优化误差求解位姿地标等。区别的话，主要有两点：

• SLAM是有序的图像序列，一般通过前后帧间匹配和局部窗口法，只有回环检测时需要进行图像间的匹配；基于图像的SfM不要求实时，数据是无序的，可以一次输入所有图像，利用所有信息。
• SLAM是个动态问题，会涉及到滤波，运动学相关的知识，而SfM主要涉及的还是图像处理的知识。

实际问题解决方案

作者在实际自动驾驶SLAM问题中，更推荐的方法还是基于深度学习完成图像语义特征的检测，将特征转换为三维信息后，利用ICP估计位姿变化。具体方案可以结合我之前对两篇论文的阅读心得，分别是
停车场场景【论文阅读】AVP-SLAM Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot
大范围的城市场景【论文阅读】RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving
自己利用激光雷达处理的大场景问题大范围室外环境三维激光点云SLAM建图。