本文总结视觉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.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}} ∇tu=∂t∂u,∇tv=∂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}} ∇uI=∂u∂I,∇vI=∂v∂I为灰度在 x , y x,y x,y方向上的梯度，也可称为像素梯度， ∇ t I = ∂ I ∂ t \nabla_{t}I=\frac{\partial{I}}{\partial{t}} ∇tI=∂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} \[∇uI,∇vI\]\[∇tu∇tv\]=−∇tI(3) 令 w = \[ ∇ t u ∇ t v \] \\mathbf{w}=\\begin{bmatrix}\\nabla_{t}u\\\\\\nabla_{t}v\\end{bmatrix} w=\[∇tu∇tv\]，上式是一个二元一次方程，仅靠该方程无法计算 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} ⎣⎢⎡∇uI1(q1),∇vI1(q1)⋮∇uI1(qw),∇vI1(qw)⎦⎥⎤w=⎣⎢⎡−∇tI(q1)⋮−∇tI(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=\[−∇uI2(pj)−∇vI2(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∑wJjJjTbk=j=1∑wJjTej 增量方程为如下式，可以通过增量方程计算更新量 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=⎣⎡fx000fy0cxcy1⎦⎤ 可以还原出三维空间位置 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=\[xi1\]∈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=\[Zfx00Zfy−Z2fxX−Z2fxY\]∂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=[Zfx00−Zfy−Z2fxX−Z2fxY−Z2fxXY−fy−Z2fyY2fx+Z2fxX2Z2fxXY−ZfxYZfxX](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×rOOO]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−1OOO]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)