七、LK光流

LK光流法

#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>

int main( int argc, char** argv )

{

    if ( argc != 2 )

    {

        cout<<"usage: useLK path_to_dataset"<<endl;

        return 1;

    }

//读入txt文本到fin中

    string path_to_dataset = argv[1];

    string associate_file = path_to_dataset + "/associate.txt";

   

    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;     

// 因为要删除跟踪失败的点，keypoints使用list，元素类型是Point2f，坐标

// 逐帧操作的将color承接为last_color，然后color读取新的图

    cv::Mat color, depth, last_color;

    for ( int index=0; index<100; index++ )

    {

//读入颜色和深度图像

//从匹配信息associate.txt输入进去

        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;

/***************************************************************************************************************/

读取方式：

13050353.359684 rgb/13050353.359684.png 1305031453.374112 depth/1305031453.374112.png

/***************************************************************************************************************/

        color = cv::imread( path_to_dataset+"/"+rgb_file );

        depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );

// 1. 对第一帧提取FAST特征点存到keypoints中

        if (index ==0 )

        {

        

            vector<cv::KeyPoint> kps;

            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();

            detector->detect( color, kps );

            for ( auto kp:kps )

//.pt转化为像素坐标系，存储到list< cv::Point2f > keypoints

// list类型的keypoints初始化第一帧检测角点后，将所有角点坐标存入。他的更新只有减少，没有增加(只有跟丢的点进行删除)

            keypoints.push_back( kp.pt );

            last_color = color;

            continue;

        }

//实际只有10张图，后面10-99没有数据所以一直continue掉，index秒到99

        if ( color.data==nullptr || depth.data==nullptr )

            continue;

    // 2. 对其他帧用LK跟踪特征点

// next_keypoints和prev_keypoints每帧跟踪循环内被定义的，也就是每次进循环被定义，出循环被释放.

        vector<cv::Point2f> next_keypoints;

        vector<cv::Point2f> prev_keypoints;

        for ( auto kp:keypoints )

            prev_keypoints.push_back(kp);

//调用calcOpticalFlowPyrLK函数

//status匹配状态，匹配上赋1，否则赋0；

        vector<unsigned char> status;

        vector<float> error;

        cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );

 // 把跟丢的点删掉

// 3. 更新keypoints列表，从prev_keypoints到next_keypoints

/***************************************************************************************************************/

iter是keypoints的迭代器，正常情况下更新keypoints为下一帧特征点坐标，直接*iter=next_keypoints[i]即可。i++,iter++二者同步更新；

特殊情况下，出现跟踪失败的特征点，在keypoints先把这个位置删除，同时.erase()迭代器返回的是下一个的位置。与此同时这里的continue能结束本次循环，重新下一次循环i++，这样可以跳过这个数值，下一次*iter=next_keypoints[i]时，iter已经指向了下一个位置，同时i++了，完美赋值；

/***************************************************************************************************************/

        int i=0;

        for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)

        {

//跟丢了删除，iter保持当前位置，不会++，但i会++；

//status 输出状态向量。如果找到相应特征的流，则向量的每个元素都设置为1。否则，它被设置为0。

            if ( status[i] == 0 )//跟踪失败，keypoints中数据被跳过

            {

                iter = keypoints.erase(iter);  //列表操作，

                continue;

            }

//跟踪上的好点，才会让iter指向

            *iter = next_keypoints[i];//跟踪失败，keypoints中数据被跳过，指向next_keypoints

            iter++;

        }

        cout<<"tracked keypoints: "<<keypoints.size()<<endl;

        if (keypoints.size() == 0)

        {

            cout<<"all keypoints are lost."<<endl;

            break;

        }

 // 画出 keypoints

        cv::Mat img_show = color.clone();

        for ( auto kp:keypoints )//遍历列表keypoints

            cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);//画圆

        cv::imshow("corners", img_show);

        cv::waitKey(0);

        last_color = color; // 一个循环完成，当前图像为过去

    }

    return 0;

}