LK 光流法

LK 光流法

首先是对输入参数个数的判断。这里我们只需传入“数据集排序文件”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;

使用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, 5, cv::Scalar(0, 240, 0), 1);
        cv::imshow("corners", img_show);

最后，深拷贝当前帧（用“=”浅拷贝会修改原图），并使用CV提供的特征点圈画函数circle画出特征点，并将圈画过的图像输出到屏幕上。

2D-2D 位姿估计

调用这个函数会返回一个cv::Point2d类的变量，而Point2d类的变量会存储一个2d点的xy坐标，即有两个成员变量.x和.y，类型为double。其中，使用K.at (0,0)访问K矩阵（相机内参）的第(0,0)号元素。

Point2d pixel2cam(const Point2d& p, const Mat& K){
    return Point2d(
        (p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}

pose_estimation_2d2d（本程序的主要计算环节）

void pose_estimation_2d2d ( const vector< KeyPoint >& keypoints_1, const vector< KeyPoint >& keypoints_2, const vector< DMatch >& matches, Mat& R, Mat& t )
{
 ...  
}

由于在进行对极几何计算时，需要使用特征点的2d坐标，而此时两帧图像中的特征点坐标还保存在两个存储KeyPoint类对象的容器keypoints_1和keypoints_2中，因此我们需要将其中的特征点坐标信息提取出来（方便一会使用OpenCV提供的计算函数进行计算）：

vector<Point2f> points1;
vector<Point2f> points2;

   for ( int i = 0; i < ( int ) matches.size(); i++ )
   {
       points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
       points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
   }

这里特征点坐标按照Point2f进行存储，因为在keypoints_1中存储的点的坐标（keypoints_1[0].pt）是Point2f类型的，因此定义两个存储Point2f类型对象的容器points1与points2。在坐标值的存储循环中，以i=0为例：

points1.push_back ( keypoints_1[matches[0].queryIdx].pt );
points2.push_back ( keypoints_2[matches[0].trainIdx].pt );

调用容器对应的push_back()函数将括号内的值加入到容器的尾部。此时加入的值分别为keypoints_1[matches[0].queryIdx].pt与keypoints_2[matches[0].trainIdx].pt，分别为第一对特征点对（角标从0开始，i=0对应第一对特征点对）中，前一帧（查询图像）中的特征点索引在keypoints_1中对应的特征点坐标，与后一帧（训练图像）中的特征点索引在keypoints_2中对应的特征点坐标（有点啰嗦）。

//-- 计算基础矩阵
   Mat fundamental_matrix;
   fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
   cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

进而，调用OpenCV提供的基础矩阵计算函数findFundamentalMat，按照八点法进行计算，并返回一个4×4的F矩阵fundamental_matrix。

//-- 计算本质矩阵
   Point2d principal_point ( 325.1, 249.7 );  //相机光心, TUM dataset标定值
   double focal_length = 521;      //相机焦距, TUM dataset标定值
   Mat essential_matrix;
   essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
   cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

同理，调用OpenCV提供的本质矩阵计算函数findEssentialMat计算本质矩阵essential_matrix。由于计算本质矩阵E时需要提供归一化平面坐标，因此需要将像素坐标转化成归一化平面坐标，需要提供相机内参cu、cv与f。最后，通过OpenCV提供的R、t计算函数recoverPose计算R和t。由于函数默认使用本质矩阵进行解算，因此需要传入E。

//-- 从本质矩阵中恢复旋转和平移信息.
   recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
   cout<<"R is "<<endl<<R<<endl;
   cout<<"t is "<<endl<<t<<endl;

下面的代码是为了验证刚才所计算得到的R和t是否满足对极约束

void verify_polar_constraint(
 const vector< KeyPoint >& keypoints_1, const vector< KeyPoint >& keypoints_2, const Mat& R, const Mat& t, const vector< DMatch >& matches )
{
 
   //-- 验证E=t^R*scale
   Mat t_x = ( Mat_<double> ( 3,3 ) <<
               0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
               t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
               -t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );

   cout<<"t^R="<<endl<<t_x*R<<endl;

   //-- 验证对极约束
   Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
   for ( DMatch m: matches )
   {
       Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
       Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );
       Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
       Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
       Mat d = y2.t() * t_x * R * y1;
       cout << "epipolar constraint = " << d << endl;
   }
}

根据两帧图像中筛选出的102对特征点对，计算出了基础矩阵F和本质矩阵E，以及单应矩阵H（这里特征点不属于共面情况，因此单应矩阵H的计算无法用来求解R、t）。进而求解出相机位姿变换R、t，并通过计算验证了各对特征点是否满足对极约束，结果是满足的。

完整代码

ubuntu系统版本18.04

Cmakelist

project( useLK )

set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

find_package( OpenCV )
include_directories( ${OpenCV_INCLUDE_DIRS} )

add_executable( useLK useLK.cpp )
target_link_libraries( useLK ${OpenCV_LIBS} )

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace std; 
using namespace cv;


// 像素坐标转相机归一化坐标
// Point2d pixel2cam ( const Point2d& p, const Mat& K );
// 找到匹配的特征点
void find_feature_matches(
    const Mat& img_1,  const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector<DMatch>& matches);
 
void pose_estimation_2d2d(
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector<DMatch> matches,
    Mat& R, Mat& t);
 
    // 像素坐标转相机归一化坐标
    Point2d pixel2cam(const Point2d& p, const Mat& K);
 
 void posedd(){
    // 读取图像

    Mat img_1 = imread("1.png", CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread("2.png", CV_LOAD_IMAGE_COLOR);
    
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout<<"一共找到了"<<matches.size()<<"组匹配点"<<endl;
 
    // 估计两张图像之间的运动
    Mat R,t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
 
    // 验证E=t^R*scale
    Mat t_x = (Mat_<double>(3,3) <<
                            0,          -t.at<double>(2,0),         t.at<double>(1,0),
                            t.at<double>(2,0),      0,                  -t.at<double>(0,0),
                            -t.at<double>(1,0),     t.at<double>(0,0),      0);
    // 这个t^R就是essential矩阵（本质矩阵），但是相差一个倍数
    cout<<"t^R = "<<endl<<t_x * R<<endl;
 
    // 验证对极约束
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    for(DMatch m:matches){
        Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        Mat y1 = (Mat_<double>(3,1)<<pt1.x, pt1.y, 1);
        Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        Mat y2 = (Mat_<double>(3,1) << pt2.x, pt2.y, 1);
        Mat d = y2.t() * t_x * R * y1;
        // 验证对极约束，理论上这个d应该是近似为0的
        cout<<"epipolar constraint = "<<d<<endl;
    }
//     return void;
}

void find_feature_matches(const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector<DMatch>& matches){
    // 初始化
    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector>detector = ORB::create();
    Ptr<DescriptorExtractor>descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");   // 暴力匹配
 
    // 第一步：检测Oriented FAST角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
 
    // 第二步： 根据角点位置计算BRIEF描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
 
    // 第三步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
    vector<DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);
 
    // 第四步：对匹配点对进行筛选
    double min_dist = 10000, max_dist = 0;
 
    // 找出所有匹配之间的最小距离和最大距离
    // 即是最相似和最不相似的两组点之间的距离
    for(int i=0; i<descriptors_1.rows; i++){
        double dist = match[i].distance;
        min_dist = min_dist<dist?min_dist:dist;
        max_dist = max_dist>dist?max_dist:dist;
    }
    printf("--Max dist : %f\n", max_dist);
    printf("--Min dist : %f \n", min_dist); 
 
    // 当描述子之间的距离大于两倍的最小距离时，即认为匹配有误
    // 设置30为阈值
    for(int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance <= max(2*min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
}
 
Point2d pixel2cam(const Point2d& p, const Mat& K){
    return Point2d(
        (p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}
 
void pose_estimation_2d2d(std::vector<KeyPoint>keypoints_1,
                                                std::vector<KeyPoint>keypoints_2,
                                                std::vector<DMatch>matches,
                                                Mat&R ,Mat& t){
    // 相机内参,TUM Feriburg2
    Mat K = (Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
 
    // 把匹配点转化为vector<Point2f>的形式
    vector<Point2f>points1;
    vector<Point2f>points2;
 
    for(int i=0; i<(int)matches.size(); i++){
        points1.push_back(keypoints_1[matches[i].queryIdx].pt);
        points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    }
 
    // 计算基础矩阵:使用的8点法，但是书上说8点法是用来计算本质矩阵的呀，这两个有什么关系吗
    // 答：对于计算来说没有什么区别，本质矩阵就是基础矩阵乘以一个相机内参
    // 多于8个点就用最小二乘去解
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);    // Eigen库计算会更快一些
    cout<<"fundamental_matrix is "<<endl<<fundamental_matrix<<endl;
 
    // 计算本质矩阵：是由对极约束定义的：对极约束是等式为零的约束
    Point2d principal_point(325.1, 249.7);  // 相机光心,TUM dataset标定值
    double focal_length = 521;      // 相机焦距，TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout<<"essential_matrix is "<<endl<<essential_matrix<<endl;
 
    // 计算单应矩阵:通常描述处于共同平面上的一些点在两张图像之间的变换关系
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
 
    // 从不本质矩阵中恢复旋转和平移信息
    // 这里的R,t组成的变换矩阵，满足的对极约束是:x2 = R * x1 + t,是第一个图到第二个图的坐标变换矩阵x2 = T21 * x1
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
}

int main( int argc, char** argv )
{
     posedd();
    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 );
	  color = cv::imread( path_to_dataset+"/data/rgb_file" );
        depth = cv::imread( path_to_dataset+"data/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, 5, cv::Scalar(0, 250, 0), 1);
        cv::imshow("corners", img_show);
  // 	cv::line(img_show,prev_keypoints,cv::Scalar(0, 250, 0));
        cv::waitKey(0);
        cv::waitKey(0);
        last_color = color;
    }
    return 0;
}