SLAM程序阅读（第8讲 LK光流法）

本期，小绿带大家阅读高翔Slambook第8讲中LK光流法程序。

细心的同学已经发现，小绿换了文章的封皮，因为有一些同学都觉得原来那张图比较捞，不沉稳也不正经…而更细心的同学也会发现，小绿连题目都改了，原来叫“解读”，现在叫“阅读”，这也是因为一些热心的同学在后台积极提问，然而小绿作为一个门徒，实在是有些束手无策，没法很透彻的解答同学们的问题…

确实，比方说在第7讲中的几个.cpp，求E矩阵需要使用findEssentialMat函数，求F矩阵需要使用findFundamentalMat函数，这两个函数虽由OpenCV提供，而且原理使用对极约束，但具体求取E、F时构造的是如何的一个最小二乘问题？求解时又使用何种方式求解？再比如说三角测量中，使用的triangulatePoint函数，其1、2号形参是两个projection matrix，也叫投影矩阵，那么这两个投影矩阵是怎么求得？又为什么简单地把R、t做一个增广就叫做projection matrix而不叫function matrix….等等诸如此类的问题，小绿确实由于没有深入阅读OpenCV源码，直接当做封装好的函数，当做一个工具去使用，却并没有深入其原理。

所以从本期开始，小绿没法再带着大家去“解读”程序啦o(╥﹏╥)o…小绿只能带着大家去“阅读”程序~~

好了，闲话到此为止，现在咱们来看一下Slambook第8讲的第一个程序：useLK.cpp。

首先，来了解一下程序的用途：useLK.cpp这个程序是一个演示使用LK光流法跟踪特征点运动轨迹的实例，通过从数据库截取9张RGB图像（这里虽然data数据集里包含了9张深度图，然而只是为了读取RGB图像方便，为了使用associate.txt中排好序的图像名称，而在之后使用直接法求解位姿时才使用深度信息），在第一张图像中寻找FAST角点作为特征点，进而在后续的图像中使用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;
   }
   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;      // 因为要删除跟踪失败的点，使用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( 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;
}

首先是对输入参数个数的判断。这里我们只需传入“数据集排序文件”associate.txt所在的文件夹就可以，因而argc的判别值为2。

string path_to_dataset = argv[1];
string associate_file = path_to_dataset + "/associate.txt";

这里定义了两个string类变量，即两个字符串，分别存储associate.txt所在文件夹的绝对路径，与associate.txt的结对路径。

ifstream fin( associate_file );
   if ( !fin )
   {
       cerr<<"I cann't find associate.txt!"<<endl;
       return 1;
   }

这里实例化了一个ifstream输入文件流类的变量fin，并直接初始化为associate_file所存储的字符串。随后判断是否能够打开fin所存储路径下的文件，而在判断语句中“！fin”并不是说判断fin是否为0或者为空，而是ifstream类重载了“!”操作符，所以当我们如此使用的时候，是“!”操作符函数返回一个bool类变量来标记是否成功，成功则为1。

list< cv::Point2f > keypoints;

还记得之前用于存储特征点的keypoints是Point2f类的容器，而现在使用list链表是为了方便插入与删除某个元素，这里是为了方便在后续光流法跟踪时删除跟丢的点。

for ( int index=0; index<100; index++ )
   {
      ...
   }

在每次循环中，输入流fin输入associate.txt每行的数据，因为associate文件的每一行分别是time_color、color、time_depth、depth，所以分别将其赋值给存储文件名称或文件产生时间的变量：

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

此后，针对第一张图像，按照FAST角点寻找特征点并存入keypoints中；进而在后续帧之间使用LK进行特征点的跟踪。

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

这里，通过判断color与depth两个Mat类变量中数据存储区data是否为空指针，来判断是否成功找到了本帧所对应的彩色图与深度图。如果有一项为空，则continue进行下一次循环。

vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;
for ( auto kp:keypoints )
     prev_keypoints.push_back(kp);

这里定义了两个存储Point2f类的容器next_keypoints与prev_keypoints，分别用来存储当前帧（下一帧）通过光流跟踪得到的特征点的像素坐标，与前一帧特征点的像素坐标。其中，前一帧的特征点需要将存储特征点的list进行遍历（每次光流跟踪后会有坏点剔除），分别存入prev_keypoints。

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

这里调用OpenCV提供的光流法计算函数calcOpticalFlowPyrLK，通过金字塔LK光流法计算当前帧跟踪得到的特征点的像素坐标并存入next_keypoints，同时会将每一个特征点的跟踪情况存入同维度的容器status与error，用来判断该特征点是否被成功跟踪。

// 把跟丢的点删掉
       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++;
       }

这里创建一个链表keypoints的迭代器iter，依次访问内部元素，通过判断status容器内同位置的标志量是否为0，来选择是否在链表内部删除该特征点。若未跟丢，则使用当前帧该特征点运动到的像素位置替换keypoints中该特征点存储的像素位置（即在前一帧的位置）。

// 画出 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提供的特征点圈画函数circle画出特征点，并将圈画过的图像输出到屏幕上。

程序的运行结果在文首已经展示过了。如果大家没看过瘾，第九章Project部分提供了一个RGBD数据集，我们在包含数百张RGBD-深度图像的数据集中再次运行本程序进行LK光流跟踪，结果如下（由于上传的gif不能超过2m，小绿只截取了其中的一些帧）：

本期程序useLK.cpp小绿就带领大家阅读到这里，水平有限，难免有疏漏之处。如有问题或者疑问，尽管在后台向小绿指出，在此表示感谢。