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

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

1、Lucas-Kanade光流法

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

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

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

1.1、解析解法

设对应于同一空间点的像素随时间变化的函数为$(u(t),v(t))$，根据灰度不变假设，存在固定灰度值$C$，有
$$I(u(t),v(t),t)=C\text{\hspace{1em}(1)}$$
在上式中，对$t$求导得到
$$\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\text{\hspace{1em}(2)}$$
${\nabla }_{t}u=\frac{\partial u}{\partial t},{\nabla }_{t}v=\frac{\partial v}{\partial t}$为$x$轴，$y$轴方向上的像素移动速度，这两个量也是LK光流法的求解目标，${\nabla }_{u}I=\frac{\partial I}{\partial u},{\nabla }_{v}I=\frac{\partial I}{\partial v}$为灰度在$x,y$方向上的梯度，也可称为像素梯度，${\nabla }_{t}I=\frac{\partial I}{\partial t}$为固定点处灰度对时间的导数

$(2)$可以化简为
$$[{\nabla }_{u}I,{\nabla }_{v}I]\left[\begin{array}{c}{\nabla }_{t}u\\ {\nabla }_{t}v\end{array}\right]=-{\nabla }_{t}I\text{\hspace{1em}(3)}$$
令$\mathbf{w}=\left[\begin{array}{c}{\nabla }_{t}u\\ {\nabla }_{t}v\end{array}\right]$，上式是一个二元一次方程，仅靠该方程无法计算$\mathbf{w}$，还需引入其他约束。

根据局部运动一致假设，可以认为像素${\mathbf{q}}_i$附近的某邻域内全部像素${\mathbf{q}}_j,j=1,\cdots ,w$再$\Delta t$时间段内具有相同的运动，因此$(3)$可以写成
$$\left[\begin{array}{c}{\nabla }_u{I}_1({\mathbf{q}}_1),{\nabla }_v{I}_1({\mathbf{q}}_1)\\ ⋮\\ {\nabla }_u{I}_1({\mathbf{q}}_w),{\nabla }_v{I}_1({\mathbf{q}}_w)\end{array}\right]\mathbf{w}=\left[\begin{array}{c}-{\nabla }_{t}I({\mathbf{q}}_1)\\ ⋮\\ -{\nabla }_{t}I({\mathbf{q}}_w)\end{array}\right]\text{\hspace{1em}(4)}$$
其中
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \nabla_{u} I_{…
记$(4)$中系数矩阵为$\mathbf{A}$，等号右侧矩阵为$\mathbf{b}$，则方程变为
$$\mathbf{A}\mathbf{w}=\mathbf{b}$$
上式是关于$\mathbf{w}$的超定方程组，可以通过最小二乘的方式求解，即令
$${\mathbf{w}}^{\ast }=\underset{\mathbf{w}}{\mathrm{argmin}}\Vert \mathbf{A}\mathbf{w}-\mathbf{b}{\Vert }^2\text{\hspace{1em}(6)}$$
根据§1，容易求出${\mathbf{w}}^{\ast }$，根据${\mathbf{q}}_i+{\mathbf{w}}^{\ast }\Delta t$即可计算新像素位置

1.2、优化解法

通过最小化两张图像对应像素邻域内的灰度差也可以求出给定点$\mathbf{q}$在第二张图像中的对应像素$\mathbf{p}$，即
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{p}^{\a…
${\mathbf{e}}_j$对$\mathbf{p}$的雅可比矩阵为
$${\mathbf{J}}_j=\frac{\partial {\mathbf{e}}_j}{\partial \mathbf{p}}=\left[\begin{array}{c}-{\nabla }_u{I}_2({\mathbf{p}}_j)\\ -{\nabla }_v{I}_2({\mathbf{p}}_j)\end{array}\right]\text{\hspace{1em}(8)}$$
再求出
$${\mathbf{H}}_k=\sum _{j=1}^w{\mathbf{J}}_j{\mathbf{J}}_j^T{\mathbf{b}}_k=\sum _{j=1}^w{\mathbf{J}}_j^T{\mathbf{e}}_j$$
增量方程为如下式，可以通过增量方程计算更新量
$${\mathbf{H}}_k\Delta {\mathbf{p}}_k=-{\mathbf{b}}_k$$
得到更新量后，第二张图片中像素坐标可以更新为
$${\mathbf{p}}_{k+1}={\mathbf{p}}_k+\Delta {\mathbf{p}}_k$$

2、直接法

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

已知像素${\mathbf{q}}_i,i=1,\cdots ,n$和其对应的深度，及摄像机内参矩阵
$$\mathbf{K}=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$
可以还原出三维空间位置${\mathbf{x}}_i$，令${\mathbf{X}}_i=\left[\begin{array}{c}{\mathbf{x}}_i\\ 1\end{array}\right]\in {\mathbb{R}}^4$，并记从第一张图片到第二张图片对应的相机位姿变换为$\mathbf{T}\in SE(3)$，则${\mathbf{x}}_i$在第二个相机坐标系下的空间坐标为
$${\mathbf{y}}_i=(\mathbf{T}{\mathbf{X}}_i{)}_{1:3}=[X,Y,Z{]}^T$$
对应的像素坐标为
$${\mathbf{p}}_i=\frac{1}{Z}(\mathbf{K}{\mathbf{y}}_i{)}_{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…
容易得到
$$\frac{\partial \mathbf{p}}{\partial \mathbf{y}}=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{x}Y}{Z^2}\end{array}\right]\frac{\partial \mathbf{y}}{\partial \mathbf{T}}=[\mathbf{I},-{\mathbf{y}}^{\wedge }]$$
因此$(10)$后两项可以写成
$$\frac{\partial \mathbf{p}}{\partial \mathbf{T}}=\frac{\partial \mathbf{p}}{\partial \mathbf{y}}\frac{\partial \mathbf{y}}{\partial \mathbf{T}}=\left[\begin{array}{cccccc}\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{array}\right]\text{\hspace{1em}(11)}$$
故$(10)$又可以写成
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \frac{\partial…
问题$(9)$的雅可比矩阵为
$${\mathbf{J}}_i=\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{T}}$$
由此得到
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲\mathbf{H}_{k}=…
则更新量可以通过下式计算
$${\mathbf{H}}_k\Delta {\mathbf{T}}_k=-{\mathbf{b}}_k$$

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

实验

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

附录

§1、标准最小二乘问题

标准最小二乘问题对给定$\mathbf{A}\in {\mathbb{R}}^{M\times N}$，计算${\mathbf{x}}^{\ast }\in {\mathbb{R}}^N$，使得
KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{x}^{\a…
首先对$\mathbf{A}$进行SVD分解
$$\mathbf{A}=\mathbf{U}\left[\begin{array}{cc}{\mathbf{\Sigma }}_{r\times r}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}\end{array}\right]{\mathbf{V}}^T$$
则$\mathbf{A}$的伪逆为
$${\mathbf{A}}^{†}=\mathbf{V}\left[\begin{array}{cc}{\mathbf{\Sigma }}_{r\times r}^{-1}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}\end{array}\right]{\mathbf{U}}^T\text{\hspace{1em}(A2)}$$
可以证明，满足$(\mathrm{A}1)$的模长最小的解为
$${\mathbf{x}}^{\ast }={\mathbf{A}}^{†}\mathbf{b}\text{\hspace{1em}(A3)}$$
特别地，当$\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{k}(\mathbf{A})=N$时，${\mathbf{A}}^{†}=({\mathbf{A}}^T\mathbf{A}{)}^{-1}\mathbf{A}$，$(\mathrm{A}1)$仅有如下一个解
$${\mathbf{x}}^{\ast }=({\mathbf{A}}^T\mathbf{A}{)}^{-1}\mathbf{A}\mathbf{b}\text{\hspace{1em}(A4)}$$