视觉SLAM笔记（42） 光流法跟踪特征点

---

1. 使用 TUM 公开数据集

准备了若干张数据集图像，存放在程序目录中的 VSLAM_note/042/data/ 文件夹下
它们来自于慕尼黑工业大学（TUM）提供的公开 RGB-D 数据集
以后就称之为 TUM 数据集

它含有许多个 RGB-D 视频，可以作为 RGB-D 或单目 SLAM 的实验数据
它还提供了用运动捕捉系统测量的精确轨迹，可以作为标准轨迹以校准 SLAM 系统
由于该数据集比较大，这里中使用了一部分“freiburg1_desk”数据集中的图像

可以在 TUM数据集主页 找到它的下载链接，或者，也可以直接使用在 GitHub 上提供的部分(十张图)

数据位于 data/ 下，以压缩包形式提供（data.tar.gz）
由于 TUM数据集是从实际环境中采集的，需要解释一下它的数据格式（数据集一般都有自己定义的格式）

在解压后，将看到以下这些文件：

1. rgb.txt 和 depth.txt 记录了各文件的采集时间和对应的文件名
2. rgb/ 和 depth/目录存放着采集到的 png 格式图像文件
   彩色图像为八位三通道，深度图为 16 位单通道图像，文件名即采集时间
3. groundtruth.txt 为外部运动捕捉系统采集到的相机位姿

格式为：$(time,$ $t$_x, $t$_y, $t$_z, $q$_x, $q$_y, $q$_z, $q$_w$)$
可以把它看成标准轨迹

请注意彩色图、深度图和标准轨迹的采集都是独立的，轨迹的采集频率比图像高很多
在使用数据之前，需要根据采集时间，对数据进行一次时间上的对齐，以便对彩色图和深度图进行配对
原则上，可以把采集时间相近于一个阈值的数据，看成是一对图像
并把相近时间的位姿，看作是该图像的真实采集位置
TUM 提供了一个 python 脚本“associate.py”来完成这件事
在 GitHub 上也有提供

请把此文件放到数据集目录下，运行：

$ python associate.py rgb.txt depth.txt > associate.txt

这段脚本会根据输入两个文件中的采集时间进行配对，最后输出到一个文件 associate.txt
输出文件含有被配对的两个图像的时间、文件名信息，可以作为后续处理的来源
此外， TUM 数据集还提供了比较估计轨迹与标准轨迹的工具

---

2. 使用 LK 光流

使用 LK 的目的是跟踪特征点
对第一张图像提取 FAST 角点，然后用 LK 光流跟踪它们，并画在图中

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

int main(int argc, char** argv)
{
    // 加载 associate.txt
    if (argc != 2)
    {
        cout << "usage: LKFlow path_to_dataset" << endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";
    ifstream fin(associate_file);
    if (!fin)
    {
        cerr << "Cann't find associate.txt!" << endl;
        return 1;
    }

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, last_color;
    list< cv::Point2f > keypoints;   // 因为要删除跟踪失败的点，使用list

    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();
        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;
        }
        
        // 画出 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;
}

该程序的运行参数里需要指定数据集所在的目录，例如：

$ ./LKFlow ../data

会在每次循环后暂停程序，按任意键可以继续运行

如果采用完整数据时
会看到图像中大部分特征点能够顺利跟踪到，但也有特征点会丢失
丢失的特征点或是被移出了视野外，或是被其他物体挡住了
如果不提取新的特征点，那么光流的跟踪会越来越少：

仔细观察特征点的跟踪过程，会发现位于物体角点处的特征更加稳定
边缘处的特征会沿着边缘“滑动”，这主要是因为沿着边缘移动时特征块的内容基本不变
因此程序容易认为是同一个地方
而既不在角点，也不在边缘的的特征点则会频繁跳动，位置非常不稳定
这个现象很像围棋中的“金角银边草肚皮”：角点具有更好的辨识度，边缘次之，区块最少

另一方面，可以看到光流法的运行时间
在跟踪 1749 个特征点时， LK 光流法大约需要 48 毫秒左右
如果减小特征点的数量，则会明显减少计算时间
看到， LK 光流跟踪法避免了描述子的计算与匹配，但本身也需要一定的计算量
在计算平台上，使用 LK 光流能够节省一定的计算量
但在具体 SLAM 系统中使用光流还是匹配描述子，最好是亲自做实验测试一下

另外， LK 光流跟踪能够直接得到特征点的对应关系
这个对应关系就像是描述子的匹配
但实际上我们大多数时候只会碰到特征点跟丢的情况，而不太会遇到误匹配
这应该是光流相对于描述子的一点优势
但是，匹配描述子的方法在相机运动较大时仍能成功，而光流必须要求相机运动是微小的
从这方面来说，光流的鲁棒性比描述子差一些

最后，可以通过光流跟踪的特征点，用 PnP、 ICP 或对极几何来估计相机运动，
总而言之，光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程
但要求相机运动较慢（或采集频率较高）

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（41） 光流
视觉SLAM笔记（40） 特征点的缺陷
视觉SLAM笔记（39） 求解 ICP
视觉SLAM笔记（38） 3D-3D: ICP
视觉SLAM笔记（37） 求解 PnP

---