视觉SLAM学习打卡【7-1】-视觉里程计·特征点法

视觉里程计根据相邻图像信息估计出粗略的相机运动（即位姿）。视觉里程计算法主要分为两类：特征点法和直接法。本节主要介绍基于特征点法的特征提取、匹配 & 估计相机位姿、深度信息.

一、特征提取&匹配

特征点：关键点+描述子
关键点：角点 / 区块 / 边缘
描述子：描述关键点周围像素信息
—— —— —— —— —— —— —— ——
ORB特征：Oriented FAST关键点+BRIEF描述子
FAST角点：像素p半径为3的圆上的16个像素点存在N个的亮度大于/小于p亮度的120%/80%，即认为p为角点.
改进的 Oriented FAST角点

• FAST角点不具有方向性和尺度不变性
• 尺度不变性 由构建图像金字塔，并在每一层上检测角点实现
• 方向性 计算图像灰度质心，连接灰度质心和几何中心得到方向向量
  图像块矩（ I(x,y)为像素灰度值 ）： m p q = ∑ x , y ∈ B x p y q I ( x , y ) , p , q = { 0 , 1 } . m_{pq}=\sum_{x,y\in B}x^py^qI(x,y),\quad p,q=\{0,1\}. mpq​=x,y∈B∑​xpyqI(x,y),p,q={0,1}.质心：$$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$$方向向量：$$\theta =\arctan (m_{01}/m_{10})$$

BRIEF描述子：二进制描述子，描述向量由许多0、1构成。取关键点附近的n两个随即像素对p、q，若p>q，取1；反之，取0。
—— —— —— —— —— —— —— ——
特征匹配原理：外观相似的特征点具有相似的描述子（两个关键点周围的描述子在向量空间上的距离相近）

• 暴力匹配法
• 快速近似最近邻（FLANN）

相似距离度量范数

• 欧氏距离：适用于浮点型描述子
• 汉明距离：适用于二进制描述子，汉明距离指不同位数的个数

特征点匹配以后得到的是两个像素坐标系下的点p1和p2

二、回顾相机投影顺序

三、估计相机位姿&空间点深度

1. 2D-2D

对极几何求位姿，三角测量求深度
（1）对极几何
归一化平面坐标用的x1，x2
$$x_2=R{x}_1+t$$两边都左乘t^ ,t^乘以一个向量，相当于t和这个向量做叉乘(t自己做叉积为0)
$$t^{\wedge }{x}_2=t^{\wedge }R{x}_1$$等式两边同左乘$x_2^T$
$$x_2^T{t}^{\wedge }R{x}_1=0$$其中，本质矩阵E=t^R
像素坐标用的p1，p2
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{\mathbf{p}}_1=0$$其中，基础矩阵F=$K^{-T}{t}^{\wedge }R{K}^{-1}$
因此，对极约束可以写为：$$X_2^{T}E{X}_1=P_2^{T}F{P}_1=0$$
（2）求解本质矩阵E
采用八点法，即用8对匹配点组成线性方程组AX=0.$$E=(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array})$$一对匹配点满足以下对极约束$$\left(u_2,v_2,1\right)\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=0.$$把E写成向量形式$$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^{\mathrm{T}}$$则对极约束可以写成如下线性形式$$[u_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2{v}_1,v_2,u_1,v_1,1]\cdot e=0.$$当采用8个点时（条件：系数矩阵满秩。因为用8个方程求9个未知量，当且仅当有n-r=9-8=1个极大无关组，自由度为8）$$\left(\begin{array}{ccccccccc}{u}_2^1{u}_1^1& u_2^1{v}_1^1& u_2^1& v_2^1{u}_1^1& v_2^1{v}_1^1& v_2^1& u_1^1& v_1^1& 1\\ u_2^2{u}_1^2& u_2^2{v}_1^2& u_2^2& v_2^2{u}_1^2& v_2^2{v}_1^2& v_2^2& u_1^2& v_1^2& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ u_2^8{u}_1^8& u_2^8{v}_1^8& u_2^8& v_2^8{u}_1^8& v_2^8{v}_1^8& v_2^8& u_1^8& v_1^8& 1\end{array}\right)\left(\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0.$$
（3）SVD分解拆开E得R、t
$$E=U\Sigma V^T$$奇异值分解出来是4个解，但是只有一个对于两个相机而言，都是正的深度。奇异值SVD分解数学推导参考
（4）单应矩阵H

• 单应矩阵(Homography)的意义在于避免“退化”现象造成基础矩阵自由度下降的问题。一般出现退化现象是因为特征点共面或者相机发生纯旋转。
• 对于纯旋转的情况，E=t^R，纯旋转意味着t是0，那么E就是0，根据E恢复R就完全无从谈起。
• 对于特征点共面的情况，可以由其中两个点表示面上其余所有点，使得自由度下降。
• 现实数据包含噪声，若用八点法求解，本质矩阵E多出来的自由度将有噪声决定，为避免退化，采用单应矩阵H。

单应矩阵的求解及SVD分解此处不作为重点 详细推导可参考
（5）总结

• 本质矩阵E
  $x_2^T{t}^{\wedge }R{x}_1=0$，x是归一化相机平面上的点，E有5个自由度（E存在尺度等价性，即$X_2^{T}E{X}_1=0$等式两边乘任何数都不影响结果，$E=t^{\Lambda }R$，t3维，R3维，6-1=5），最少五个点，但是用8点法.
• 基础矩阵F
  $p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1=0$，p是像素平面上的点，F有7个自由度($F_{3\times 3}$秩为2，9-2=7)，最少7个点，一般也用8点法.
• 单应矩阵H
  $\begin{array}{rl}{p}_2& =K\left(R-\frac{t{n}^T}{d}\right)K^{-1}{p}_1\end{array}$，p是像素平面上的点，H有8个自由度（实际处理中令$H_{3\times 3}$中的$h_9$=1），最少四个匹配点，因为每对匹配点可以提供两个约束。

（6）三角测量

• 有同一个点在两个图里的归一化相机坐标x1和x2
• 现在都变到相机坐标系下，也就是都乘以各自的第三维s
• 把s2x2变换到s1x1相机坐标系下，因为是同一个点，所以肯定相同
  $$s_1{\mathbf{x}}_1=s_2\mathbf{R}{x}_2+t$$
• 两边都左乘x1^$$0=s_2{\mathbf{x}}_1^{\wedge }R{\mathbf{x}}_2+{\mathbf{x}}_1^{\wedge }\mathbf{t}$$ 其中，R、t、x1、x2均已知，只剩下一个未知量s2.

2. 3D-2D（PnP）

已知点的3D坐标和这三个点的2D坐标
（1）直接线性变化法DLT
3D：世界坐标系下的齐次坐标 $p=(X,Y,Z,1)$
2D：相机归一化坐标系下的坐标 $x=(u_1,v_1,1)$
$$s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$定义中间的3x4矩阵为增广矩阵[R|t]
用最后一行消去s
$$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}},v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}.$$用下式简化表示$${\mathbf{t}}_1=(t_1,t_2,t_3,t_4{)}^{\mathrm{T}},{\mathbf{t}}_2=(t_5,t_6,t_7,t_8{)}^{\mathrm{T}},{\mathbf{t}}_3=(t_9,t_{10},t_{11},t_{12}{)}^{\mathrm{T}}$$则可得两个约束条件$$\begin{array}{c}{t}_1^{T}P-t_3^{T}P{u}_1=0,\\ \\ t_2^{T}P-t_3^{T}P{v}_1=0.\end{array}$$得用6对匹配点求出增广矩阵，需要对左边的3*3的矩阵块用一个最好的旋转矩阵来近似（R存在正交约束，而求解时只把其当成简单方程组），可以用QR分解完成。
（2）P3P
3对3D-2D匹配点
3D:世界坐标系下的坐标
2D：相机坐标系下坐标
此处只给出P3P求解思路：通过3个相似、余弦定理得到两个约束方程，利用吴消元法求出解析解，得到空间点在相机坐标系下的3D坐标，转化为3D-3D问题求位姿（ICP）.
（3）Bundle Adjustment（BA）
把相机位姿和空间点位置都看成优化变量（此类问题统称为BA），构建重投影误差，让重投影误差（观测值减去预测值）最小化。
3D：世界坐标系下的齐次坐标$P_i=[x_i,y_i,z_i]$
2D：像素坐标系下的坐标 $u_i=[u_i,v_i]$
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=\mathbf{K}\mathbf{T}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$写成矩阵形式$$s_i{u}_i=KT{P}_i$$
转化为最小二乘问题$$T^{\ast }=\arg \underset{T}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert u_i-\frac{1}{s_i}KT{P}_i\Vert }_2^2.$$

3. 3D-3D（ICP）

已知两组匹配好的3D点，进行位姿估计.
P = { p 1 , ⋯   , p n } , P ′ = { p 1 ′ , ⋯   , p n ′ } P=\{p_{1},\cdots,p_{n}\},P^{\prime}=\{p_{1}^{\prime},\cdots,p_{n}^{\prime}\} P={p1​,⋯,pn​},P′={p1′​,⋯,pn′​}$$\forall i,p_i=R{p}_i^{\prime }+t.$$（1）SVD方式
定义第i对误差项$$e_i=p_i-(R{p}_i^{\prime }+t).$$构造最小二乘问题$$\underset{\mathbf{R},\mathbf{t}}{\min }\frac{1}{2}\sum _{i=1}^n\Vert (p_i-({\mathbf{R}{\mathbf{p}}_{\mathbf{i}}}^{\prime }+\mathbf{t})){\Vert }_2^2.$$定义两组质心（不带下标）$$p=\frac{1}{n}\sum _{i=1}^n(p_i),p^{\prime }=\frac{1}{n}\sum _{i=1}^n(p_i^{\prime }).$$对误差函数做如下处理$$\begin{array}{rl}\frac{1}{2}\sum _{i=1}^n{\Vert p_i-\left(R{p}_i^{\prime }+t\right)\Vert }^2& =\frac{1}{2}\sum _{i=1}^n{\Vert p_i-R{p}_i{}^{\prime }-t-p+R{p}^{\prime }+p-R{p}^{\prime }\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n{\Vert \left(p_i-p-R\left(p_i^{\prime }-p^{\prime }\right)\right)+\left(p-R{p}^{\prime }-t\right)\Vert }^2\\ & =\frac{1}{2}\sum _{i=1}^n{(\Vert p_i-p-R{(p_i}^{\prime }-p^{\prime })\Vert }^2+{\Vert p-R{p}^{\prime }-t\Vert }^2+\\ & 2{\left(p_i-p-R\left(p_i^{\prime }-p^{\prime }\right)\right)}^T\left(p-R{p}^{\prime }-t\right)).\end{array}$$交叉项$\left(p_i-p-R\left(p_i^{\prime }-p^{\prime }\right)\right)$求和后为0，优化目标函数化简为$$\underset{R,t}{\min }J=\frac{1}{2}\sum _i^n{\Vert p_i-p-R\left(p_i^{\prime }-p^{\prime }\right)\Vert }^2+{\Vert p-R{p}^{\prime }-t\Vert }^2.$$可以发现，左边一项只和R有关，右边一项和R、t有关。因此，可以先根据左边项求出最优R，再通过$t^{\ast }=p-R{p}^{\prime }.$求解t。
计算去质心坐标$$q_i=p_i-p,q_i^{\prime }=p_i^{\prime }-p^{\prime }$$则R优化项变为$$R^{\ast }=\arg \underset{R}{\min }\frac{1}{2}\sum _{i=1}^n{\Vert q_i-R{q}_i^{\prime }\Vert }^2.$$展开$$\frac{1}{2}\sum _{i=1}^n{\Vert q_i-R{q}_i^{\prime }\Vert }^2=\frac{1}{2}\sum _{i=1}^n\left(q_i^{\mathrm{T}}{q}_i+q_i^{\prime \mathrm{T}}{R}^{\mathrm{T}}R{q}_i^{\prime }-2q_i^{\mathrm{T}}R{q}_i^{\prime }\right).$$前两项与R无关，因为第二项R和自身转置乘积为I（R为正交矩阵），就是优化第三项，则优化目标函数再变为$$\sum _{i=1}^n-q_i^{\mathrm{T}}R{q}_i^{\prime }=\sum _{i=1}^n-\mathrm{tr}\left(R{q}_i^{\prime }{q}_i^{\mathrm{T}}\right)=-\mathrm{tr}\left(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^{\mathrm{T}}\right)$$（迹的性质：$a^{T}b=tra(b{a}^T)$）
之后定义了W（$W={\sum }_{i=1}^n{q}_i{q}_i^{\prime T}$），对W进行SVD分解，W满秩的时候，$R=U{V}^T$，然后根据R恢复t。为什么$R=U{V}^T$
（2）非线性优化
类似于PnP，转化为最小二乘问题，以迭代方式求最优解