【toc】目录

一、视觉里程计Visual Odometry

• 1.1 特征点法
• 1.2 特征匹配

二、2D-2D：对极几何

三、PNP

四、3D-3D ICP

一、视觉里程计Visual Odometry

1.1 特征点法

经典SLAM模型中以位姿——路标Landmark来描述SLAM过程。路标是三维空间中固定不变的点，能够在特定位姿下观测到，首先对路标有几点要求：

• 数量充足，以实现良好的定位
• 较好的区分性，以实现数据关联

在视觉SLAM中，可利用图像特征点作为SLAM中路标，特征点是图像中具有代表性的部分。比如角点，边缘，区块，这些特征描述应该在光照、视角发生少量变化时，仍能保持一致。这些特征点需具备的条件：

• 可重复性
• 可区别性
• 高效
• 本地

特征点的信息：位置、大小、方向、评分——关键点

特征点周围的信息——描述子（Descriptor）

例如SIFT、SURF、ORB：Oriented Fast and Rotated Brief

以ORB（2011年提出）举例：相比SIFT高效简单，但是精度略低。

• 关键点：Oriented FAST，连续N个点灰度有明显差异
• 描述：BRIEF

比如以图中p为圆心，周围比较周围16个点的灰度值是否比中心点p大于某个阈值。只要进行比较就行，不用计算。然后如果连续N个点的灰度有明显差异，那这个点就是关键点，然而这样可能会检测很多关键点，所以后面还要滤除一部分。然后在此基础上还要计算旋转。

计算流程：
1、在一个小的图像块B中，定义图像的矩为： 
2、通过矩可以找到图像块的质心：

3、连接图像块的几何中心O与质心C，得到一个方向向量  ，于是特征点的方向可以定义为： 

BRIEF-128这种描述子，在特征点附近的128次像素比较。是一种二进制描述，需要汉明距离度量。

1.2 特征匹配

• 通过描述子的差异判断哪些特征点为同一个点
• 暴力匹配：比较两种图像中每个特征的距离
• 加速：快速最近邻FLANN

二、2D-2D：对极几何

特征匹配之后，得到了特征点之间的对应关系：

• 如果只有两个单目图像，得到2D-2D间的关系——对极几何
• 如果匹配的事帧和地图，得到3D-2D间的关系——PnP
• 如果匹配的是RGBD图，得到3D-3D间的关系——ICP

几何关系：  在两个图像的投影为  ，两个相机之间的变换为  ， 在第二个图像上投影为 ,记作  称为极线，反之亦然。  称为极点。

在实践中：  通过特征匹配得到，  未知，  未知，  待求：

• 世界坐标： 
• 以第一个图为参考系，投影方程： 
• 使用归一化坐标（去掉内参）： 
• 齐次关系： 
• 两侧左乘： 
• 再进一步右乘： 
• 对极约束： 
• 带内参的形式： 

对极约束刻画了  共面的关系，定义：

两步计算位姿：

• 由匹配点计算E
• 由E恢复R，t。使用奇异值分解。  ,四个可能的解，但只有一个深度为正。

堆积约束的性质：

• 乘任意非零常数依然满足
• E共五个自由度，当成普通矩阵的话，有八个自由度，可用八点法求解。最少得5个点。

关于八点法的讨论：

• 用于单目SLAM的初始化
• 尺度不确定性：归一化t或特征点的平均深度
• 纯旋转问题：t=0时无法求解
• 多于八对点时：最小二乘或RANSAC

另外，八点法在特征点共面时会退化，因此需从单应矩阵恢复R，t。乱七八糟，什么玩意！

三、PNP

已知3D点的空间位置和相机上的投影点，求相机的旋转和平移(外参)。有代数的解法和优化的解法。

代数解法

• DLT
• P3P
• EPnP/UPnP

优化解法：Bundle Adjustment

DLT

• 设空间点 
• 投影点：  归一化坐标
• 投影关系： 
• 展开：
• 

将它看成一个关于t的线性方程，求解t，最后一行

用它消掉前两行中的s，则第一个特征点提供两个方程：

为求解12个未知数，需要12/2=6对点。超定时求最小二乘解。

DLT将R，t看成独立的未知量，所以在求出结果后，需要将t组成的矩阵投影回SO(3)（通常用QR分解实现）。

P3P

利用三对点求相机外参，对应关系：

• 余弦定理：

对以上三式全体除以  ，并且记作  得：

记  有：

消去v：=>得到关系关于x，y的二次方程，用吴氏消元法解析得：

得到  后，利用 

解得v，从而解得OC的长度，进而得到各点的距离。

• P3P的缺点：
• 对三个点以上的情况难处理
• 误匹配时算法失效

优化解法：Bundle Adjustment，数学证明根本看不懂

• 最小化重投影误差Minimizing a reprojection error
• 投影关系
• 定义重投影误差并取最小化

四、3D-3D ICP

给定配对好的两组3D点，求其旋转和平移，可用迭代最近点Interative Closest Point，ICP求解：

设 

• 运动关系： 
• 同样定义误差项： 
• 以及最小二乘问题： 

ICP也可以从非线性优化角度求解，但：

• 已知匹配时，ICP问题存在唯一解或无穷多解的情况，在唯一的情况下，只要找到较小值解，那么这个极小值解是全局最优解；
• 正常情况下，SVD结果和优化一样，且优化很快收敛

注：

• 在激光情况下，匹配点未知，将指定最近点为匹配点，此时问题为非凸，极小值不一定为最小值；
• 利用非线性优化可以将ICP与PnP结合在一起求解。

真tm难！记录我兄弟十一连跪的一天！

继续加油~！