本文总结视觉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)