SLAM 视觉里程计和回路检测

综述

视觉里程计：通过连续关联图像来进行位置和方向的估计
1. 基于特征点的方法：
构建：稀疏点云
方法：世界坐标系->相机坐标系->图像平面，投影+外参

PTAM、ORB-SLAM

2. 基于光测误差
构建：（半）稠密点云
方法：投影+光测度误差
LSD-SLAM、DVO

Feature-Based

Direct

直接把两个图片相减，使其差最小，省略特征提取匹配的过程

特征法

特征点云的初始化：

1. Mono：随机初始化（EKF SLAM 会进行update）
2. local SfM ：
   

单应矩阵：
Mint×Mext：H，homography

适用于相机看到的是同一个平面

基础矩阵：

适用于相机看到的两个画面有视差（有边缘）

特征点的提取和匹配：

2D到2D图像特征匹配（单目）：两帧图像中的feature进行匹配
3D到2D图像特征匹配（双目）：上一帧图像+深度信息生成了点云，点云中的位置和本帧图像特征进行匹配（RANSAC PNP）

ICP（迭代最邻近点）：双目或者RGBD
双目需要Disparity

运动估计：

一般利用匀速假设模型，若是假设相机位置不变，则在优化的时候计算量较大，假设匀速运动，则估计位姿和实际位姿相差不大，在优化的时候只需要微调一下即可。

优化：

利用BA优化

直接法

利用同一像素曝光度不变的的性质min

比较

feature-based
优势：

1. 不需要很好的初始化（后面可以被优化）
2. 快
3. outliers（匹配错了的点）很容易被检测出来
4. rolling shutter（由于拍照时抖动等原因发生移位，拍摄物体形状改变）鲁棒
   因为rolling shutter是行扫描，若拍摄时抖动，则行与行之间的图像中物体的关系是错乱的
   global shutter是全局扫描，不会出现上述情况

缺点：
6. 利用corners
7. 特征点比较少，对图像的利用率比较低

Direct
优势：

1. 整张图片每个像素都可以被利用
2. 平行移动、图片模糊的情况下效果很好
3. 使用像素多
   缺点：
4. 慢
5. outliers不容易被检测出来
6. rolling shutter不鲁棒（图片中的物体发生移位，无法通过视差来计算）
7. 需要很好的初始化

视觉里程计的典型工作：

1. EKF Monocular SLAM（特征法）
2. PTAM-SLAM（单目，基于图优化的方式）（特征法）
3. RGBD-SLAM（特征法）
4. ORB-SLAM（特征法）
5. LSD-SLAM（直接法）

基于特征法的视觉里程计：PNP

世界坐标系->相机坐标系

于是最终求旋转平移就变成了三维特征点云矩阵和图像上特征点坐标的乘积：

RANSAC PNP 比 PNP 多了remove outliers的过程

基于直接法的视觉里程计

依然基于关键帧、图优化SLAM

即

π：pinhole model由三维投影到二维的过程
π-1：pinhole model由二维反投影到三维的过程

通过上一帧的像素（每一个）位置和深度之后，反投影可得上一帧的三维点云，再加上位姿变化可得这一帧的三维位姿，正投影可得本帧像素位置，和相机拍摄到的像素位置比对，最小化差即可


已知上一帧的像素位置p和深度D，再加上旋转平移，通过投影变换得到估计像素的位置，和真实相机看到的位置作差求最小

直接法：稀疏表达、半稠密表达、稠密表达

稀疏表达：特征点
半稠密表达（semi-dense）：主要是边缘信息（轮廓信息）

稠密表达（dense）：图片的每个像素点都被使用（完整信息）

基于立体视觉法的视觉里程计：ICP

使用双目或者RGBD相机直接获取点云数据，经过旋转平移后将两组数据进行merge
方法：迭代最近邻匹配（ICP）
给定两组待匹配点云：
X = {x1,…,xn}
Y = {y1,…,yn}
最小化方程，求最优的旋转平移矩阵

先求平均值，得到点云的几何中心，平移得到，接下来只需要优化旋转即可

CCNY-RGBD

基于词袋模型的回路检测

有回路产生（即使是走直线，只要两帧画面有足够的重合区域即可认为是有回路产生）：

1. 可以有效的减少误差
2. 可以避免单目的尺度损失

回路检测的方式：

1. 地图对地图：匹配地图尺度上的特征（如ICP）
2. 地图对传感器：匹配地图与当前传感器信息（如ADF）
3. 传感器对传感器：匹配当前传感器和以前传感器的信息

词袋模型：

核心思想： 用特征来描述图像

1. 分块，计算每个单元在每个方向上的梯度(两个像素的像素差)，计算了8个方向上的梯度（0°，45°，90°，…），共用了16*8个向量来描述了图像特征。

2. 提取特征获得高维向量，再将这些向量进行聚类（通过与词袋中的每个类别的聚类中心比较进行分类）

3.计算每类特征的数量：

设特征1类的有N1个，特征2类的有N2个，特征3类的有N3个，特征4类的有N4个
则频率：特征1：N1/N，特征2：N2/N，特征3：N3/N，特征4：N4/N
所以最终用1*4的向量来表示一张图片

提取特征向量，分类：

计算欧式尺度:

在训练过程中，会出现：

某一个特征数量特别多，一些特别少，此时数量多的权重小（重复性高，边缘等信息，你有的别人也有，区分度不大），数量少的权重大（一般这种特征区分度大）

DBOW：