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;      
   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 )
       {
           
           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;
       
       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;
       }
       
       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中該特徵點存儲的像素位置（即在前一幀的位置）。

 

       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小綠就帶領大家閱讀到這裡，水平有限，難免有疏漏之處。如有問題或者疑問，儘管在後臺向小綠指出，在此表示感謝。

相關閱讀

圖像特徵點|moravec特徵點

入門學習SLAM（Ubuntu16.04安裝ROS kinetic）

高翔Slambook第七講代碼解讀（特徵點提取）

高翔Slambook第七講代碼解讀（三角測量）

高翔Slambook第七講代碼解讀（2d-2d位姿估計）

高翔Slambook第七講代碼解讀（3d-2d位姿估計）

高翔Slambook第七講代碼解讀（3d-3d位姿估計）