LK金字塔光流法与简单实现

闲谈时刻

不务正业预警
眼看着一个学期又告一段落，几个月来拢共还是没写几篇博客。不过手头上倒是还积累着不少资料值得一写，趁着新春得闲可以好好梳理梳理了。

介绍

开场依照惯例是得简单介绍什么是光流，来自 Wiki：

光流(Optical flow or optic flow)是关于视域中的物体运动检测中的概念。用来描述相对于观察者的运动所造成的观测目标、表面或边缘的运动。

简单来讲，光流描述了场景中物体运动在视觉中的变化。光流的概念由Gibson在1950年提出，其通过相邻帧之间像素点的对应关系计算像素点的瞬时速度，从而描述物体信息。

为了应用光流计算物体运动法，相邻帧需要满足两个假设：

• 亮度不变：两个间隔帧之间的像素点的亮度需要保持恒定，从而对两个相邻帧之间的像素点进行对应；
• 运动较小：需要保证像素点并不会随着时间的变化而剧烈变化，从而通过相邻帧之间对应点位置变化引起的灰度值变化来近似为灰度对位置的偏导。

以二维的图像为例，以 I(x, y, t) 描述图像在 t 时刻的灰度值。其灰度值在两个间隔帧之间的变化值为 Δx，Δy，Δt。由于假设1，根据间隔帧的灰度值恒定，可以得出公式1：
（1）
由于假设2，可以通过泰勒级数对该方程进行展开，得到公式2：
（2）
H.O.T 为高阶小量，其在移动足够小的时候可以忽略，从而得到公式3：
（3）
公式3对左右两边除以 Δt 可以得到公式4：
（4）
也即是公式5：
（5）
其中，Vx，Vy 是像素点在 x 与 y 方向上的速度，也即是 I(x, y, t) 的光流 。用 Ix，Iy 和 It 分别表示像素点对于 x，y，t 的偏导后，可以得到最终的光流表达式：
（6）
抑或是：
（7）
了解了光流法的基本要素，那么应当如何求解光流呢？

Lucas–Kanade光流算法

首先介绍Lucas–Kanade光流算法（L-K算法），其本质是通过最小二乘法以不需要迭代的方法求解光流，是光流算法中最简单的一种。
为了简化光流计算的流程，L-K光流算法为其增加了一个额外的假设：

• 空间一致：某个像素点领域内保持相同的瞬时速度。

对于给定大小的窗口，可以列出公式：


…

其中，qn 代表窗口中的像素点，Ix(qn)是像素qn在x方向上的偏导。写成矩阵形式也就是公式8：
Av = b， 其中 （8）
该方程组为超定方程，等式个数多于未知数的个数，因此通过最小二乘法计算出近似解，如公式9所示。
（9）
也即是公式10：
（10）

L-K 金字塔光流算法

虽然光流法假设2本身并不容易满足，在实际应用中容易遇到间隔帧出现较大变化的情况，从而对计算结果造成较大的误差。为了减缓这一假设不成立情况所导致的问题，可以在计算中缩小图像的尺寸，从而使得像素点的运动减少（图像尺寸减半时，像素运动自然也同时减半）。
通过对图像尺寸进行缩放，建立图像金字塔，可以使得L-K金字塔算法应用于较大的运动。

算法原理

L-K金字塔算法的主要的步骤可以分为三步：建立金字塔，金字塔跟踪以及迭代过程。

建立金字塔

首先需要对原始图像建立金字塔，其中原始图像位于底层，其上每一层均基于上一层进行计算。金字塔中每一层均是上一层的下采样，其计算公式为：
（11）
其中 IL(x, y) 为 L 层图像中 x, y 位置所在像素点的灰度值，其相当于通过[0.25 0.5 0.25]的低通滤波器进行迭代计算。

金字塔迭代

金字塔迭代过程为：

• 建立金字塔后，首先对于最高层的图像计算出其上的光流；
• 通过上一层（L+1层）的计算结果对下一层（L层）的的图像进预平移，并在L层的基础上计算出该层的残余光流向量dL。由于金字塔中每一层的尺寸均是上一层的一半，因此其每一层的光流均是其上一层的二分之一。
  通过L+1层计算出的光流作为初值计算L层的光流，可以保证每一层的残余光流向量较小，从而应用L-K光流算法；
• 对于每一层迭代光流计算，最终得到的光流也即是所有层光流的叠加。

由于顶层图像尺寸较小，其初始的光流估计量可以设置为0，即 gL = [0, 0] 。

迭代过程

介绍了对于整个金字塔迭代过程，还需要提供对于每一层的残余光流计算方法。
在 L 层上计算残余光流，首先需要对于光流进行更新，即 v = 2 * v，并通过其某个像素点的偏导值计算出其空间梯度矩阵，如公式12所示：
（12）
其中 Ix 为上x方向的偏导，Iy 为y方向上的偏导。
迭代计算间隔帧之间对应点的灰度误差，如公式13所示：
（13）
并计算对应的误差矩阵，如公式14所示：
（14）
通过L-K算法可以计算出当前的仿射光流，并对当前层的光流进行更新。当该仿射光流的模小于阈值时，可以认为迭代计算结果已经收敛，并结束迭代过程。

算法流程

上述的L-K 金字塔光流算法原理较为清晰，具体的计算结果可以参照论文中所提供的伪代码：

算法实现

在OpenCV中对L-K金字塔光流算法提供了实现，其API为calcOpticalFlowPyrLK。这里给出一个简单的 C++ 版本的实现。
首先是头文件，lk_tracker.h：

#ifndef LK_TRACKER
#define LK_TRACKER

#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>

using namespace std;
using namespace cv;

void trace(string out) {
   
  cout << out << endl;
};

struct TrackedPoint {
   
  Point point;
  Mat opticalFlow;
  TrackedPoint() {
   };
  TrackedPoint(const Point p, const Mat of):point(p), opticalFlow(of) {
   };
};

class LKTracker {
   
  private:
    int maxPyramidLayer = 3;
    int wx = 2;
    int wy = 2;
    int maxIteration = 50;
    double opticalflowResidual = 0.0001;

  private:
    static int getMatInt(Mat mat, int row, int col);
    static double getMatDouble(Mat mat, int row, int col);
    static double getMatDouble(Mat mat, double row, double col);
    void lowpassFilter(InputArray src, OutputArray dst);
    Mat calcGradientMatrix(InputArray frame, Point2f p);
    Mat calcMismatchVector(InputArray preFrame, InputArray curFrame, Point2f p, Mat g, Mat v);
    vector<Mat> buildPyramid(InputArray src);
    double calcResidual(Mat mat);
    bool isOpticalFlowValid(Mat of);
    double calcHarrisResponse(Mat gradient, double alpha);

  public:
    LKTracker();
    ~LKTracker();
    vector<TrackedPoint> track(InputArray preFrame, InputArray curFrame, vector<Point> keyPoints);
    vector<TrackedPoint> trackAll(InputArray preFrame, InputArray curFrame, double qualityLevel);
};

#endif

实现部分 lk_tracker.cc：

#include "lk_tracker.h"

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#