第七讲: 视觉里程计1

第七讲: 视觉里程计1

文章目录

• 第七讲: 视觉里程计1
• 1. 特征点法
• 1.1. 特征点
• 1.2. ORB特征
• 1.3. 特征匹配
• 2. 实践: 特征提取和匹配
• 3. D-2D:对极几何
• 3.1. 对极约束
• 3.2. 本质矩阵
• 3.3. 单应矩阵
• 4. 实践:对极约束求解相机运动
• 5. 三角测量
• 6. 实践:三角测量
• 6.1. 三角测量代码
• 6.2. 讨论
• 7. D-2D:PnP
• 7.1. 直接线性优化(6对匹配点)
• 7.2. P3P(3对匹配点)
• 7.3. Bundle adjustment
• 8. 实践:求解UPnP
• 8.1. 使用ePnP求解
• 8.2. 使用BA优化
• 9. D-3D:ICP
• 9.1. SVD
• 9.2. 非线性优化
• 10. 实践:求解ICP
• 10.1. SVD方法
• 10.2. 非线性优化方法

1. 特征点法

slam分为视觉前端和视觉后端.前端也称为视觉里程计(VO). 它根据相邻图像的信息粗略估计相机的运动,给后端提供较好的初始值.

本讲学习如果提取匹配特征点,然后估计两帧之间的相机运动和场景结构.

1.1. 特征点

图像本身是由亮度和色彩组成的矩阵.

特征点:

• 朴素角点: 简单,受环境,相机旋转等影响.
• 人工设计的特征点: 可重复性,可区别性,高效率性,本地性.

特征点是由关键点和描述子组成:

• 关键点: 图像中的位置
• 描述子: 附加信息,为了更好的区别其他点.

描述子通常是一个向量,按照某种人为设计的方式,描述了该关键点周围像素的信息.设计原则: 外观相似的特征应该有相似的描述子.

ORB(Oriented FAST and Rotated BRIEF) 是目前非常具有代表性的实时图像特征.

ORB是质量与性能较好的折中.

1.2. ORB特征

ORB由关键点和描述子组成.

关键点称为: Oriented FAST.是一种改进的FAST角点.

描述子称为: BRIEF(Binary Robust Independent Elementary Feature).

提取ORB特征点的步骤为:

• FAST角点提取: 找出图像中的角点. 相较于原版FAST,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变性.
• BRIEF描述子: 对前一步中的特征点的周围进行描述.

FAST 关键点:

FAST是一种角点,主要检测局部像素灰度变化明显的地方.特点是速度快.

$FAST特征点: \begin{cases} 优点: 仅比较像素间亮度的差异速度快 \\ 缺点: \begin{cases} 特征点很多且不确定-解决方法:指定要提取的角点数N,选取前N个具有最大响应值的点作为最终角点的 合 \\ 不具有方向信息-解决方法: 添加旋转描述,其特征的旋转由灰度实心法实现 \\ 存在尺度问题-解决方法: 添加尺度描述,尺度不变性由构建图像金字塔,并在金字塔每层检验角点来实现. \end{cases} \end{cases}$FAST特征点:⎩⎪⎪⎪⎨⎪⎪⎪⎧​优点:仅比较像素间亮度的差异速度快缺点:⎩⎪⎨⎪⎧​特征点很多且不确定−解决方法:指定要提取的角点数N,选取前N个具有最大响应值的点作为最终角点的合不具有方向信息−解决方法:添加旋转描述,其特征的旋转由灰度实心法实现存在尺度问题−解决方法:添加尺度描述,尺度不变性由构建图像金字塔,并在金字塔每层检验角点来实现.​​

质心:图像块灰度值作为质心.

操作步骤如下:

1. 在一个小的图像块B中,定义图像块的矩阵为:

   $m_{pq} = \sum_{x,y \in B}x^py^qI(x,y),\qquad p,q=\{0,1\}$mpq​=x,y∈B∑​xpyqI(x,y),p,q={0,1}

2. 通过矩可以找到图像块的质心

   $C = (\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$C=(m00​m10​​,m00​m01​​)

3. 连接图形块的几何中心O与质心C, 得到一个方向向量 $\overrightarrow{OC}$OC ,于是特征点的方向可以定义为:

   $\theta = \arctan(\frac{m_{01}}{m_{10}})$θ=arctan(m10​m01​​)

通过以上信息,FAST便具有了尺度与旋转的描述.提升了健壮性. 在ORB中把这种改进后的FAST称为:Oriented FAST.

BRIEF描述子:

提取关键点后对每个点计算其描述子.

BRIEF是一种二进制描述子. 例如,取关键点附近的p和q,如果p比q大,则取1,否则取0,如果去了128对这样的点,那么描述子可以用一个128位的二进制数来表示.

与原始的BRIEF相比,ORB描述子具有良好的旋转不变性.

1.3. 特征匹配

特征匹配解决了SLAM中数据关联问题,即确定当前看到的路标与之前看到的路标之间的对于关系.

通过对图像与图像之间或者图像与地图之间的描述进行准确匹配,我们可以为后续姿态估计,优化等操作减轻大量负担.

如何两个图片中特征点集合的对应关系呢?

• 暴力匹配: 对每个特征点都测量描述子的距离,然后排序,取得最近的一个作为匹配点.

描述子的距离,
对于浮点型的描述子使用欧氏距离
对于二进制类型的描述子使用汉明距离

• 快速近似最邻近匹配: 更适合与匹配点的情况.它已经集成到了opencv当中.

2. 实践: 特征提取和匹配

目前主流的几种特征提速方法在OpenCV中已经集成.

以下为openCV中图像特征提取,计算和匹配的过程.

代码见:code\第七讲

我们希望根据匹配的点对来估计相机的运动.

1. 当相机为单目时,我们只知道2D的像素坐标,因而问题是根据两组2D点估计运动.该问题用对极几何来解决.
2. 当相机为双目,RGB等,即有距离信息. 那么就根据3D点估计运动. 常用ICP来解决.
3. 如果有3D点及在其他相机的投影位置,也能估计相机的运动. 该问题是通过PnP求解.

3. D-2D:对极几何

3.1. 对极约束

在第一帧的坐标系下,设P的空间位置为;
$P = [X,Y,Z]^T$P=[X,Y,Z]T

根据第五讲针孔相机模型,我们知道两个像素点 $p_1,p_2$p1​,p2​的像素位置为:
$s_1p_1 = KP , \quad s_2p_2 = K(RP+t)$s1​p1​=KP,s2​p2​=K(RP+t)

K为相机的内参矩阵. $R,t$R,t为相机的运动,也可以写成李代数的形式.

如果使用齐次坐标,那么上式也可以写成如下形式;
$p_1 = KP , \quad p_2 = K(RP+t)$p1​=KP,p2​=K(RP+t)

取:
$x_1 = K^{-1}p_1, \quad x_2 = K^{-1}p_2$x1​=K−1p1​,x2​=K−1p2​

$x_1,x_2$x1​,x2​是两个像素点 归一化平面的坐标.带入上式得:
$x_2 = Rx_1 + t$x2​=Rx1​+t

两边同时左乘 $t^\wedge$t∧(^表示将向量变成矩阵),相当于两侧同时与t做外积:
$t^\wedge x_2 = t^\wedge Rx_1$t∧x2​=t∧Rx1​

然后同时左乘 $x_2^T$x2T​
$x_2^Tt^\wedge x_2 = x_2^T t^\wedge Rx_1$x2T​t∧x2​=x2T​t∧Rx1​

因为 $t^\wedge x_2 = 0$t∧x2​=0,所以上式化简为:
$x_2^Tt^\wedge R x_1 = 0$x2T​t∧Rx1​=0

重新带入 $p_1,p_2$p1​,p2​得:
$p_2^TK^{-T} t^\wedge RK^{-1} p_1 = 0$p2T​K−Tt∧RK−1p1​=0

以上两个式子都成为 对极约束.

它的几何意义是 $O_1,P,O_2$O1​,P,O2​三者共面. 対极约束中同时包含了 平移和旋转.

把中间部分记做两个矩阵:

• F : 基础矩阵
• E : 本质矩阵

于是进一步简化対极约束;
$E = r^\wedge R , \quad F = K^{-T}EK^{-1}, \quad x_2^TEx_1 = p_2^TFp_1 = 0$E=r∧R,F=K−TEK−1,x2T​Ex1​=p2T​Fp1​=0

$対极约束简洁的给出了两个匹配点的空间位置关系.$対极约束简洁的给出了两个匹配点的空间位置关系. 于是相机的位姿估计可以分为以下步骤:

1. 根据匹配点的像素位置求出E 或者 F
2. 根据E或者F 求出 $R,t$R,t

由于E和F只相差了相机内参,所以往往使用更简洁的EK

3.2. 本质矩阵

本质矩阵 $E=t^\wedge R$E=t∧R. 它是一个 $3\times 3$3×3矩阵. 从E的构造方式看,有以下几点需要注意:

1. 本质矩阵由対极约束定义.由于対极约束是等式为0的,所以对E乘以任意非零常数, 対极约束仍然满足. 我们把这件事称为E在不同尺度下是等价的.
2. 根据 $E=t^\wedge R$E=t∧R ,课证明本质矩阵E的奇异值必定是 $[\sigma,\sigma,0]^T$[σ,σ,0]T的形式. 这称为 $本质矩阵的内在性质.$本质矩阵的内在性质.
3. 平移加旋转共有6个自由度,所以 $t^\wedge R$t∧R共有6个自由度.但是由于 尺度等价性, 实际上只有5个自由度.

使用8点法求解E.

$\begin{bmatrix} u_1^1u_2^1 & u_1^1v_2^1 & u_1^1 & v_1^1u_2^1 & v_1^1v_2^1 & v_1^1 & u_2^1 & v_2^1 &1 \\ u_1^2u_2^2 & u_1^2v_2^2 & u_1^2 & v_1^2u_2^2 & v_1^2v_2^2 & v_1^2 & u_2^2 & v_2^2 &1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ u_1^8u_2^8 & u_1^8v_2^8 & u_1^8 & v_1^8u_2^8 & v_1^8v_2^8 & v_1^8 & u_2^8 & v_2^8 &1 \end{bmatrix} \begin{bmatrix} e_1 \\ e_2 \\ e_3 \\ e_4 \\ e_5 \\ e_6 \\ e_7 \\ e_8 \end{bmatrix} = 0$⎣⎢⎢⎢⎡​u11​u21​u12​u22​⋮u18​u28​​u11​v21​u12​v22​⋮u18​v28​​u11​u12​⋮u18​​v11​u21​v12​u22​⋮v18​u28​​v11​v21​v12​v22​⋮v18​v28​​v11​v12​⋮v18​​u21​u22​⋮u28​​v21​v22​⋮v28​​11⋮1​⎦⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡​e1​e2​e3​e4​e5​e6​e7​e8​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤​=0

一声个方程构成了线性方程组. 它的系数矩阵由特征点位置构成,大小为 $8\times 9$8×9. e位于该矩阵的零空间中. 如果系数矩阵的秩为8,那么零空间的维数为1,也就是e构成一条线.

如果系数矩阵的秩为8,那么上述方程可以解得E的值.

接下来根据估计的本质矩阵E,恢复出相机运动R,t.这个过程由奇异值分解(SVD)得到.
$E = U \Sigma V^T$E=UΣVT

$U,V$U,V为正交矩阵, $\Sigma$Σ为奇异值矩阵.
根据E的内在性质, $\Sigma = diag(\sigma,\sigma,0)$Σ=diag(σ,σ,0)

对于任意E,存在两个可能的 $t,R$t,R与之对应:
$t_1^\wedge = UR_Z(\frac{\pi}{2})\Sigma U^T, \quad R_1 = UR_Z^T(\frac{\pi}{2})V^Tt \newline t_2^\wedge = UR_Z(-\frac{\pi}{2})\Sigma U^T, \quad R_2 = UR_Z^T(-\frac{\pi}{2})V^Tt$t1∧​=URZ​(2π​)ΣUT,R1​=URZT​(2π​)VTtt2∧​=URZ​(−2π​)ΣUT,R2​=URZT​(−2π​)VTt

$R_Z(\frac{\pi}{2})$RZ​(2π​)表示旋转90度的旋转矩阵. 同时,由于 $-E 等价于 E$−E等价于E,所以任意一个t取负号,会得到同样的结果. 因此,从E分解得到 $t,R$t,R,一个存在4种结果.

如下图;

最后还有一个问题,线性方程组求解出E后,它可能不满足E的内在性质:奇异值不为 $(\sigma, \sigma, 0)$(σ,σ,0) 的形式.
对8点大求得的E进行SVD分解后,会得到奇异值矩阵 $\Sigma = diag(\sigma,\sigma,0)$Σ=diag(σ,σ,0).取( $\sigma _1 \geqslant \sigma _2 \geqslant \sigma _3$σ1​⩾σ2​⩾σ3​):

$E = U diag(\frac{\sigma_1+\sigma_2}{2},\frac{\sigma_1+\sigma_2}{2},0)V^T$E=Udiag(2σ1​+σ2​​,2σ1​+σ2​​,0)VT

最简单的方法是将奇异值取 $diag(1,1,0)$diag(1,1,0)

3.3. 单应矩阵

单应矩阵H:它描述了两个平面之间的映射关系.

单应矩阵通常扫描处于共平面上的一些点在两张图像之间的变换关系.

经过计算我们可以得到一个直接描述图像坐标 $p_1$p1​到 $p_2$p2​之间的变换,把中间部分记做H:
$p_2 = H p_1$p2​=Hp1​

把上式展开:
$\begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix}= \begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}$⎣⎡​u2​v2​1​⎦⎤​=⎣⎡​h1​h4​h7​​h2​h5​h8​​h3​h6​h9​​⎦⎤​⎣⎡​u1​v1​1​⎦⎤​

在实际中通常乘以一个非零因子,使得 $h_9 = 1$h9​=1,于是有:
$u_2 = \frac{h_1u_1+h_2v_2+h_3}{h_7u_1+h_8v_1+h_9} \newline \quad\newline v_2 = \frac{h_4u_1+h_5v_1+h_6}{h_7u_1+h_8v_1+h_9}$u2​=h7​u1​+h8​v1​+h9​h1​u1​+h2​v2​+h3​​v2​=h7​u1​+h8​v1​+h9​h4​u1​+h5​v1​+h6​​

于是自由度为8的单应矩阵可以通过4对匹配特征点算出.即求解以下方程:
$(\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{u}_2^2& -v_1^2\end{array}$+h5​v1​+h6​​

于是自由度为8的单应矩阵可以通过4对匹配特征点算出.即求解以下方程:
$(\begin{array}{cccccccc}{u}_1^1& v_1^1& 1& 0& 0& 0& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ 0& 0& 0& u_1^1& v_1^1& 1& -u_1^1{u}_2^1& -v_1^1{u}_2^1\\ u_1^2& v_1^2& 1& 0& 0& 0& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ 0& 0& 0& u_1^2& v_1^2& 1& -u_1^2{u}_2^2& -v_1^2{u}_2^2\\ u_1^3& v_1^3& 1& 0& 0& 0& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ 0& 0& 0& u_1^3& v_1^3& 1& -u_1^3{u}_2^3& -v_1^3{u}_2^3\\ u_1^4& v_1^4& 1& 0& 0& 0& -u_1^4{u}_2^4& -v_1^4{u}_2^4\\ 0& 0& 0& u_1^4& v_1^4& 1& -u_1^4{u}_2^4& -v_1^4\end{array}$