去掉左边0_SLAM从0到1——11. 视觉里程计VO内容框架

「本文是之前学习VO 部分整理的思维导图，笔记写入之后均折叠了起来，正文中采用markdown格式展开，可看到笔记内容」

放上来的目的其实是方便自己查阅笔记，同样给有需要的同学提供一点思路。

整体上的结构分为5部分，包括常用特征点介绍、运动估计、深度估计、光流法、直接法。按照这个顺序可以比较好的回忆和串联内容，建议的阅读方式为：主要看思维导图，遇到不清楚的地方去下文找笔记和具体内容。

# 视觉里程计VO## 常见特征点### ORB
- 关键点：Oriented FAST
- 检测亮度变化明显处
- 步骤
- ①像素p_{i}的亮度I_{p_{i}}
- ②阈值T
- ③半径=3，16个像素点
- ④预处理：1,5,9,13至少三个满足阈值条件
- ⑤连续12个像素点满足阈值条件
- ⑥灰度质心计算方向
- 描述子：BRIEF（二进制）
- ORB特征点提取
- 1. 提取FAST角点
- 方向问题：计算灰度质心
- 尺度问题：构建图像金字塔
- 2. 计算BRIEF描述子
- 特征点匹配
- 二进制描述子
- 汉明距离
<2*最小距离 || < 阈值
- 浮点类描述子
- 欧氏距离### SIFT
尺度不变特征变换(Scale invariant feature Transform,SIFT)https://blog.csdn.net/songzitea/article/details/13627823
- 原理相关
- 高斯差分 DOG
- 对各层图像进行不同程度的高斯滤波后形成DOG金字塔
- 特征的特点
- 具有图像尺度
- 具有旋转不变形
对应着下方求关键点方向：为每一个特征点都计算了方向，后面的操作都在各个点的方向上进行，保证了旋转不变形
- 对光照变化也具有一定程度的不变性
- 目标是解决低层次特征提取
- 可减少低由遮挡、杂乱和噪声引起的低提取概率
- 缺点
- ①实时性低
- ②特征点少
- ③对边缘光滑的图像有时无能为力
- 步骤
可以看到计算量很大，因此实时性不容易满足
SIFT算法建立一幅图像的金字塔，在每一层进行高斯滤波并求取图像差(DOG)进行特征点的提取
- 1. 尺度空间极值检测
- ①由原图像生成图像金字塔
- ②关键点初步探查
非极大值抑制
通过检测不同层之间的图像的极值点来进行关键点的初步探查。
过程很简单，就是与周围的像素点进行比较，当大于或小于所有相邻点时，即为极值点。下图中的X不仅要跟周围的O进行比较，还要跟上一层和下一层的9x2个O进行比较，才能确定是否是极值点。
- ③拟合真正极值点
使用泰勒展开式
- 2. 关键点定位
- ①去除低对比度特征点
- ②去除不稳定边缘相应点
- 3. 求取特征点方向
利用关键点邻域像素的梯度方向分布特性，我们可以为每个关键点指定方向参数方向，从而使描述子对图像旋转具有不变性，我们通过求每个极值点的梯度来为极值点赋予方向。
对于在DOG金字塔检测到的关键点，采集所在图像3σ邻域窗口内像素的梯度和方向特征，并进行统计。取幅值最高的方向为主方向，超过峰值百分之80的方向为辅方向。
- 4. 求描述子描述关键点
为了保证特征向量具有旋转不变性，需要将坐标轴旋转到关键点的方向。
- 5. 生成特征匹配点
将特征点邻域分为几个区块，计算八个方向的梯度方向直方图。这里有16个区域，所以生成了16x8=128个维度的数据。在进行统计之前，还要进行一次高斯加权，特征点附近的区域权值大，相反权值小。### SURF
Speeded Up Robust Featureshttps://blog.csdn.net/songzitea/article/details/16986423
- 与SIFT相似
- 是SIFT加强版，同时加速的具有鲁棒性的特征
- SIFT：较稳定，检测特征点更多，但复杂度较高
- SURF：运算简单，效率高，运算时间短
- SIFT先利用非极大抑制，再用到Hessian矩阵进行滤除；SURF先用Hessian矩阵，再进行非极大抑制。
- 特征
- 采用了harr特征
- 采用了积分图像integral image概念
- 步骤
SURF使用Hessian Matrix进行特征点的提取，所以海塞矩阵是SURF算法的核心
- ①构建海塞矩阵
由于我们的特征点需要尺度无关性，所以在进行Hessian矩阵构造前，需要对其进行高斯滤波。
- ②尺度空间生成
图像的尺度空间是这幅图像在不同解析度下的表示。
在进行高斯模糊时，SIFT的高斯模板大小是始终不变的，只是在不同的octave之间改变图片的大小。
而在SURF中，图片的大小是一直不变的，不同的octave层得到的待检测图片是改变高斯模糊尺寸大小得到的，同一octave中的图片用到的高斯模板尺度也不同。
- ③非极大抑制初步确定特征点和精确定位特征点
将经过hessian矩阵处理过的每个像素点与其3维领域的26个点进行大小比较，如果它是这26个点中的最大值或者最小值，则保留下来，当做初步的特征点。
然后，采用3维线性插值法得到亚像素级的特征点，同时也去掉那些值小于一定阈值的点，增加极值使检测到的特征点数量减少，最终只有几个特征最强点会被检测出来。
- ④确定特征点主方向
在SURF中，不统计其梯度直方图，而是统计特征点领域内的Harr小波特征。
即以特征点为中心，计算半径为6s(S为特征点所在的尺度值)的邻域内，统计60度扇形内所有点在x(水平)和y(垂直)方向的Haar小波响应总和(Haar小波边长取4s)，并给这些响应值赋高斯权重系数，使得靠近特征点的响应贡献大，而远离特征点的响应贡献小，然后60度范围内的响应相加以形成新的矢量，遍历整个圆形区域，选择最长矢量的方向为该特征点的主方向。这样，通过特征点逐个进行计算，得到每一个特征点的主方向。
- ⑤构造描述子## 由匹配点估计相机运动### 单目相机：2D-2D
- 对极约束
- x_{2}^{T}*E*x_{1} = p_{2}^{T}*F*p_{1} = 0
- 本质矩阵 E = t^R
- ①内在性质：奇异值为[σ，σ，0]
- ②不同尺度下等价：尺度等价性
- ③由于②，自由度为6-1=5个
- 基础矩阵 F = K^{-T}*E *K^{-1}
- 单应矩阵 H = R - t*n^{T}/d
- 描述两个平面映射关系
- 用于特殊情况：特征点位于同一平面
- 求解方法：直接线性变换 DLT
- 求解步骤
- 1. 根据配对点计算 E 或者 F
- 2. 根据 E or F or H 计算 R 和 t
实际中往往同时估计E/F/H，选择重投影误差最小的作为最终的运动估计矩阵
- ①由于自由度为5，最少用5对点求解
- ②常用8点法，即8对点
- ③对 E 进行奇异值分解(SVD)
- ④由于E的尺度不变形，直接取奇异值矩阵Σ=diag (1, 1, 0)
- 存在问题
- 尺度不确定性
- 来源于对 t 归一化
这么做的起因是E具有尺度等价性，因此 t 也具有尺度等价性，令其为1即称为归一化。
- 初始化
以 t 为单位1，计算相机运动和特征点的3D位置
- 纯旋转问题
- 单目SLAM初始化必须有平移
若平移t=0，那么E也将为0，就没法求解了
- 多余8对点
- 最小二乘解决
- 随机采样一致性(实际中采用)
可处理带错误匹配的数据，适用于误匹配时### 3D-2D
最重要的姿态(运动)估计方法。
3D位置可以由三角测量或者RGB-D深度图获得
- 无需使用对极约束
- PnP
(Perspective-n-Point)
- 使用情况
- 双目orRGB-D：直接使用
- 单目：必须经过初始化
- 求解方法
- DLT 直接线性变换
- ①使用齐次坐标P, x
P=(X, Y, Z, 1)
x=(u, v, 1)：投影点用归一化坐标
- ②定义增广矩阵T
T = [ R | t]
显然包含了平移和旋转信息
- ③线性展开s*x = T*P
- ④对于R，从上面求解的T中去除左边3*3矩阵块进行分解
- P3P
使用3对配对点估计位姿
注意:
3D点的坐标是在世界坐标系中；
2D点坐标是在相机坐标系中
- ①三角形相似+余弦定理
- ②求解方程组，得到3D点在相机坐标系中的坐标
- ③利用3D-3D解法求解
- EPnP
Efficient PnP
- BA 非线性优化
Bundle Adjustment;
捆集调整/光束法平差
- 相机位姿与3D位置一起优化
优化变零即相机位姿(R, t)和特征点3D位置
- ①定义误差项 e =
e = u - KTP/s
被称为重投影误差
- ②构建最小二乘问题 T = arg min ||……||^{2}
- ③获得误差对于优化变零的雅克比矩阵
误差对于相机位姿李代数的雅克比矩阵
误差对于特征点3D位置的雅克比矩阵
- ④选择优化算法求解：GN/LM等
- 使用g2o实现BA优化
- ①声明图优化器
- ②配置优化求解器和梯度下降方法
- ③根据估计得到的特征点，将位姿和空间点放入图中
- ④调用优化函数进行求解### 双目、RGB-D：3D-3D
- 使用情况：匹配好的3D点对
未出现相机模型，在3D-3D情况下，与相机并没有关系
- 求解：ICP
ICP（Iterative Closest Point）迭代最近点
这里说的ICP问题是指：已有特征匹配的情况下进行位姿估计的问题
- 线性代数求解：SVD
- 推导：定义误差项
e_{i} = p_{i} - ( R*p_{i}^{ ' } + t )
注意这里不要使用李群中的变换矩阵T
- 推导：构建最小二乘问题
- 推导：添项、展开、等价
- ICP的SVD求解步骤
- ①计算两组点质心位置，再计算各点去质心坐标
质心位置：p, p_{ ' }
q_{i} = p_{i} - p
q_{i}^{ ' } = p_{i}^{ ' } - p^{ ' }
- ②根据最小二乘优化问题计算 R
R^{*} = arg min ∑||q_{i} - R*q_{i}^{ ' } ||^{2}
- R的求解要用到SVD
- ①令W = ∑q_{i}*q_{i}^{ ' T }
- ②对W进行SVD：W = U*∑*V^{T}
- ③则R = U*V^{T}
- ③由得到的 R 计算 t
t = p - R*p^{ ' }
- 非线性优化：BA
- 以迭代方式寻找最优
- 求解步骤（类似PnP）
- ①以李代数表示位姿，构建目标函数
- ②推导雅克比矩阵（使用李代数扰动模型）
- ③选择优化算法求解
- 求解特点
- 存在无穷多解情况：失效
- 存在唯一解情况
- 极小值解即为全局最优解
- 意味着ICP求解可任意选定初值
- 实际使用中：混用PnP和ICP
- 特征点深度已知：
- 建模3D-3D误差
- 特征点深度未知：
- 建模3D-2D重投影误差## 由相机运动估计特征点空间位置### 三角测量
- 思路来源
- 单目SLAM无法通过单张图像获得像素深度信息
- 三角测量用于估计点的深度
- 步骤
- ①由对极约束得到了相机运动后
- ②再由相机运动估计特征点空间位置
- 定义
- ①在不同位置对同一个路标点进行观察
- ②从观察到的位置推断路标点深度
- 求解
- ① s_{2}*x_{2} = s_{1}*R*x_{1} + t
- ②已知R, t，求解s_{1}, s_{2}
- 两侧左乘x_{2}^{^}
- ①式等于0，求解s_{1}## 光流法### 光流
- 描述了像素在图像中的运动
- 分类
- 稠密光流
- 稀疏光流
- LK光流### 以LK光流为例
- 灰度不变假设
- 思路
- 下一时刻灰度等于之前的灰度
I(x+dx, y+dy, t+dt) = I(x, y, t)
进行一阶泰勒展开
frac{partial I}{partial x}*(dx) + frac{partial I}{partial y}*(dy) + frac{partial I}{partial t}*(dt) = 0
写成矩阵形式：
[I_x I_y] * [u v]_T = -I_t
- 某一窗口内的像素具有相同的运动
- 用途效果
常用于追踪角点。可以加速基于特征点法的视觉里程计算法。
- 多层光流法
构造图像金字塔，从顶层向底层进行计算，由粗至精的计算思路，相当于是：在顶层是粗略定位，往下做是不断精细化像素点。## 直接法### 思路
直接根据像素灰度信息获得相机运动和对应点的投影，因此不知道在第二幅图像中哪个 p_2 对应着 p_1 。此时我们根据估计的相机位姿来找 p_2 ，当估计的位姿不好时， p_2 与 p_1 会有明显不同（灰度不变假设），这时我们通过构造优化问题，不断调整位姿以减小像素灰度差别。### 分类
- 稀疏直接法
速度最快，只能计算稀疏的重构
- 半稠密直接法
只是用带有梯度的像素点
- 稠密直接法
计算所有像素点### 优缺点
- 优点
- 无需计算特征点及描述子，节省时间
- 只要求有像素梯度，无需特征点
- 可构建(半)稠密地图
- 缺点
- 非凸性问题带来的局部极小问题
- 单个像素不具备区分度
- 需要灰度不变假设，实际难以满足