5. 特征点法视觉里程计
- 特征点提取与匹配
- 根据几何关系建模相机运动
- 2D-2D 对极几何
- 3D-2D PnP
- 代数方法：DLT
- 优化方法
- 3D-3D ICP
- 三角化与深度估计
声明：本文是深蓝学院 高翔博士主讲的《SLAM理论与实践》的学习笔记。
特征点提取与匹配
经典SLAM模型中以位姿(x)、路标Landmark(y)来描述SLAM过程。 
路标点的作用主要是，建立同一点在不同图像上的匹配关系，它一般满足如下性质：
- 三维空间中固定不变的点
- 能够在特定位姿下观测到
- 数量充足，以实现良好的定位
- 较好的区分性，以实现数据关联
在视觉SLAM中，可利用图像特征点作为SLAM中的路标。特征点是图像当中具有代表性的部分，具有如下特点：
- 可重复性（A中可以看到，B中也能看到）
- 可区别性
- 高效
- 本地
特征点的信息:
位置（u,v）、大小（区块大小s）、方向( θ θ )、评分等——关键点。
- FAST：连续N个点的灰度有明显差异（FAST9 ，FAST10, FAST11）
- Oriented FAST：在FAST基础上计算旋转方向。然后可以根据方向选取patch可以更好的匹配相同的区块（减少旋转导致的难以匹配问题）
- FAST：连续N个点的灰度有明显差异（FAST9 ，FAST10, FAST11）
特征点周围的图像信息——描述子（Descriptor）对图像块进行编码以保持信息的不变特性（例如光照不变） 见OpenCV features2d模块
- SIFT（可重复可区分性都挺好但是提取效率不高需要的计算量太大可能无法在较弱的本地芯片上实时运行）
- BRIEF-128：在特征点附近的128次像素比较（规定好这128个pair的位置）
记录了多个pair之间的相对明暗情况，可编码出光照不变的表达。BRIEF是一种二进制描述，需要用汉明距离度量（不同位的个数）
- ORB：根据旋转方向先做旋转，再选择patch进行BRIEF描述
- SURF
例子：ORB特征 ：
- 关键点：Oriented FAST （计算方向）
- 描述：BRIEF （根据方向选择patch 计算描述子）
它具有平移不变，旋转不变，没有尺度不变！（可以用图像金字塔实现）
特征匹配
通过描述子的差异判断哪些特征为同一个点 
- 暴力匹配：比较图1中每个特征和图2特征的距离
- 加速：快速最近邻（FLANN）
根据几何关系建模相机运动
特征匹配之后，得到了特征点之间的对应关系
- 如果只有两个单目图像，得到2D-2D间的关系 ——对极几何
- 如果匹配的是帧和地图，得到3D-2D间的关系 ——PnP
- 如果匹配的是RGB-D图，得到3D-3D间的关系 ——ICP
2D-2D 对极几何
2D-2D的情况一般利用对极约束得到相机运动。
几何关系：
- P在两个图像的投影为 p1,p2 p 1 , p 2
- 两个相机之间的变换为 T12 T 12 ， 对应有 T12=R12,t12 T 12 = R 12 , t 12 坐标系1中的点X1与坐标系2中的点X2的关系为 X1=T12X2 X 1 = T 12 X 2 。 一般情况下我们使用 T21 T 21 ?
- O1P O 1 P 在第二个图像上投影为 e2p2 e 2 p 2 记为 l2 l 2 ，称为极线（P的投影一定在极线的所在的射线上），反之亦然。 e1,e2 e 1 , e 2 称为极点
实践当中 p1,p2 p 1 , p 2 通过特征匹配得到，P未知， e1e2 e 1 e 2 未知， T12 T 12 待求
下面推导对极约束：
- 世界坐标： P=[X,Y,Z]T P = [ X , Y , Z ] T ，
- 以第一个图为参考系，存在投影方程 s1p1=KP,s2p2=K(RP+t) s 1 p 1 = K P , s 2 p 2 = K ( R P + t )
- 不过我们此时知道 p1,p2,K p 1 , p 2 , K 所以可以利用手里的信息得到两个坐标系下面的归一化坐标为 x1=K−1p1 x 1 = K − 1 p 1 (也就是 Ps1 P s 1 ) 、 x2=K−1p2 x 2 = K − 1 p 2 (也就是 RP+ts2 R P + t s 2 )
- 它们存在着齐次关系 x2=Rx1+t x 2 = R x 1 + t 。 (为什么？)
- 两侧左乘： t\^x2=t\^Rx1 t \^ x 2 = t \^ R x 1
- 再进一步左乘： xT2t\^x2=xT2t\^Rx1 x 2 T t \^ x 2 = x 2 T t \^ R x 1
- 左侧项由于垂直关系必然为0，得到对极约束 xT2t\^Rx1=0 x 2 T t \^ R x 1 = 0 带内参的形式为： pT2K−Tt\^RK−1p1=0 p 2 T K − T t \^ R K − 1 p 1 = 0
- 対极约束刻画了 O1,O2,P O 1 , O 2 , P 公民的关系，定义 E=t\^R E = t \^ R (t^和R都是3x3的矩阵), F=K−TEk−1 F = K − T E k − 1 则有 xT2Ex1=pT2Fp1=0 x 2 T E x 1 = p 2 T F p 1 = 0 （Essential矩阵，Fundamental矩阵）在内参已知的情况下，可以使用E
- 对极约束 xT2Ex1=0 x 2 T E x 1 = 0 的性质：a）乘任意非零常数依然满足（具有尺度不变性，丢失了尺度的自由度 λE λ E 与 E E 等价） b） E共五个自由度（t有3个+R有3个-丢失的尺度自由度=5个），如果当成普通矩阵的话，有八个自由度，可用八点法求解
两步计算位姿态(特征点不在平面上)：
有匹配点根据対极约束计算出E
- 八点法求E（将E看成通常3x3的矩阵，去掉因子后剩八个自由度，线性代数求解），
- 一对匹配点带来一个约束方程：
转换为向量形式 
8个自由度只需要8个方程就可以解，所以至少需要八对点构成方程组：
SVD得到真正的E： 过程中取
E的内在性质（5个自由度）要求它的奇异值为 σ,σ,0 σ , σ , 0 （不过八点法求解的E并不满足这个性质 所以需要转换一下）.
SVD恢复出R,t: E=UΣVT E = U Σ V T
四个可能的解（R t的所有组合2x2=4? 详细推导可以看MVG ），但只有一个深度为正(通过验证 深度为正).
.
最少可使用五个点计算R,t称为五点法，求出的E位于5个自由度的流形( 但需要利用E的非线性性质,原理较复杂)
八点法的讨论：
- 用于单目SLAM的初始化（单目特有的环节，缺少3D信息，）
- 尺度不确定性： λt λ t 都可以，归一化 t （ |t|t | t | t ， 将用于初始化的两帧之间的距离，作为平移的单位长度，但是具体一个单位是多少m是未知的， 而双目知道两个镜头之间的距离也就可以算具体的深度和两帧之间的移动的具体距离的），或着归一化特征点的平均深度
- 纯旋转问题：如果没有平移，t=0 导致 E=t∧R=0 E = t ∧ R = 0 无法求解
- 多于八对点时：超定方程（方程数量大于未知数数量？）没法直接求解，可以使用最小二乘
- 有外点时：两帧之间存在的误匹配点称为外点，通过RANSAC去除
两步计算位姿(特征点在平面上):
八点法在特征点共面时（例如 俯视桌面）会退化,设特征点位于某平面上, nTP+d=0→nTPd=1 n T P + d = 0 → n T P d = 1 .两个图像特征点的坐标关系: 
该式是在非零因子下成立的.去掉第三行有:
一对点提供两个约束（四对点就可以算出）. 写成关于H的线性方程. 
类似八点法
- 先单应矩阵H
- 再用H恢复R,t,n,d,K （8选1）
小结
- 2D-2D情况下，只知道图像坐标之间的对应关系
- 当特征点在平面上时（例如俯视或仰视），使用H恢复R,t
- 否则，使用E或F恢复R,t
- t 没有尺度
- 求得R,t后：
- 利用三角化计算特征点的3D位置（即深度 ）
- 实际中用于单目SLAM的初始化部分(尺度不可知， 对轨迹（t?）和地图(深度z?)同时乘以n倍，仍然成立 ， 不过可以通过其它传感器补充该信息，例如扫地机的轮式里程计 \ IMU \ 或者事先知道长度的东西标定板) ， 纯单目无法获得尺度信息
3D-2D PnP
前两帧初始化之后就可以估算出两帧图像中对应特征点的3D信息，然后第三帧与第二帧匹配出特征点，这些特征点 在第二帧中可以获得3D信息，在第三帧只能得到2D信息。
PnP:有N个点知道它们真实位置和相机里的位置如何计算相机的姿态（旋转和平移）已经3D点的空间位置和相机上的投影点，求相机的旋转和平移（外参）
代数的解法/优化的解法:
- 代数的
- DLT: 直接线性变换
- P3P
- EPnP/UPnP/…
- 优化的：Bundle Adjustment
代数方法：DLT
DLT将R,t看成独立的未知量，所以在求出结果后，需要将t组成的矩阵投影回SO(3)（通常用QR分解实现） 
（注意：一般情况下左右两边的数据都有噪声，线性性质不能很好的保持，特别是跟矩阵的特征值相关的东西）
此外，也可代入内参矩阵K，但SLAM中一般假设K已知，所以这里没有代入。
除DLT外：
P3P：利用三对点求相机外参
EPnP
……
优化方法
Bundle Adjustment
- 最小化重投影误差（Minimizing a reprojection error）
- 投影关系：
- 定义重投影误差并取最小化
- 线性化和雅可比: 考虑单个投影点误差：
， 线性化：
雅可比的形式？
- 求第一项： ∂e∂P′ ∂ e ∂ P ′ 因为： e=u^−1sKexp(ξ∧)P=u^−1sKP′ e = u ^ − 1 s K e x p ( ξ ∧ ) P = u ^ − 1 s K P ′ 如果令 u=1sKP u = 1 s K P 则 ∂e∂P′=∂e∂u∂u∂P′=−1⋅∂u∂P′ ∂ e ∂ P ′ = ∂ e ∂ u ∂ u ∂ P ′ = − 1 ⋅ ∂ u ∂ P ′
- 下面求 ∂u∂P′ ∂ u ∂ P ′ ： 而根据内参投影关系有 su=KP′ s u = K P ′ 写成矩阵形式
得到 
- 可得
- 下面求 ∂u∂P′ ∂ u ∂ P ′ ： 而根据内参投影关系有 su=KP′ s u = K P ′ 写成矩阵形式
- 求第二项 ∂P′∂δξ ∂ P ′ ∂ δ ξ :
非齐次形式
- 两项相乘：
- 求第一项： ∂e∂P′ ∂ e ∂ P ′ 因为： e=u^−1sKexp(ξ∧)P=u^−1sKP′ e = u ^ − 1 s K e x p ( ξ ∧ ) P = u ^ − 1 s K P ′ 如果令 u=1sKP u = 1 s K P 则 ∂e∂P′=∂e∂u∂u∂P′=−1⋅∂u∂P′ ∂ e ∂ P ′ = ∂ e ∂ u ∂ u ∂ P ′ = − 1 ⋅ ∂ u ∂ P ′
最后得到更新姿态的公式为：
也可以对3D点求导：
3D-3D ICP
给定配对好的两组3D点，求其旋转和平移，可用迭代最近点（Iterative Closest Point，ICP求解）
- 设：
- 运动关系
- 同样定义误差项
- 以及最小二乘问题
- 稍加推导： 定义质心
改写目标函数： 
目标函数简化为
- 最小化第一项，然后取t，使得后一项为零 。
- 左侧项（旋转项）如何求？ 定义去质心坐标：
最小化：
推导
最右一项 
SVD解法：
- 左侧项（旋转项）如何求？ 定义去质心坐标：
ICP也可以从非线性优化角度求解，
- 但已知匹配时，ICP问题存在唯一解或无穷多解的情况。
- 在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值。所以正常情况下，SVD结果和优化一样，且优化很快收敛。
- 存在无穷多解：退化的情况，点都在直线上，旋转可以让直线 有无穷多种情况
- 存在无穷多解：退化的情况，点都在直线上，旋转可以让直线 有无穷多种情况
- 在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值。所以正常情况下，SVD结果和优化一样，且优化很快收敛。
- 不给匹配时不解析，因为对应关系找不到（通过最近邻找 非常不可靠） 可能出现多个点， 非凸问题，可能收敛到一个局部最小 。 在激光情况下，匹配点未知，将指定最近点为匹配点。此时问题非凸，极小值不一定为最小值。
- 利用非线性优化可以将ICP与PnP结合在一起求解。 （有些点可以获取3D信息(3D-3D)有些只能获取2D信息（PnP:3D-2d））
三角化与深度估计
已知运动时（相机的位姿R,t），，求解特征点的3D位置
几何关系： 归一化坐标记做 x1,x2 x 1 , x 2 乘以深度之后变成 未归一化的原始坐标 
- 求 s2 s 2 时两侧乘以 x∧1 x 1 ∧
反之亦然 - 或者同时解 s1,,s2 s 1 , , s 2 （矩阵论：超定方程的最小二乘解）
方程存在有些问题：
- 系数矩阵伪逆不可靠
- ATA A T A 行列式为零 （说明太平行）
- 例如：相机前进时，虽然有位移，但位于图像中心的点无法三角化（没有视差）
三角化中的问题：
- 解得深度的质量与平移相关,平移小时误差大，平移大时误差虽小但是特征匹配可能不成功
小结：
本章介绍了与特征点相关的视觉里程计部分算法，包括：
- 特征点是如何提取并匹配的；
- 如何通过2D-2D的特征点估计相机运动（用E或H求解R,t，一般称作单目相机的初始化）；
- 三角化原理(根据R，t, u,v 估计深度 完成初始化地图点?)；
- 3D-2D的PnP问题（有了初始化好的 3D坐标就可以解PnP问题求解位姿），线性解法(DLT)与BA解法；
- 3D-3DICP问题，线性解法与BA解法。 线性解法已经是最优了， 不过BA可以将所有损失融合在一起整体优化