光流法与直接法在SLAM中的应用

本文总结视觉SLAM中常用的光流法与直接法

1、Lucas-Kanade光流法

相机所拍摄到的图像随相机视角的变化而变化，这种变化也可以理解为图像中像素的反向移动。“光流”（Optical Flow）是指通过分析连续图像帧来估计场景中像素或特征点的运动的技术，即根据连续的两张图片和已知某个固定的空间点在 t t t时刻对应的的像素坐标 q \mathbf{q} q，估计其他时刻该空间点对应的像素坐标 p \mathbf{p} p光流法常用算法为LK光流法

LK光流法常用算法为常用的光流法，在LK光流法中，认为图像中每个像素坐标 [ u , v ] T [u,v]^{T} [u,v]T处的灰度都是随时间 t t t变化的函数，且做如下两条假设：

1. 灰度不变假设：同一空间点对应的像素坐标的灰度值，在各个图像中是不变的
2. 局部运动一致假设：相邻区域内的像素具有相同的运动

1.1、解析解法

设对应于同一空间点的像素随时间变化的函数为 ( u ( t ) , v ( t ) ) (u(t),v(t)) (u(t),v(t))，根据灰度不变假设，存在固定灰度值 C C C，有
I ( u ( t ) , v ( t ) , t ) = C (1) I(u(t),v(t),t)=C\tag{1} I(u(t),v(t),t)=C(1)
在上式中，对 t t t求导得到
∂ I ∂ u ∂ u ∂ t + ∂ I ∂ v ∂ v ∂ t + ∂ I ∂ t = 0 (2) \frac{\partial{I}}{\partial{u}}\frac{\partial{u}}{\partial{t}}+\frac{\partial{I}}{\partial{v}}\frac{\partial{v}}{\partial{t}}+\frac{\partial{I}}{\partial{t}}=0\tag{2} ∂u∂I​∂t∂u​+∂v∂I​∂t∂v​+∂t∂I​=0(2)
∇ t u = ∂ u ∂ t , ∇ t v = ∂ v ∂ t \nabla_{t}u=\frac{\partial{u}}{\partial{t}},\nabla_{t}v=\frac{\partial{v}}{\partial{t}} ∇t​u=∂t∂u​,∇t​v=∂t∂v​为 x x x轴， y y y轴方向上的像素移动速度，这两个量也是LK光流法的求解目标， ∇ u I = ∂ I ∂ u , ∇ v I = ∂ I ∂ v \nabla_{u}I=\frac{\partial{I}}{\partial{u}},\nabla_{v}I=\frac{\partial{I}}{\partial{v}} ∇u​I=∂u∂I​,∇v​I=∂v∂I​为灰度在 x , y x,y x,y方向上的梯度，也可称为像素梯度， ∇ t I = ∂ I ∂ t \nabla_{t}I=\frac{\partial{I}}{\partial{t}} ∇t​I=∂t∂I​为固定点处灰度对时间的导数

( 2 ) (2) (2)可以化简为
[ ∇ u I , ∇ v I ] [ ∇ t u ∇ t v ] = − ∇ t I (3) [\nabla_{u}I,\nabla_{v}I]\begin{bmatrix}\nabla_{t}u\\\nabla_{t}v\end{bmatrix}=-\nabla_{t}I\tag{3} [∇u​I,∇v​I][∇t​u∇t​v​]=−∇t​I(3)
令 w = [ ∇ t u ∇ t v ] \mathbf{w}=\begin{bmatrix}\nabla_{t}u\\\nabla_{t}v\end{bmatrix} w=[∇t​u∇t​v​]，上式是一个二元一次方程，仅靠该方程无法计算 w \mathbf{w} w，还需引入其他约束。

根据局部运动一致假设，可以认为像素 q i \mathbf{q}_{i} qi​附近的某邻域内全部像素 q j , j = 1 , ⋯   , w \mathbf{q}_{j},j=1,\cdots,w qj​,j=1,⋯,w再 Δ t \Delta{t} Δt时间段内具有相同的运动，因此 ( 3 ) (3) (3)可以写成
[ ∇ u I 1 ( q 1 ) , ∇ v I 1 ( q 1 ) ⋮ ∇ u I 1 ( q w ) , ∇ v I 1 ( q w ) ] w = [ − ∇ t I ( q 1 ) ⋮ − ∇ t I ( q w ) ] (4) \begin{bmatrix}\nabla_{u} I_{1}(\mathbf{q}_{1}),\nabla_{v} I_{1}(\mathbf{q}_{1})\\\vdots\\ \nabla_{u} I_{1}(\mathbf{q}_{w}),\nabla_{v} I_{1}(\mathbf{q}_{w})\end{bmatrix}\mathbf{w}=\begin{bmatrix}-\nabla_{t}I(\mathbf{q}_{1})\\\vdots\\-\nabla_{t}I(\mathbf{q}_{w})\end{bmatrix}\tag{4} ⎣⎢⎡​∇u​I1​(q1​),∇v​I1​(q1​)⋮∇u​I1​(qw​),∇v​I1​(qw​)​⎦⎥⎤​w=⎣⎢⎡​−∇t​I(q1​)⋮−∇t​I(qw​)​⎦⎥⎤​(4)
其中
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \nabla_{u} I_{…
记 ( 4 ) (4) (4)中系数矩阵为 A \mathbf{A} A，等号右侧矩阵为 b \mathbf{b} b，则方程变为
A w = b \mathbf{A}\mathbf{w}=\mathbf{b} Aw=b
上式是关于 w \mathbf{w} w的超定方程组，可以通过最小二乘的方式求解，即令
w ∗ = arg ⁡ min ⁡ w   ∥ A w − b ∥ 2 (6) \mathbf{w}^{\ast}=\underset{\mathbf{w}}{\arg\min}\,\|\mathbf{A}\mathbf{w}-\mathbf{b}\|^{2}\tag{6} w∗=wargmin​∥Aw−b∥2(6)
根据§1，容易求出 w ∗ \mathbf{w}^{\ast} w∗，根据 q i + w ∗ Δ t \mathbf{q}_{i}+\mathbf{w}^{\ast}\Delta{t} qi​+w∗Δt即可计算新像素位置

1.2、优化解法

通过最小化两张图像对应像素邻域内的灰度差也可以求出给定点 q \mathbf{q} q在第二张图像中的对应像素 p \mathbf{p} p，即
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{p}^{\a…
e j \mathbf{e}_{j} ej​对 p \mathbf{p} p的雅可比矩阵为
J j = ∂ e j ∂ p = [ − ∇ u I 2 ( p j ) − ∇ v I 2 ( p j ) ] (8) \mathbf{J}_{j}=\frac{\partial\mathbf{e}_{j}}{\partial\mathbf{p}}=\begin{bmatrix}-\nabla_{u}I_{2}(\mathbf{p}_{j})\\ -\nabla_{v}I_{2}(\mathbf{p}_{j})\end{bmatrix}\tag{8} Jj​=∂p∂ej​​=[−∇u​I2​(pj​)−∇v​I2​(pj​)​](8)
再求出
H k = ∑ j = 1 w J j J j T b k = ∑ j = 1 w J j T e j \mathbf{H}_{k}=\sum_{j=1}^{w}\mathbf{J}_{j}\mathbf{J}_{j}^{T}\quad\quad \mathbf{b}_{k}=\sum_{j=1}^{w}\mathbf{J}^{T}_{j}\mathbf{e}_{j} Hk​=j=1∑w​Jj​JjT​bk​=j=1∑w​JjT​ej​
增量方程为如下式，可以通过增量方程计算更新量
H k Δ p k = − b k \mathbf{H}_{k}\Delta\mathbf{p}_{k}=-\mathbf{b}_{k} Hk​Δpk​=−bk​
得到更新量后，第二张图片中像素坐标可以更新为
p k + 1 = p k + Δ p k \mathbf{p}_{k+1}=\mathbf{p}_{k}+\Delta\mathbf{p}_{k} pk+1​=pk​+Δpk​

2、直接法

直接法并不单独估计第二张图片中的像素点位置，而是对第一张图片中的像素点，根据相机位姿估计值寻找其在第二张图片中对应的像素位置，并通过图片中对应像素的灰度差不断优化相机位姿变换，得到最优位姿变换，同时使两张图片的灰度差最小。下面进行详细说明。

已知像素 q i , i = 1 , ⋯   , n \mathbf{q}_{i},i=1,\cdots,n qi​,i=1,⋯,n和其对应的深度，及摄像机内参矩阵
K = [ f x 0 c x 0 f y c y 0 0 1 ] \mathbf{K}=\left[\begin{array}{ccc} f_{x}&0&c_{x}\\ 0&f_{y}&c_{y}\\ 0&0&1 \end{array}\right] K=⎣⎡​fx​00​0fy​0​cx​cy​1​⎦⎤​
可以还原出三维空间位置 x i \mathbf{x}_{i} xi​，令 X i = [ x i 1 ] ∈ R 4 \mathbf{X}_{i}=\begin{bmatrix}\mathbf{x}_{i}\\1\end{bmatrix}\in\mathbb{R}^{4} Xi​=[xi​1​]∈R4，并记从第一张图片到第二张图片对应的相机位姿变换为 T ∈ S E ( 3 ) \mathbf{T}\in SE(3) T∈SE(3)，则 x i \mathbf{x}_{i} xi​在第二个相机坐标系下的空间坐标为
y i = ( T X i ) 1 : 3 = [ X , Y , Z ] T \mathbf{y}_{i}=(\mathbf{T}\mathbf{X}_{i})_{1:3}=[X,Y,Z]^{T} yi​=(TXi​)1:3​=[X,Y,Z]T
对应的像素坐标为
p i = 1 Z ( K y i ) 1 : 2 \mathbf{p}_{i}=\frac{1}{Z}(\mathbf{K}\mathbf{y}_{i})_{1:2} pi​=Z1​(Kyi​)1:2​
直接法求解优化问题
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{T}^{\a…
暂时省略下标，根据链式求导法则得到
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \frac{\partial…
容易得到
∂ p ∂ y = [ f x Z 0 − f x X Z 2 0 f y Z − f x Y Z 2 ] ∂ y ∂ T = [ I , − y ∧ ] \frac{\partial{\mathbf{p}}}{\partial\mathbf{y}}=\begin{bmatrix} \frac{f_{x}}{Z}&0&-\frac{f_{x}X}{Z^{2}}\\ 0&\frac{f_{y}}{Z}&-\frac{f_{x}Y}{Z^{2}} \end{bmatrix}\quad\quad\frac{\partial\mathbf{y}}{\partial\mathbf{T}}=[\mathbf{I},-\mathbf{y}^{\wedge}] ∂y∂p​=[Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fx​Y​​]∂T∂y​=[I,−y∧]
因此 ( 10 ) (10) (10)后两项可以写成
∂ p ∂ T = ∂ p ∂ y ∂ y ∂ T = [ 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 x Y Z 2 − f y − f y Y 2 Z 2 f x X Y Z 2 f x X Z ] (11) \frac{\partial\mathbf{p}}{\partial\mathbf{T}}=\frac{\partial\mathbf{p}}{\partial\mathbf{y}}\frac{\partial\mathbf{y}}{\partial\mathbf{T}}=\begin{bmatrix} \frac{f_{x}}{Z}&0&-\frac{f_{x}X}{Z^{2}}&-\frac{f_{x}XY}{Z^{2}}&f_{x}+\frac{f_{x}X^{2}}{Z^{2}}&-\frac{f_{x}Y}{Z}\\ 0&-\frac{f_{y}}{Z}&-\frac{f_{x}Y}{Z^{2}}&-f_{y}-\frac{f_{y}Y^{2}}{Z^{2}}&\frac{f_{x}XY}{Z^{2}}&\frac{f_{x}X}{Z} \end{bmatrix}\tag{11} ∂T∂p​=∂y∂p​∂T∂y​=[Zfx​​0​0−Zfy​​​−Z2fx​X​−Z2fx​Y​​−Z2fx​XY​−fy​−Z2fy​Y2​​fx​+Z2fx​X2​Z2fx​XY​​−Zfx​Y​Zfx​X​​](11)
故 ( 10 ) (10) (10)又可以写成
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \frac{\partial…
问题 ( 9 ) (9) (9)的雅可比矩阵为
J i = ∂ e i ∂ T \mathbf{J}_{i}=\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{T}} Ji​=∂T∂ei​​
由此得到
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲\mathbf{H}_{k}=…
则更新量可以通过下式计算
H k Δ T k = − b k \mathbf{H}_{k}\Delta\mathbf{T}_{k}=-\mathbf{b}_{k} Hk​ΔTk​=−bk​

并通过下式更新
T k + 1 = E x p ( Δ T k ) T k \mathbf{T}_{k+1}=\mathrm{Exp}(\Delta\mathbf{T}_{k})\mathbf{T}_{k} Tk+1​=Exp(ΔTk​)Tk​
最终得到最优的位姿变换

实验

直接法在kitti数据集上的效果如下图，可以看到追踪效果良好

附录

§1、标准最小二乘问题

标准最小二乘问题对给定 A ∈ R M × N \mathbf{A}\in\mathbb{R}^{M\times{N}} A∈RM×N，计算 x ∗ ∈ R N \mathbf{x}^{\ast}\in\mathbb{R}^{N} x∗∈RN，使得
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{x}^{\a…
首先对 A \mathbf{A} A进行SVD分解
A = U [ Σ r × r O O O ] V T \mathbf{A}=\mathbf{U} \begin{bmatrix} \boldsymbol\Sigma_{r\times{r}}&\mathbf{O}\\ \mathbf{O}&\mathbf{O} \end{bmatrix}\mathbf{V}^{T} A=U[Σr×r​O​OO​]VT
则 A \mathbf{A} A的伪逆为
A † = V [ Σ r × r − 1 O O O ] U T (A2) \mathbf{A}^{\dagger}=\mathbf{V} \begin{bmatrix} \boldsymbol\Sigma_{r\times{r}}^{-1}&\mathbf{O}\\ \mathbf{O}&\mathbf{O} \end{bmatrix}\mathbf{U}^{T}\tag{A2} A†=V[Σr×r−1​O​OO​]UT(A2)
可以证明，满足 ( A 1 ) \mathrm{(A1)} (A1)的模长最小的解为
x ∗ = A † b (A3) \mathbf{x}^{\ast}=\mathbf{A}^{\dagger}\mathbf{b}\tag{A3} x∗=A†b(A3)
特别地，当 r a n k ( A ) = N \mathrm{rank}(\mathbf{A})=N rank(A)=N时， A † = ( A T A ) − 1 A \mathbf{A}^{\dagger}=(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A} A†=(ATA)−1A， ( A 1 ) \mathrm{(A1)} (A1)仅有如下一个解
x ∗ = ( A T A ) − 1 A b (A4) \mathbf{x}^{\ast}=(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A}\mathbf{b}\tag{A4} x∗=(ATA)−1Ab(A4)