【学习笔记】视觉SLAM十四讲第八讲：使用LK光流

最近在学习视觉SLAM十四讲，发现数学方面的内容比较好理解（机械学院自动控制工科出身），但涉及到实践部分与代码相关的内容就感觉有点啃不懂（没系统学习过编程），经常有些代码的细节看不懂，网上查到的内容大多也是一笔带过（估计我水平太低， 很多显而易见的内容都不懂），所以决定把自己的学习心得记录下来，一个是方便自己学习和整理知识，另一个也是方便其他像我一样的小白在搜索相关问题的时候能找到详细的解释。

     代码和解释如下:  

#include <iostream>
#include <fstream>    //为了代码后面对文件的读写操作
#include <list>        // list将数据存储在非连续内存空间中，由前驱和后驱指针将所有元素连接起来， 方便删除和插入元素。而vector对元素操作后，所有元素的存储地址都会有所影响。
#include <vector>    // vector是c++支持的另一种容器类型
#include <chrono>    // 与时间相关的库， 包括duration， time point等
using namespace std;

// 下面是需要用到的opencv相关的库
#include <opencv2/core/core.hpp>        //核心模块，主要包含了opencv用到的数据结构，如point
#include <opencv2/highgui/highgui.hpp>    // 图形用户界面，包含imread，imshow等
#include <opencv2/features2d/features2d.hpp>    // 2d特征框架，包含特征点检测子，特征点描述子以及匹配框架
#include <opencv2/video/tracking.hpp>    // 包括不同的跟踪方法的实现。

int main(int argc, char** argv)
{
    if (argc !=2)    //检查输入参数是否为2，第一个参数是编译后的可执行文件，本例中第二个参数是associate文件的所在目录，所以判别值为2，没有其他参数。
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset = argv[1];    //把第二个参数，associate文件所在路径传递给path_to_dataset。
    string associate_file = path_to_dataset + "/associate.txt";
    ifstream fin(associate_file);    //以读取方式打开文件associate_file
    string rgb_file, depth_file, time_rgb, time_depth;
    list<cv::Point2f> keypoints;    //定义一个类型为cv::Point2f的list容器用来存储特征点
    cv::Mat color, depth, last_color;    //定义三个类型为cv::Mat的矩阵用来存储颜色，深度和最后一个颜色的数据。
    for (int index=0; index<100; index++)
    {
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;    //分别把associate文件里的数据写入四个string，数据顺序用associate文件中每行的数据顺序决定，不同数据用空格分开。
        color = cv::imread(path_to_dataset + "/" + rgb_file);
        depth = cv::imread(path_to_dataset + "/" + depth_file, -1);    //第二个参数为-1，读取深度图像。
        if (index == 0)
        {
            //第一帧， 提取FAST特征点, cv::KeyPoint 是opencv中的关键点类型，
//包含pt(x,y):关键点的点坐标
//size():该关键点邻域直径大小；
//angle:角度，表示关键点的方向，值为[零,三百六十)，负值表示不使用。
// response:响应强度
//等信息。
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector -> detect (color, kps); //第一个参数是Image，第二个参数是feature
            
            for (auto kp:kps)
                keypoints.push_back(kp.pt);    //注意，消息的keypoints在之前定义为point2f类型的list， 这一步是要把KeyPoint类型的点转换成point2f类型。
            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();
        cv::calcOpticalFlowPyrLK(last_color, color, prev_keypoints, next_keypoints, status, error);    //opencv实现的金字塔Lucas-Kanade光流法
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        //解释duration
        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)    // 等于0表示跟丢特征点
            {
                iter = keypoints.erase(iter);
                continue;
            }
            *iter = next_keypoints[i];
            iter++;
        }
        
        // 画出 keypoints
        cv::Mat img_show = color.clone();
        for (auto kp: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;
}