VSLAM前端：金字塔光流跟踪算法
一、光流
像素点在二维图像中的运动被定义为光流，其在相邻帧图像中存在有位移运动，即存在像素的光流。我们的目的是计算出光流，计算要满足几个前提假设：1.灰度不变性：同一个像素的灰度值在各个图像中是固定不变的；2. 相邻帧之前像素的位移不能太大；3.运动像素周围的像素具有同样的运动规律。
假设图中像素点的位移向量为
d，即光流，设第一帧中像素的二维坐标为
u= \begin{bmatrix} u_x & u_y\end{bmatrix}，下一帧像素的位置可以表示为
v=u+d=\begin{bmatrix} u_x+d_x& u_y+d_y\end{bmatrix}，前两帧图像分别用
I(x,y)和
J(x,y)表示。我们假设邻域内像素具有相同的运动规律，我们建立以光流为优化变量的误差函数：
\epsilon(d)=\epsilon(d_x, d_y)=\sum_{x=u_x-\omega_x}^{u_x + w_x}\sum_{y=u_y-\omega_y}^{u_y+\omega_y}(I(x,y)-J(x+d_x,y+d_y))^2
其中
\omega_x和
\omega_y表示整数，一般取2，3，4，5，6，7。
二、图像金字塔
上图为4层图像金字塔，从下往上进行图像的下采样即可，这里不多解释，读者可自行了解。
三、基于金字塔的Lucas-Kanade算法
算法的基本思路为：首先在金字塔的最高层计算光流大小，将它作为下一层金字塔光流的初始值，以此类推计算第0层的光流大小，作为最终的光流结果。
我们将第一小节的误差函数改写为金字塔第
L层的损失函数：
\epsilon^L(d^L)=\epsilon^L(d_x^L, d_y^L)=\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}(I^L(x,y)-J^L(x+g_x^L+d_x^L,y+g_y^L+d_y^L))^2
其中
g^L为猜测光流，表示金字塔第
L层迭代的光流初始值，
d^L为剩余光流，表示金子塔第
L层迭代的光流误差。
3.1 剩余光流
d^L的计算
我们使用上式对
d^L求导：
\frac{\partial \epsilon^L(d^L)}{\partial d^L}=-2\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}(I^L(x,y)-J^L(x+g_x^L+d_x^L,y+g_y^L+d_y^L))
\begin{bmatrix} \frac{\partial J^L}{\partial x} & \frac{\partial J^L}{\partial y}\end{bmatrix}
然后将图像
J^L(x+d_x^L, y+d_y^L)在
d^L=\begin{bmatrix} 0 & 0\end{bmatrix}处进行一阶泰勒展开可得：
\frac{\partial \epsilon^L(d^L)}{\partial d^L}=-2\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}(I^L(x,y)-J^L(x+g_x^L,y+g_y^L)-
\begin{bmatrix} \frac{\partial J^L}{\partial x} & \frac{\partial J^L}{\partial y}\end{bmatrix}d^L)
\begin{bmatrix} \frac{\partial J^L}{\partial x} & \frac{\partial J^L}{\partial y}\end{bmatrix}
其中
\begin{bmatrix} \frac{\partial J^L}{\partial x} & \frac{\partial J^L}{\partial y}\end{bmatrix}表示图片
J^L对
x,y坐标求导。对于领域内的像素我们定义：
\delta I(x,y)=I^L(x,y)-J^L(x+g_x^L,y+g_y^L)
\Delta I = \begin{bmatrix} I_x & I_y\end{bmatrix}^T=\begin{bmatrix} \frac{\partial J^L}{\partial x} & \frac{\partial J^L}{\partial y}\end{bmatrix}^T
其中
\Delta I由图像梯度求出，即：
I_x(x,y)=\frac{I^L(x+1,y)-I^L(x-1,y)}{2}
I_y(x,y)=\frac{I^L(x,y+1)-I^L(x,y-1)}{2}
我们将上述公式全部带入泰勒展开式，并且两边同时取转置，可得：
\frac{1}{2}\begin{bmatrix}\frac{\partial \epsilon^L(d^L)}{\partial d^L}\end{bmatrix}^T=
\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}
(\begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2\end{bmatrix}d^L-
\begin{bmatrix} \delta I \cdot I_x \\ \delta I \cdot I_y\end{bmatrix})
我们化简公式：
G=\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}\begin{bmatrix} I_x^2 & I_xI_y \\ I_xI_y & I_y^2\end{bmatrix}
b_k=\sum_{x=u_x^L-\omega_x}^{u_x^L + w_x}\sum_{y=u_y^L-\omega_y}^{u_y^L+\omega_y}\begin{bmatrix} \delta I \cdot I_x \\ \delta I \cdot I_y\end{bmatrix}
则可以得到：
\frac{1}{2}\begin{bmatrix}\frac{\partial \epsilon^L(d^L)}{\partial d^L}\end{bmatrix}^T=Gd^L-b_k
当取得极小值时，导数为0，则
d^L=G^{-1}b_k，至此我们得到经典的Lucas-Kanade光流公式，但其建立在像素位移很小的前提下。我们现在讨论的是金字塔光流，其对光流法进行了改进，进行多次迭代，我们改写公式：
\eta^k=G^{-1}b_k
当
\eta^k小于阈值后，迭代终止，迭代公式为：
d_L^k=d_L^{k-1}+\eta^k，以上就是每层金字塔剩余光流的计算过程。
3.2 猜测光流
g^L的计算
最高层
g^L=\begin{bmatrix} 0 & 0\end{bmatrix}^T，每层金字塔图像猜测光流公式为:
g^{L-1}=2(g^L+d^L)。
最终光流
d=g^0+d^0。
四、原始论文算法流程
原始论文的算法流程更加简洁明了，推荐大家看下面流程:
五、opencv接口函数
void calcOpticalFlowPyrLK(
InputArray prevImg, InputArray nextImg, InputArray prevPts, InputOutputArray nextPts,
OutputArray status, OutputArray err,
Size winSize=Size(21,21), int maxLevel=3,
TermCriteria criteria=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
int flags=0, double minEigThreshold=1e-4)
prevImg：第一帧金字塔图像；
nextImg：第二帧金字塔图像；
prevPts：第一帧的像素坐标；
nextPts：需要找到的第二帧像素坐标；
status：状态变量；
err：错误矢量输出；
winSize：每层金字塔的搜索窗口大小；
maxLevel：金字塔最大层级；
criteria：迭代终止条件；
flags：OPTFLOW_USE_INITIAL_FLOW与OPTFLOW_LK_GET_MIN_EIGENVALS使用的标志位；
minEigThreshold：最小特征值阈值；
