《SLAM十四讲》8.3 使用LK光流法useLK.hpp

LK光流法的步骤：

1.对第一帧提取FAST特征点存到keypoints中
2.对其他帧用LK跟踪特征点
3.更新keypoints列表，从prev_keypoints到next_keypoints
4.画出 keypoints圆圈

本博客不讲解理论部分，直接上主程序。因为本博客是博主的学习分享帖子，可能有些繁琐，见谅~

#include <iostream>
#include<fstream>//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<opencv2/video/tracking.hpp>,用到列表，所以要include<list><vector>


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
    
    string associate_file = path_to_dataset + "/associate.txt";//拼接，形成文件地址用于获取associate文件地址
    
    ifstream fin( associate_file );//打开associate_file下的txt文件，以fin的方式打开，如果找不到文件则不会创建相关度的文件夹
    
    string rgb_file, depth_file, time_rgb, time_depth;//因为associate文件的每一行分别是time_color,color,time_depth,depth.所以定义这4个路径，方便之后读取深度图和彩色图。
    
    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