动手实现LK光流特征点跟踪

光流

光流是一种像素随时间在图像之间运行的方法。视觉slam为了估计相机的运行，需要追踪像素中特征点的运行过程。其中，计算部分像素运行的称为稀疏光流（以Lucas-Kanada光流为代表）和稠密光流（以Horn-Schunck位移代表）。本文主要介绍并实现LK光流。

---

LK光流

原理简介

给定两幅图片，分别记为输入图片：$I(\mathbf{x})$，和模板图片：$T(\mathbf{x})$，其中$\mathbf{x}=(x,y{)}^{\mathrm{T}}$为包含像素坐标的列向量。LK光流的目标是对模板图片中的每一个特征点计算相应的像素坐标位移：$\mathbf{p}$，使得$I(\mathbf{x})$和$T(\mathbf{x})$之间的像素灰度值最小:
$$\sum _{\mathbf{x}}[I(\mathbf{W}(\mathbf{x};\mathbf{p}))-T(\mathbf{x}){]}^2\text{\hspace{1em}(1)}$$
其中$\mathbf{W}(\mathbf{x};\mathbf{p})$表示对输入图片的变换，其中$\mathbf{p}$即是我们要求解的位移向量：
$$\mathbf{W}(\mathbf{x};\mathbf{p})=\left(\begin{array}{l}x+p_1\\ y+p_2\end{array}\right)\text{\hspace{1em}(2)}$$
（注：也可以使用更为复杂的仿射变换实现图像转换，如式（3）所示）
$$\mathbf{W}(\mathbf{x};\mathbf{p})=\left(\begin{array}{ccccc}\left(1+p_1\right)\cdot x& +& p_3\cdot y& +& p_5\\ p_2\cdot x& +& \left(1+p_4\right)\cdot y& +& p_6\end{array}\right)\text{\hspace{1em}(3)}$$

LK光流通常假设特征点周围窗口内的像素具有相同的运动，因此式（1）中的求和表示对特征点附近一个像素窗口内的像素值和模板图片对应像素区域的灰度误差求和的结果。

为了最小化式（1）的目标函数，我们采用迭代求解的方法，即：
$$\Delta \mathbf{p}=min\sum _{\mathbf{x}}[I(\mathbf{W}(\mathbf{x};\mathbf{p}+\Delta \mathbf{p}))-T(\mathbf{x}){]}^2\text{\hspace{1em}(4)}$$
并将像素位移的更新量$\Delta \mathbf{p}$叠加到当前的像素位移向量$\mathbf{p}$上，即：
$$\mathbf{p}\leftarrow \mathbf{p}+\Delta \mathbf{p}\text{\hspace{1em}(5)}$$
如此不断迭代直到$\mathbf{p}$收敛。

求解式（4）可以使用Gauss-Newton法。首先对$I(\mathbf{W}(\mathbf{x};\mathbf{p}+\Delta \mathbf{p})$进行一阶线性展开，此时式（4）变为：

$$\sum _{\mathbf{x}}{\left[I(\mathbf{W}(\mathbf{x};\mathbf{p}))+\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\Delta \mathbf{p}-T(\mathbf{x})\right]}^2\text{\hspace{1em}(6)}$$
其中$\nabla I=\left(\frac{\partial I}{\partial x},\frac{\partial I}{\partial y}\right)$为输入图片$I(\mathbf{x})$在$\mathbf{W}(\mathbf{x};\mathbf{p})$的梯度。$\frac{\partial \mathbf{W}}{\partial \mathbf{p}}$为变换操作相对于位移向量的导数。（注：如果采用的变换操作为式（2），则$\frac{\partial \mathbf{W}}{\partial \mathbf{p}}$等效为单位矩阵$I$）对式（6）求关于$\Delta \mathbf{p}$的导数得到：

$$\sum _{\mathbf{x}}{\left[\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\right]}^{\mathrm{T}}\left[I(\mathbf{W}(\mathbf{x};\mathbf{p}))+\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\Delta \mathbf{p}-T(\mathbf{x})\right]\text{\hspace{1em}(7)}$$

令式（7）等于0可得：
$$\Delta \mathbf{p}=H^{-1}\sum _{\mathbf{x}}{\left[\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\right]}^{\mathrm{T}}[T(\mathbf{x})-I(\mathbf{W}(\mathbf{x};\mathbf{p}))]$$

其中$H$为海森矩阵的近似：
$$H=\sum _{\mathbf{x}}{\left[\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\right]}^{\mathrm{T}}\left[\nabla I\frac{\partial \mathbf{W}}{\partial \mathbf{p}}\right]$$

---

实验代码

代码中使用到图片的获取方式：https://github.com/gaoxiang12/slambook/tree/master/ch8/data

C++代码：

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
using namespace std; 

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>

#include <eigen3/Eigen/Eigen>

bool isIndexValid(cv::Mat image, int rowIndex, int colIndex)
{
    if (rowIndex >= 0 && rowIndex < image.rows && colIndex >= 0 && colIndex < image.cols)
    {
        return true;
    }
    return false;
}

void myCalcOpticalFlowLK(cv::Mat image1, cv::Mat image2,
                          vector<cv::Point2f> &prev_kp, vector<cv::Point2f> &next_kp,
                          vector<uchar> &status, int patch_size=21, int iteration=10)
{
    int i;
    double threshold = 0.5;
    cv::Mat image1_gray, image2_gray;
    cv::cvtColor(image1, image1_gray, cv::COLOR_BGR2GRAY);
    cv::cvtColor(image2, image2_gray, cv::COLOR_BGR2GRAY);
    status.reserve(prev_kp.size());

    for(i = 0; i < prev_kp.size(); i++)
    {
        uchar success = 1;
        Eigen::Vector2f p = Eigen::Vector2f::Zero();
        int x = prev_kp[i].x, y = prev_kp[i].y;
        Eigen::Vector2f delta_p = Eigen::Vector2f::Zero();

        for(int iter = 0; iter <= iteration; iter++)
        {
            Eigen::Matrix2f Hessian = Eigen::Matrix2f::Zero();
            Eigen::Vector2f err_total = Eigen::Vector2f::Zero();

            for(int x_step = -patch_size/2; x_step < patch_size/2; x_step++)
                for(int y_step = -patch_size/2; y_step < patch_size/2; y_step++)
                {
                    if(!isIndexValid(image1_gray, y + y_step, x + x_step) || !isIndexValid(image2_gray, y + y_step + p(1), x + x_step + p(0)))
                    {
                        continue;
                    }

                    float err = image1_gray.at<uchar>(y + y_step, x + x_step) - image2_gray.at<uchar>(y + y_step + p(1), x + x_step + p(0));
                    float jacob_x = 0.5 * ( image2_gray.at<uchar>(y + y_step + p(1), x + x_step + p(0) + 1) - image2_gray.at<uchar>(y + y_step + p(1), x + x_step + p(0) - 1));
                    float jacob_y = 0.5 * ( image2_gray.at<uchar>(y + y_step + p(1) + 1, x + x_step + p(0)) - image2_gray.at<uchar>(y + y_step + p(1) - 1, x + x_step + p(0)));
                    Eigen::Vector2f J(jacob_x, jacob_y);
                    err_total += J * err;
                    Hessian += J * J.transpose();
                }

            delta_p = Hessian.ldlt().solve(err_total);
            p += delta_p;

            if(delta_p.norm() < threshold)
            {
                break;
            }
        }

        next_kp.emplace_back(x + p(0), y + p(1));
        status[i] = 1;
    }
}


int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    cout << "associate file is: " << associate_file << endl;
    
    ifstream fin( associate_file );
    if ( !fin ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
        return 1;
    }
    
    string rgb_file, depth_file, time_rgb, time_depth;
    list< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    cv::Mat color, depth, last_color;
    
    for ( int index=0; index<100; index++ )
    {
        fin >> time_rgb >> rgb_file >> time_depth >> depth_file;
        color = cv::imread( path_to_dataset + "/" + rgb_file );
        depth = cv::imread( path_to_dataset + "/" + depth_file, -1 );
        if (index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect( color, kps );
            for ( auto kp:kps )
                keypoints.push_back( kp.pt );
            last_color = color;
            continue;
        }
        if ( color.data == nullptr || depth.data == nullptr )
            continue;
        // 对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_keypoints; 
        vector<cv::Point2f> prev_keypoints;
        for ( auto kp:keypoints )
            prev_keypoints.push_back(kp);
        vector<unsigned char> status;
        vector<float> error;
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        myCalcOpticalFlowLK(last_color, color, prev_keypoints, next_keypoints, status);
//        cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
        cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;
        // 把跟丢的点删掉
        int i=0; 
        for ( auto iter = keypoints.begin(); iter != keypoints.end(); i++)
        {
            if ( status[i] == 0 )
            {
                iter = keypoints.erase(iter);
                continue;
            }
            *iter = next_keypoints[i];
            iter++;
        }
        cout << "tracked keypoints: " << keypoints.size() << endl;
        if (keypoints.size() == 0)
        {
            cout << "all keypoints are lost." << endl;
            break;
        }
        // 绘制特征点和光流线段
        for (int j = 0; j < prev_keypoints.size(); j++)
        {
            if (status[j])
            {
                cv::line(last_color, prev_keypoints[j], next_keypoints[j], cv::Scalar(255, 0, 0), 2);
                cv::circle(last_color, prev_keypoints[j], 3, cv::Scalar(0, 0, 255), -1);
            }
        }

        // 显示结果图像
        cv::imshow("Optical Flow", last_color);
        cv::waitKey(0);
        last_color = color;
    }
    return 0;
}

CMakeLists.txt：

cmake_minimum_required( VERSION 2.8 )
project(LKOptical)

set( CMAKE_BUILD_TYPE Debug )

find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS} )
include_directories( ${EIGEN3_INCLUDE_DIRS} )

add_executable(LKOptical useLK.cpp)
target_link_libraries(LKOptical ${OpenCV_LIBS})

输出结果