OpenCV LK光流法测试

OpenCV版本： 3.2.0

例程文件目录/samples/cpp/lkdemo.cpp

原始程序是采集相机数据，台式机没有摄像头，用Euroc测试集，偷ORB_SLAM2 /Examples/Monocular/mono_euroc.cc里的LoadImages函数读取数据

在cpp目录下新建一个自己的目录mylk，拷贝lkdemo和example_cmake目录下的CMakeLists.txt并修改：

add_executable(lk_example lkdemo.cpp)
target_link_libraries(lk_example ${OpenCV_LIBS})

新建目录build，进入后cmake .. + make 编译运行即可。run.sh如下:

./lkdemo ~/slamData/EuRoC/mav02/cam0/data ~/ORB_SLAM2/Examples/Stereo/EuRoC_TimeStamps/MH02.txt

先提取角点，亚像素精度提炼角点，光流跟踪。

运行过程中会监控鼠标和键盘：

按下鼠标左键 -> 添加一个角点

按下r -> 初始化角点

按下c -> 清除角点

按下n -> 只显示角点（右图）

按下esc -> 退出

修改后的程序：

#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/videoio.hpp"
#include "opencv2/highgui.hpp"

#include <iostream>
#include <ctype.h>
#include <fstream>
#include <algorithm>
using namespace std;


using namespace cv;
void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImages.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImages.push_back(strImagePath + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}
static void help()
{
    // print a welcome message, and the OpenCV version
    cout << "
This is a demo of Lukas-Kanade optical flow lkdemo(),
"
            "Using OpenCV version " << CV_VERSION << endl;
    cout << "
It uses camera by default, but you can provide a path to video as an argument.
";
    cout << "
Hot keys: 
"
            "	ESC - quit the program
"
            "	r - auto-initialize tracking
"
            "	c - delete all the points
"
            "	n - switch the "night" mode on/off
"
            "To add/remove a feature point click it
" << endl;
}

Point2f point;
bool addRemovePt = false;

static void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ )
{
    if( event == EVENT_LBUTTONDOWN )
    {
        point = Point2f((float)x, (float)y);
        addRemovePt = true;
    }
}

int main( int argc, char** argv )
{
    VideoCapture cap;
    TermCriteria termcrit(TermCriteria::COUNT|TermCriteria::EPS,20,0.03);
    Size subPixWinSize(10,10), winSize(31,31);

    const int MAX_COUNT = 500;
    bool needToInit = false;
    bool nightMode = false;


    namedWindow( "LK Demo", 1 );
    setMouseCallback( "LK Demo", onMouse, 0 );

    Mat gray, prevGray, image, frame;
    vector<Point2f> points[2];

    vector<string> vstrImageFilenames;
    vector<double> vstrstamps;
    LoadImages(string(argv[1]), string(argv[2]), vstrImageFilenames, vstrstamps);
    for(int i=0;i<vstrImageFilenames.size();i++)
    {

        image = imread(vstrImageFilenames[i],IMREAD_COLOR);
        cvtColor(image, gray, COLOR_BGR2GRAY);

        if( nightMode )
            image = Scalar::all(0);

        if( needToInit )
        {
            // automatic initialization
            goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, Mat(), 3, 0, 0.04);
            cornerSubPix(gray, points[1], subPixWinSize, Size(-1,-1), termcrit);
            addRemovePt = false;
        }
        else if( !points[0].empty() )
        {
            vector<uchar> status;
            vector<float> err;
            if(prevGray.empty())
                gray.copyTo(prevGray);
            calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize,
                                 3, termcrit, 0, 0.001);
            size_t i, k;
            for( i = k = 0; i < points[1].size(); i++ )
            {
                if( addRemovePt )
                {
                    if( norm(point - points[1][i]) <= 5 )
                    {
                        addRemovePt = false;
                        continue;
                    }
                }

                if( !status[i] )
                    continue;

                points[1][k++] = points[1][i];
                circle( image, points[1][i], 3, Scalar(0,255,0), -1, 8);
            }
            points[1].resize(k);
        }

        if( addRemovePt && points[1].size() < (size_t)MAX_COUNT )
        {
            vector<Point2f> tmp;
            tmp.push_back(point);
            cornerSubPix( gray, tmp, winSize, Size(-1,-1), termcrit);
            points[1].push_back(tmp[0]);
            addRemovePt = false;
        }

        needToInit = false;
        imshow("LK Demo", image);
    
        char c = (char)waitKey(10);
        if( c == 27 )
            break;
        switch( c )
        {
        case 'r':
            needToInit = true;
            break;
        case 'c':
            points[0].clear();
            points[1].clear();
            break;
        case 'n':
            nightMode = !nightMode;
            break;
        }

        std::swap(points[1], points[0]);
        cv::swap(prevGray, gray);
    }

    return 0;
}

参考：goodFeaturesToTrack函数详细注释