文章目录
- 07 视觉里程计 2
- 7.1 直接法的引出
- 7.2 2D 光流
- 7.2.1 Lucas-Kanade 光流
- 7.2.1 实践：LK 光流
- 7.3 直接法
- 7.3.1 推导过程
- 7.3.2 直接法的优缺点
07 视觉里程计 2
7.1 直接法的引出
特征点的缺点：
-
关键点的提取与描述子的计算非常耗时，实时性差；
-
使用特征点时，忽略了特征点以外的所有信息，而丢弃了部分可能有用的信息；
-
有时会出现特征缺失的情况，如白墙或者空荡荡的走廊等。
为了克服上述问题，提出了 光流法 和 直接法。
光流法：只计算关键点，不计算描述子，采用光流跟踪。
直接法：由光流法演变而来。根据像素灰度信息同时估计相机运动和点的投影，不要求必须为角点，甚至可以是随机点。
7.2 2D 光流
光流描述了像素随时间在图像之间运动的过程。其中，计算部分像素的运动称为 稀疏光流，计算所有像素的运动称为 稠密光流。下面主要介绍以 LK 光流为代表的稀疏光流。
7.2.1 Lucas-Kanade 光流
（1）我们认为图像是随时间变化的，也就是说图像可以看做时间的函数，那么，一个在 t t t 时刻，位于 ( x , y ) (x, y) (x,y) 处的像素，它的灰度可以写成
I ( x , y , t ) \boldsymbol{I}(x,y,t) I(x,y,t)
首先，引入光流法的基本假设：
灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。
（2）对于 t t t 时刻位于 ( x , y ) (x,y) (x,y) 处的像素，在 t + d t t+\mathrm{d}t t+dt 时刻运动到 ( x + d x , y + d y ) (x+\mathrm{d}x,y+\mathrm{d}y) (x+dx,y+dy) 处，根据灰度不变假设
I ( x + d x , y + d y , t + d t ) = I ( x , y , t ) (7-1) \boldsymbol{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\boldsymbol{I}(x,y,t) \tag{7-1} I(x+dx,y+dy,t+dt)=I(x,y,t)(7-1)
对左边进行泰勒展开，并保留一阶项：
I ( x + d t , y + d y , t + d t ) ≈ I ( x , y , t ) + ∂ I ∂ x d x + ∂ I ∂ y d y + ∂ I ∂ t d t (7-2) \boldsymbol{I}(x+\mathrm{d}t,y+\mathrm{d}y,t+\mathrm{d}t)\approx\boldsymbol{I}(x,y,t)+\frac{\partial \boldsymbol{I}}{\partial x}\mathrm{d}x+\frac{\partial \boldsymbol{I}}{\partial y}\mathrm{d}y+\frac{\partial \boldsymbol{I}}{\partial t}\mathrm{d}t \tag{7-2} I(x+dt,y+dy,t+dt)≈I(x,y,t)+∂x∂Idx+∂y∂Idy+∂t∂Idt(7-2)
由灰度不变，所以
∂ I ∂ x d x + ∂ I ∂ y d y + ∂ I ∂ t d t = 0 (7-3) \frac{\partial \boldsymbol{I}}{\partial x}\mathrm{d}x+\frac{\partial \boldsymbol{I}}{\partial y}\mathrm{d}y+\frac{\partial \boldsymbol{I}}{\partial t}\mathrm{d}t=0 \tag{7-3} ∂x∂Idx+∂y∂Idy+∂t∂Idt=0(7-3)
两边同除 d t \mathrm{d}t dt，得
∂ I ∂ x d x d t + ∂ I ∂ y d y d t = − ∂ I ∂ t (7-3) \frac{\partial \boldsymbol{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial \boldsymbol{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial \boldsymbol{I}}{\partial t} \tag{7-3} ∂x∂Idtdx+∂y∂Idtdy=−∂t∂I(7-3)
其中， d x / d t {\mathrm{d}x}/{\mathrm{d}t} dx/dt、 d y / d t {\mathrm{d}y}/{\mathrm{d}t} dy/dt 分别为像素在 x x x、 y y y 轴上运动的速度，记为 u u u、 v v v； ∂ I / ∂ x {\partial \boldsymbol{I}}/{\partial x} ∂I/∂x、 ∂ I / ∂ y {\partial \boldsymbol{I}}/{\partial y} ∂I/∂y 分别为图像在 x x x、 y y y 方向上的梯度，记为 I x \boldsymbol{I}_x Ix、 I y \boldsymbol{I}_y Iy；把像素灰度对时间的变化量记为 I t \boldsymbol{I}_t It。那么上式写成矩阵形式
[ I x I y ] [ u v ] = − I t (7-4) \left[\begin{array}{ll} \boldsymbol{I}_{x} & \boldsymbol{I}_{y} \end{array}\right]\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{I}_{t} \tag{7-4} [IxIy][uv]=−It(7-4)
我们希望求出 u u u 和 v v v，但上式是一个二元一次方程，条件不足，因此还需引入额外的约束。假设在一个大小为 w × w w \times w w×w 的图像窗口中，这 w 2 w^2 w2 个像素都具有相同的运动，那么就可以得到 w 2 w^2 w2 个方程：
[ I x I y ] k [ u v ] = − I t k ， k = 1 , 2 , . . . , w 2 (7-5) \left[\begin{array}{ll} \boldsymbol{I}_{x} & \boldsymbol{I}_{y} \end{array}\right]_k\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{I}_{tk}，\quad k=1,2,...,w^2 \tag{7-5} [IxIy]k[uv]=−Itk，k=1,2,...,w2(7-5)
令
A = [ [ I x , I y ] 1 ⋮ [ I x , I y ] k ] , b = [ I t 1 ⋮ I t k ] \boldsymbol{A}=\left[\begin{array}{c} {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{1}} \\ \vdots \\ {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{k}} \end{array}\right], \boldsymbol{b}=\left[\begin{array}{c} \boldsymbol{I}_{t 1} \\ \vdots \\ \boldsymbol{I}_{t k} \end{array}\right] A= [Ix,Iy]1⋮[Ix,Iy]k ,b= It1⋮Itk
则
A [ u v ] = − b (7-6) \boldsymbol{A}\left[\begin{array}{l} u \\ v \end{array}\right]=-b \tag{7-6} A[uv]=−b(7-6)
这又是一个超定方程，采用最小二乘解，即
[ u v ] ∗ = − ( A T A ) − 1 A T b (7-7) \left[\begin{array}{l} u \\ v \end{array}\right]^*=-(\boldsymbol{A}^{\mathrm{T}}\boldsymbol{A})^{-1}\boldsymbol{A}^{\mathrm{T}}\boldsymbol{b} \tag{7-7} [uv]∗=−(ATA)−1ATb(7-7)
7.2.1 实践：LK 光流
7.3 直接法
7.3.1 推导过程
如图，已知空间点
P
P
P 的世界坐标为
[
X
,
Y
,
Z
]
T
[X,Y,Z]^\mathrm{T}
[X,Y,Z]T，在两帧图像上的像素坐标分别为
p
1
\boldsymbol{p}_1
p1、
p
2
\boldsymbol{p}_2
p2（未知）。
我们希望求出第一个相机到第二个相机的相对位姿变换，以第一个相机为初始参照系，经旋转和平移 R \boldsymbol{R} R、 t \boldsymbol{t} t 到第二个相机，有
p
1
=
[
u
v
1
]
1
=
1
Z
1
K
P
\boldsymbol{p}_1=\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]_1=\frac{1}{Z_1}\boldsymbol{KP}
p1=
uv1
1=Z11KP
p
2
=
[
u
v
1
]
2
=
1
Z
2
K
(
R
P
+
t
)
=
1
Z
2
K
(
T
P
)
1
:
3
(7-8)
\boldsymbol{p}_2=\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]_2=\frac{1}{Z_2}\boldsymbol{K}(\boldsymbol{RP+t})=\frac{1}{Z_2}\boldsymbol{K}(\boldsymbol{TP})_{1:3} \tag{7-8}
p2=
uv1
2=Z21K(RP+t)=Z21K(TP)1:3(7-8)
直接法中，由于没有特征点匹配，我们无法知道哪一个 p 1 \boldsymbol{p}_1 p1 和 p 2 \boldsymbol{p}_2 p2 对应着同一个点。于是，可以通过优化相机位姿，来寻找与 p 1 \boldsymbol{p}_1 p1 更相似的 p 2 \boldsymbol{p}_2 p2。这里优化的是 光度误差 ，也就是两个像素的亮度误差：
e = I 1 ( p 1 ) − I 2 ( p 2 ) (7-9) e=\boldsymbol{I}_1(\boldsymbol{p}_1)-\boldsymbol{I}_2(\boldsymbol{p}_2) \tag{7-9} e=I1(p1)−I2(p2)(7-9)
注意，这里的 e e e 是标量。目标优化函数为
min T J ( T ) = ∥ e ∥ 2 (7-10) \min_{\boldsymbol{T}}J(\boldsymbol{T})=\|e\|^2 \tag{7-10} TminJ(T)=∥e∥2(7-10)
假设一个空间点在各个视角下成像的灰度是不变的，我们有许多个空间点 P i P_i Pi，那么，整个相机位姿估计问题变为
min T J ( T ) = ∑ i = 1 N e i T e i , e i = I 1 ( p 1 , i ) − I 2 ( p 2 , i ) (7-11) \min _{\boldsymbol{T}} J(\boldsymbol{T})=\sum_{i=1}^{N} e_{i}^{\mathrm{T}} e_{i}, \quad e_{i}=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1, i}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2, i}\right) \tag{7-11} TminJ(T)=i=1∑NeiTei,ei=I1(p1,i)−I2(p2,i)(7-11)
定义
q
=
T
P
\boldsymbol{q=TP}
q=TP
u
=
1
Z
K
q
\boldsymbol{u}=\frac{1}{Z}\boldsymbol{Kq}
u=Z1Kq
那么，误差 e e e 关于 位姿 T \boldsymbol{T} T 的导数为
∂ e ∂ T = ∂ I 2 ∂ u ∂ u ∂ q ∂ q ∂ δ ξ δ ξ (7-12) \frac{\partial e}{\partial \boldsymbol{T}}=\frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}}\frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}\frac{\partial \boldsymbol{q}}{\partial \delta\boldsymbol{\xi}}\delta\boldsymbol{\xi} \tag{7-12} ∂T∂e=∂u∂I2∂q∂u∂δξ∂qδξ(7-12)
其中， δ ξ \delta\boldsymbol{\xi} δξ 为 T \boldsymbol{T} T 的左扰动。分别看每一项：
（1）第一项 ∂ I 2 / ∂ u {\partial \boldsymbol{I}_2}/{\partial \boldsymbol{u}} ∂I2/∂u 为 u \boldsymbol{u} u 处的像素梯度；
（2）第二项 ∂ u / ∂ q {\partial \boldsymbol{u}}/{\partial \boldsymbol{q}} ∂u/∂q，也就是像素坐标关于相机坐标的导数，记 q = [ X , Y , Z ] T \boldsymbol{q}=[X,Y,Z]^\mathrm{T} q=[X,Y,Z]T，则有
∂ u ∂ q = [ ∂ u ∂ X ∂ u ∂ Y ∂ u ∂ Z ∂ v ∂ X ∂ v ∂ Y ∂ v ∂ Z ] = [ f x Z 0 − f x X Z 2 0 f y Z − f y Y Z 2 ] (7-13) \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}=\left[\begin{array}{lll} \frac{\partial u}{\partial X} & \frac{\partial u}{\partial Y} & \frac{\partial u}{\partial Z} \\ \frac{\partial v}{\partial X} & \frac{\partial v}{\partial Y} & \frac{\partial v}{\partial Z} \end{array}\right]=\left[\begin{array}{ccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} \end{array}\right] \tag{7-13} ∂q∂u=[∂X∂u∂X∂v∂Y∂u∂Y∂v∂Z∂u∂Z∂v]=[Zfx00Zfy−Z2fxX−Z2fyY](7-13)
（3）第三项 ∂ q / ∂ δ ξ {\partial \boldsymbol{q}}/{\partial \delta\boldsymbol{\xi}} ∂q/∂δξ 为变换后的三维点对李代数的导数，前面已经推导
∂ q ∂ δ ξ = [ I , − q ∧ ] (7-14) \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{q}^{\wedge}\right] \tag{7-14} ∂δξ∂q=[I,−q∧](7-14)
将后两式合并（见式（6-43）），得
∂ u ∂ δ ξ = [ f x Z 0 − f x X Z 2 − f x X Y Z 2 f x + f x X 2 Z 2 − f x Y Z 0 f y Z − f y Y Z 2 − f y − f y Y 2 Z 2 f y X Y Z 2 f y X Z ] (7-15) \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{cccccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} & -\frac{f_{x} X Y}{Z^{2}} & f_{x}+\frac{f_{x} X^{2}}{Z^{2}} & -\frac{f_{x} Y}{Z} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} & -f_{y}-\frac{f_{y} Y^{2}}{Z^{2}} & \frac{f_{y} X Y}{Z^{2}} & \frac{f_{y} X}{Z} \end{array}\right] \tag{7-15} ∂δξ∂u=[Zfx00Zfy−Z2fxX−Z2fyY−Z2fxXY−fy−Z2fyY2fx+Z2fxX2Z2fyXY−ZfxYZfyX](7-15)
于是，推导出误差关于李代数的雅克比矩阵：
J = − ∂ I 2 ∂ u ∂ u ∂ δ ξ (7-16) \boldsymbol{J}=-\frac{\partial \boldsymbol{I}_2}{\partial \boldsymbol{u}}\frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\boldsymbol{\xi}}} \tag{7-16} J=−∂u∂I2∂δξ∂u(7-16)
采用迭代优化的方法求解出式（7-11）的最优解。
7.3.2 直接法的优缺点
（1）直接法优点
-
可以省去计算特征点、描述子的时间；
-
只需要像素梯度即可，不需要特征点；
-
可构件半稠密乃至稠密地图，这是特征点法做不到的。
（2）直接法缺点
-
非凸性；
-
单个像素没有区分度：选点少时效果较差，一般用 500 个点以上；
-
灰度不变假设是很强的假设，相机会自动调整曝光参数、或者光照变化，都会使得图像整体亮度发生变化。
