table of Contents
Note: The original is not easy, reproduced, please be sure to indicate the source and author, thanks for the support!
This lesson on:
(1) the visual sensor SLAM
(2) Visual SLAM framework classical
mathematical formulation of the problem of SLAM (3)
A visual sensor of SLAM
Imagine a indoor mobile robots in the freedom to explore the indoor environment, then localization and mapping may be intuitively understood as:
(1) Where am I? - Positioning
(2) What is the environment? - Construction Plan
And to complete the positioning and construction plans need to support a variety of sensors. Sensor generally be divided into two categories, one sensor is mounted on the robot body, and the other sensor is installed in the environment.
(1) mounted on a robot body sensor
wheel encoder
camera
laser sensor
IMU inertial measurement unit
(2) a sensor mounted in the environment
the GPS device
QR code
guide
Sensor is installed in the environment, though simple and reliable, but they limit the use range of the robot in a certain extent. To get a general, generic SLAM programs, more still have to rely on sensors mounted on the robot body.
Visual SLAM, the vision sensor is used: monocular camera, the camera and binocular RGB-D camera (depth camera). Monocular camera advantage of simple structure, low cost, has dimensions disadvantage is uncertain. Binocular camera can calculate the depth of each pixel by two left and right eye images, but there are complex calibration computational cost disadvantage. RGB-D camera can be directly obtained by the pixel depth physical means, but there is a narrow measuring range, a large noise, small field of view and other shortcomings.
Two classic visual SLAM framework
整个视觉SLAM流程包括以下步骤：
（1） 传感器信息读取，视觉SLAM中主要是相机图像信息的读取和预处理。可能还有其它的传感器，比如码盘，IMU等信息等
（2） 视觉里程计（Visual Odometry, VO），视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又被称为前端
（3） 后端优化（Optimization），后端优化接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于在VO之后，又被称为后端。
（4） 回环检测（Loop Closing Detection），回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
（5） 建图（Mapping），它根据估计的轨迹，建立与任务要求对应的地图。
VO能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。称它为里程计是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的过去的信息没有关系。然而，光靠VO是没法完成整个SLAM任务的。仅通过VO来估计轨迹，将不可避免地出现累积漂移。这是由于VO在最简单的情况下，只估计两个图像间的运动造成的。
回环检测主要解决位置随时间漂移的问题。为了实现回环检测，我们需要让机器人具有识别到过的场景的能力。我们希望机器人能使用携带的传感器——也就是图像本身来完成这一任务。例如，我们可以判断图像间的相似性来完成回环检测。所以视觉回环检测实质上是一种计算机图像数据和相似性的算法。
笼统地说，后端优化主要指处理SLAM过程中的噪声的问题。现实中，再精确的传感器也带有一定的噪声。后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这被称为最大后验概率估计（MAP）。这里的状态包括了机器人自身的轨迹，也包含地图。
建图（Mapping）是指构建地图的过程。地图可以分为度量地图和拓扑地图两种。度量地图强调精确地表示地图中物体的位置关系，通常可分为稀疏（Sparse）和稠密（Dense）两种。
稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。而稠密地图着重于建模所有看到的东西。对于定位，稀疏路标地图就足够，而用于导航则需要稠密地图。
相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如A，B点是连通的，而不考虑如何从A点到达B点。
三 SLAM问题的数学表述
详细内容请参考《视觉SLAM十四讲》。
考虑一段连续时间里的\(K\)个离散时刻\(t=1, 2, ..., K\)。用\(x\)表示机器人自身的位置，则各个时刻的标记为\(x_1, x_2, ..., x_K\)。设路标共有\(N\)个，分别为\(y_1, y_2, ..., y_N\)。
考虑下面两个问题：
（1） 什么是运动？所谓运动就是考虑机器人从\(k-1\)时刻到\(k\)时刻，机器人的位置\(x_k\)是如何变化的，如何用数学表示这一过程。
（2） 什么是观测？假设机器人在\(k\)时刻于位置\(x_k\)处探测到了某一个路标\(y_j\)，如何用数学表示该事件。
对于问题（1），我们可以使用一个通用的，抽象的数学模型来表示
\[ x_k = f(x_{k-1}, u_k, w_k) \]
其中，\(u_k\)是运动传感器的读书，\(w_k\)为噪声。\(f\)为一个通用的一般函数。这个方程被称为运动方程
对于问题（2），我们可以用一个观测方程来表示，该方程描述的是：当机器人在\(x_k\)位置上观测到某个路标点\(y_j\)，产生了一个观测数据\(z_{k,j}\)。同样，用一个一般的通用函数\(h\)表示这个关系，其中\(v_{k,j}\)表示观测噪声。
\[ z_{k,j} = h(y_j, x_k, v_{k,j}) \]
下面举一个具体的参数化例子。
假设机器人在平面中运动，其位姿由两个位置和一个转角来表示，即\(x_k=[x,y, \theta]_k^T\)。同时，运动传感器能够测量到机器人在任意两个时间间隔的位置和转角变化量\(u_k=[\Delta x,\Delta y, \Delta \theta]_k^T\)，于是，运动方程可以具体化如下
\[ \begin{bmatrix}x\\y\\ \theta \end{bmatrix}_k = \begin{bmatrix}x\\y\\ \theta \end{bmatrix}_{k-1} + \begin{bmatrix}\Delta x\\ \Delta y\\ \Delta \theta \end{bmatrix}_k + w_k \]
SLAM过程可以总结为两个基本方程：
\[ x_k = f(x_{k-1}, u_k, w_k)\\ z_{k,j} = h(y_j, x_k, v_{k,j}) \]
对于观测方程，假设机器人携带了一个二维激光传感器，二维激光传感器观测一个2D路标时，能够测量到两个量：路标点与机器人本体之间的距离\(r\)和夹角\(\phi\)。记路标点为\(y=[p_x, p_y]^T\)（下标已忽略），观测数据为\(z=[r, \phi]^T\)，那么观测方程为
\[ \begin{bmatrix}r\\ \phi\end{bmatrix} = \begin{bmatrix}\sqrt{(p_x-x)^2 + (p_x-x)^2}\\ arctan(\frac{p_y-y}{p_x-x})\end{bmatrix} + v \]
对于简单的线性高斯系统（LG系统），其无偏最优估计可以由卡尔曼滤波（KF）给出。
对于复杂的非线性非高斯系统（NLNG系统），我们一般使用扩展卡尔曼滤波器（EKF）和非线性优化方法求解。时至今日，主流视觉SLAM使用以图优化的优化方法进行状态估计。