金字塔 Lucas-Kanade 光流法算法原理和C++代码实现

金字塔 Lucas-Kanade 光流法（Pyramidal Lucas-Kanade Optical Flow）是一种经典的稀疏光流估计算法，用于在图像序列中追踪关键点的位置变化。它在计算机视觉领域被广泛用于目标跟踪、SLAM、视觉里程计等任务。

---

一、算法背景与原理概述

1️、Lucas-Kanade 光流基本原理

Lucas-Kanade 方法假设：

• 图像在很短时间内灰度保持不变（亮度恒常）；
• 相邻帧之间点的运动较小；
• 某个像素邻域内的运动是一致的（即：局部一致性）。

亮度一致性假设公式：

$$I(x,y,t)=I(x+u,y+v,t+1)$$

对该式在 $(x,y,t)$ 处泰勒展开，忽略高阶项，有：

$$I_{x}u+I_{y}v+I_t=0$$

其中：

• $I_x,I_y$：图像的一阶空间梯度；
• $I_t$：时间梯度；
• $u,v$：当前点的光流向量。

这是一个只有一个方程两个未知数的欠定问题，所以 Lucas-Kanade 用邻域窗口内的多个点，构造一个最小二乘系统求解。

---

2️、金字塔光流的动机

Lucas-Kanade 假设小运动，但图像中可能存在大位移（如快速移动、低帧率），因此需要通过**图像金字塔（Pyramid）**将图像从低分辨率到高分辨率逐步迭代处理：

• 在金字塔顶层（低分辨率）估计一个粗略光流；
• 然后逐层向高分辨率 refine 这个结果；
• 每一层中用 Lucas-Kanade 方法优化。

---

二、金字塔 LK 光流推导流程

假设已有一对连续图像 $I_1$ 和 $I_2$，追踪点 $\mathbf{p}=(x,y)$：

（1）构建图像金字塔

对 $I_1$、$I_2$ 进行高斯金字塔降采样，生成多层（例如 L 层）图像。

（2）初始化顶层（最小图）中的位移为 0

在顶层 $L$：

• 设定初始位移 ${\mathbf{d}}_L=(0,0)$

（3）逐层从顶向下计算光流

对每一层 $l=L\to 0$：

1. 将上层位移放大两倍作为当前层初始估计：

   $${\mathbf{d}}_l=2\cdot {\mathbf{d}}_{l+1}$$

2. 在该层中以 ${\mathbf{p}}_l$ 为中心构建窗口 $\Omega$，对窗口内所有像素构造光流方程：

   $$A\cdot {\mathbf{d}}_l=b$$

   • $A\in {\mathbb{R}}^{N\times 2}$：梯度矩阵，行向量为 $[I_x,I_y]$；
   • $b\in {\mathbb{R}}^{N\times 1}$：为 $-I_t$；
   • 求解 ${\mathbf{d}}_l=(A^{T}A{)}^{-1}{A}^{T}b$

3. 迭代 refine ${\mathbf{d}}_l$，常用高斯-牛顿或牛顿法迭代。

---

三、基于OpenCV的C++ 实现示例

下面是一个简化版的 Pyramidal Lucas-Kanade 实现，追踪单个点：

所需依赖

#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;

单层 LK 光流追踪函数

bool singleLevelLK(
    const Mat& img1, const Mat& img2, 
    const Point2f& kp1, Point2f& kp2,
    int window_size = 8, int max_iter = 10, double epsilon = 1e-3) 
{
    double dx = 0, dy = 0;
    for (int iter = 0; iter < max_iter; ++iter) {
        double H00 = 0, H01 = 0, H11 = 0, b0 = 0, b1 = 0;

        for (int x = -window_size; x <= window_size; ++x) {
            for (int y = -window_size; y <= window_size; ++y) {
                double u1 = kp1.x + x, v1 = kp1.y + y;
                double u2 = kp2.x + x + dx, v2 = kp2.y + y + dy;

                if (u1 < 1 || v1 < 1 || u2 < 1 || v2 < 1 ||
                    u1 >= img1.cols - 1 || v1 >= img1.rows - 1 ||
                    u2 >= img2.cols - 1 || v2 >= img2.rows - 1)
                    continue;

                double I1 = bilinearInterp(img1, u1, v1);
                double I2 = bilinearInterp(img2, u2, v2);
                double Ix = (bilinearInterp(img2, u2 + 1, v2) - bilinearInterp(img2, u2 - 1, v2)) / 2;
                double Iy = (bilinearInterp(img2, u2, v2 + 1) - bilinearInterp(img2, u2, v2 - 1)) / 2;
                double It = I1 - I2;

                H00 += Ix * Ix;
                H01 += Ix * Iy;
                H11 += Iy * Iy;
                b0  += -Ix * It;
                b1  += -Iy * It;
            }
        }

        double det = H00 * H11 - H01 * H01;
        if (fabs(det) < 1e-6) break;

        double dx_update = (H11 * b0 - H01 * b1) / det;
        double dy_update = (H00 * b1 - H01 * b0) / det;

        dx += dx_update;
        dy += dy_update;

        if (dx_update * dx_update + dy_update * dy_update < epsilon * epsilon) break;
    }

    kp2.x += dx;
    kp2.y += dy;
    return true;
}

金字塔 LK 封装函数

bool pyramidLK(
    const Mat& img1, const Mat& img2,
    const Point2f& pt1, Point2f& pt2,
    int levels = 4)
{
    vector<Mat> pyr1, pyr2;
    buildPyramid(img1, pyr1, levels);
    buildPyramid(img2, pyr2, levels);

    Point2f pt_pyr = pt1 * (1.0 / (1 << levels));
    pt2 = pt_pyr;

    for (int lvl = levels; lvl >= 0; --lvl) {
        pt_pyr *= 2.0;
        pt2 *= 2.0;

        singleLevelLK(pyr1[lvl], pyr2[lvl], pt_pyr, pt2);
    }
    return true;
}

---

四、总结

模块	内容
核心思想	基于亮度恒常性 + 局部一致性，解局部线性方程组估计光流
金字塔优势	分层迭代解决大位移问题，提高鲁棒性
应用	特征点跟踪（如 ORB-SLAM2）、运动估计、目标跟踪等

---