SLAM学习笔记(六)

视觉里程计

Posted by QIY on September 6, 2020

高翔老师SLAM14讲学习笔记

  • 本章介绍了与特征点相关的视觉里程计部分算法,包括:
    • 特征点是如何提取并匹配的;
    • 如何通过2D-2D的特征点估计相机运动;
    • 三角化原理;
    • 3D-2D的PnP问题,线性解法与BA解法;
    • 3D-3DICP问题,线性解法与BA解法。

      1 特征提取

      1)特征:关键点(例如角点,明暗变化大的点)+描述子(记录的关键点的特征信息,方向,旋转 等) 2)FAST特征点:角点,检测局部像素灰度变化大的地方,FAST-N 半径为3的圆上有16个像素点,计算是否连续N个点亮度大于或小于阈值 3)ORB = ORrented Fast 特征点(像素几何中心到平均亮度中心的连线是方向)+ BRIEF 描述子(二进制,用汉明距离描述差异) 4)匹配方法:暴力匹配BFM,快速近似最近邻FLANN 5)opencv2/features2d/features2d.hpp KeyPoint Dmatch

      2 2D-2D 对极几何

      ================ 几何关系:

  • P在两个图像的投影为\(p_{1},p_{2}\)
  • 两个相机之间的变换为\(T_{12}\)
  • \(O_{1}P\)在第二个图像上投影为\(e_{2}p_{2}\)
  • 记\(l_{2}\),称为极线,反之亦然
  • \(e_{1},e_{2}\)称为极点 实践当中:
  • \(p_{1},p_{2}\)通过特征匹配得到,P未知,\(e_{1},e_{2}\) 未知
  • \(T_{12}\)待求

    2.1 目的和思路

    目的:计算特征点的3D位置(即深度) 思路:

  • 2D-2D情况下,只知道图像坐标之间的对应关系
    • 当特征点在平面上时(例如俯视或仰视),使用H恢复R,t
    • 否则,使用E或F恢复R,t
  • 求得R,t后:
    • 利用三角化计算特征点的3D位置(即深度)

      2.2 对极约束

      由第五章知识可推: (s1:P垂直与图像平面到O的距离) x2为对称矩阵,此式为零,既有: x1 为左平面中点,x2为右平面中点。由两平面组成的对极约束可得,x1,x2已知,可求t,R。

      2.3 Essential和Fundamental矩阵

      Essential:本质(征)矩阵;Fundamental:基础矩阵,当K已知情况,我们使用E来求解。

      2.4 计算R t

      1.  由匹配点计算E
      2.  由E恢复R,t 对极约束的性质: 乘任意非零常数依然满足 E(R三个自由度,t三个自由度)因乘任意非零常数依然满足,故E共五个自由度。 如果非线性求解的化可按5自由度求解,非线性不太好做。 当成普通矩阵的话,有八个自由度(3x3 - 1) 可用八点法求解 1 从每个图像像素特征点中抽取8对,带入式子求 2 因为E不是方阵,故使用SVD奇异值分解方法求R,t。 具体如下: 1)
      
  • 将E看成通常3x3的矩阵,去掉因子后剩八个自由度 一对匹配点带来的约束: 8点:
    1. SVD 产生4个解,但只有一个深度为正的解:
    2. 深度为正,其他不符合。

      2.5 补充

      1)SVD 取 因为E的内在性质要求它的奇异值为 。 2)五点法 最少可使用五个点计算R,t,称为五点法,但需要利用E的非线性性质,原理较复杂。

      2.6 H矩阵

      八点法在特征点共面时会退化,此时不能再用E矩阵了,使用H矩阵方法,设特征点位于某平面上: 。 两图像特征点坐标关系: == 类似8点法:先计算H,再由H恢复R,t,n,d,K。

      2.7 三角化

      得到R,t后通过三角化的方法求s(深度)。 几何关系: 求\(s_{2}\) 时,两侧乘\(x_{1}^{\land}\) ; S1求法一样; 或者同时解 \(s_{1},s_{2}\) 求\(\left\lbrack - Rx_{2},x_{1} \right\rbrack\begin{bmatrix} s_{1} \\ s_{2} \\ \end{bmatrix} = t\)的最小二乘解\(x = \left( A^{T}A \right)^{- 1}A^{T}b\); 平移问题:

      2.8 八点法存在的问题

      用于单目SLAM的初始化

  • 尺度不确定性:归一化 t 或特征点的平均深度(E乘任意常亮都满足)
  • 纯旋转问题:t=0 时无法求解(t=0,E便等于0)
  • 多于八对点时:最小二乘或RANSAC来挑选特征点

    3 PNP(3D-2D)

    目的:已经3D点的空间位置和相机上的投影点,求相机的旋转和平移(外参)。 两种解法:代数的解法/优化的解法 代数的(利用几何关系) DLT P3P EPnP/UPnP/… 优化的:Bundle Adjustment(设置误差,不断的缩小误差),当运动比较连续的情况下,一般采用优化的方法,因为观测值的噪声一般比较大,使用代数法计算很难那么准确。

    3.1 DLT(代数法)

    为求解12个未知数,需要12/2=6对点。(超定时求最小二乘解) DLT将R,t看成独立的未知量,所以在求出结果后,需要将t组成的矩阵投影回SO(3)(通常用QR分解实现) 此外,也可代入内参矩阵K,但SLAM中一般假设K已知,所以这里没有代入。

    3.2 P3P(几何法)

  • 利用三对点求相机外参 => 消去v,得到关于x,y的二次方程,用吴氏消元法解析解得: 三对点即可;

    3.3 Bundle Adjustment

    最小化重投影误差(Minimizing a reprojection error) 投影关系: P:3d点,\(\xi\) 相机位姿\(K\)内参\(u\) 投影(平面一点) 定义重投影误差并取最小化: 求最小化问题,接着就是求导了。

    3.3.1 线性化和雅可比

    考虑单个投影点误差:\(e_{i} = u_{i} - \frac{1}{s_{i}}K\exp\left( \xi^{\hat{}} \right)P_{i}\) 线性化:\(e_{i}(x + \text{Δx}) \approx e_{i}\left( x \right) + J\left( x \right)\text{Δx}\) 雅可比的形式?(扰动模型) 定义中间量(e先对p,再对科萨): 第一项: 第二项: 非齐次形式: 两项相乘(得到倒数): 也可以对3D点求导: \(e_{i} = u_{i} - \frac{1}{s_{i}}K\exp\left( \xi^{\hat{}} \right)P_{i}\)

    4 ICP

    给定配对好的两组3D点,求其旋转和平移,可用迭代最近点(Iterative Closest Point,ICP求解)(p 和 p`相对运行关系) 稍加推导:

    1. 定义质心:
    2. 改写目标函数: 目标函数简化为:
  • ICP也可以从非线性优化角度求解,但:
    • 已知匹配时,ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下,只要能找到极小值解,那么这个极小值就是全局最优值。
    • 所以正常情况下,SVD结果和优化一样,且优化很快收敛。
  • 注:
    • 在激光情况下,匹配点未知,将指定最近点为匹配点。此时问题非凸,极小值不一定为最小值。
    • 利用非线性优化可以将ICP与PnP结合在一起求解。