视觉SLAM（一）

思维导图

视觉SLAM要解决的问题

用 x 表示车辆位置，各时刻位置就记为 x1，x2…xk，构成了车辆运动轨迹。地图由许多个路标组成，而每个时刻传感器会测量到一部分路标，得到观测数据z。设路标共有 N 个用 y1，y2…yn 表示。通过运动测量 u 和传感器测量量 z 来定位 (估计x)和建图 (估计y)

形式化的表述（类似卡尔曼滤波的描述）：

这里 u_k 是运动传感器的读数（有时也可以作为输入），w_k 为噪声。我们用一个一般函数 f 来描述运动过程，而不具体指明 f
当车辆在 x_k 位置上看到某个路标点 y_j，产生了一个观测量 z_k,j。同样，我们用一个抽象的函数 h 来描述这观察关系（后面的相机针孔模型）

经典SLAM框架

• 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动， 以及局部地图的样子。VO
• 又称为前端（Front End）。 后端优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回
• 环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后， 又称为后端（Back End）。 回环检测（Loop
  Closing）。回环检测判断机器人是否曾经到达过先前的位置。如果 检测到回环，它会把信息提供给后端进行处理。
• 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图

针孔相机模型

设 P 的坐标为 [X, Y, Z]，P′ 为 [X′, Y ′, Z′]，并且物理成像平面到小孔的距离为 f（焦距），根据三角形相似关系有：

为了式子简洁把负号去掉，归一化成像模型，形象描述如下：
得到：
不过，在相机中，我们最终获得的是像素，这需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，设在物理成像平面上固定着一个像素平面 o − u − v。我们在像素平面得到了 P′ 的像素坐标：[u, v]。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。设像素坐标在 u 轴上缩放了 α 倍，在 v 上缩放了 β 倍。同时，原点平移了 [cx, cy]。那么，P′ 的坐标与像素坐标 [u, v]的关系为：把针孔模型带入，f 的单位为米，α, β 的单位为像素每米，所以 fx, fy 的单位为像素，得到：
进一步，设P_w为世界坐标系下的坐标，而相机存在旋转（R）和平移（t），带入到上式，得：
总结一下，上面的式子从观察方程的角度来描述问题，其中P_uv, Pw, K已知，求解R/t即相机的运动

畸变

针孔模型中，我们不考虑畸变，一条直线投影到像素平面还是一条直线。可实际，因为透镜的形状/传感器安装位置等原因会产生畸变/切向畸变
径向畸变，图像放大率随着离光轴的距离增加而减小或变大，下图：

这类畸变可以用和距中心距离有关的二次及高次多项式函数进行描述：

切向畸变，相机组装过程中不能使得透镜和成像面严格平行，如下图：

这类畸变可以二次及联合多项式函数进行描述：
联立上面两个式子得：

双目相机

根据图像视差估计像素深度，如下图：

OL, OR 为左右光圈中心，蓝色框为成像平面，f 为焦距。UL 和 UR 为成像平面的坐标。请注意按照图中坐标定义，UR 应该是负数，所以图中标出的距离为 −UR
根据相似三角形 P −PL −PR 和 P −OL −OR关系：

根据上式，我们可以估计像素点的深度。同样双目深度存在一个理论上最大值，由 fb 确定，当基线（b）越长，双目最大能测到的距离就越远。

RGBD相机

RGB-D 相机按原理可分为两大类，结构光（一般是红外光）/ToF（Time-of-flight）
在结构光原理中，相机根据返回的结构光图案，计算物体离自身的距离。而在 ToF
中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体离自身的距离，如下图：

特征点法-ORB

此章节要解决的问题是，从图像中选取比较有代表性的点用来进行位姿估计。在位姿估计的问题下，特征点的评价指标：

• 可重复性（Repeatability）：相同的“区域”可以在不同的图像中被找到。
• 可区别性（Distinctiveness）：不同的“区域”有不同的表达。
• 高效率（Efficiency）：同一图像中，特征点的数量应远小于像素的数量。
• 本地性（Locality）：特征仅与一小片图像区域相关。

经典的图像特征点抽取方法有：SIFT,SURF等；特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。 当前ORB SLAM中使用特征点 = Oriented FAST(关键点) + Rotated BRIEF（描述子） 提取 ORB 特征分为两个步骤：

• FAST 关键点提取：找出图像中的” 角点”。思想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是关键点

  1. 在图像中选取像素 p，假设它的亮度为 Ip。
  2. 设置一个阈值 T(比如 Ip 的 20%)。
  3. 以像素 p 为中心, 选取半径为 3 的圆上的 16 个像素点。
  4. 假如选取的圆上，有连续的 N 个点的亮度大于 Ip + T 或小于 Ip − T，那么像素 p
     可以被认为是特征点 (N 通常取 12，即为 FAST-12。其它常用的 N 取值为 9 和 11，
     他们分别被称为 FAST-9，FAST-11)。
  5. 循环以上四步，对每一个像素执行相同的操作。
     
     Oriented FAST：FAST 角点不具有方向性和尺度的弱点，ORB 添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法（Intensity Centroid）实现

• BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述。它的描述向量由许多个 0 和 1 组成，0 和 1 编码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就取 0
  

• 特征匹配：确定两帧图片特征点是否对应
  最简单的特征匹配方法就是暴力匹配（Brute-Force Matcher）。即对每一个特征点 x(t)_m与所有的 x(t+1)_n计算特征点对应描述子的距离，然后排序，取最近的一个作为匹配点。而对于 BRIEF 这样的描述子，我们往往使用汉明距离（Hamming distance）做为度量——两个二进制串之间的汉明距离。另，快速近似最近邻（FLANN）算法更加适合于匹配点数量极多的情况。该匹配算法理论已经成熟，且实现上也已集成到OpenCV
  

2D-2D对极几何

前面通过特征点法拿到了两帧图片的点，即同一个空间点在两个成像平面上的投影，本章通过对极几何对R/t进行估算。以下图为例两帧图像 I1, I2设第一帧到第二帧的运动为R, t。两个相机中心为 O1, O2。现在，考虑 I1 中有一个特征点 p1，它在 I2 中对应着特征点 p2，如下图：

O1, O2, P 三个点可以确定一个平面，称为极平面（Epipolar plane）。O1O2 连线与像平面 I1, I2 的交点分别为 e1, e2。e1, e2，称为极点（Epipoles），O1O2 被称为基线（Baseline）。称极平面与两个像平面 I1, I2 之间的相交线 l1, l2 为极线（Epipolar line）。
设 P 的空间位置为：P = [X, Y, Z]，K 为相机内参矩阵，R, t 为两个坐标系的相机运动，根据前面的相机模型有：
以此为起点进行对极约束推导：

它的几何意义是 O1, P, O2 三者共面。定义基础矩阵F和本质矩阵E进一步简化对极约束如下：

到此，我们总结一下求解问题，根据配对点的像素位置，求出 E 或者 F，根据 E 或者 F，求出 R, t

参考资料：

《视觉slam十四讲》，感谢高翔老师