視覺SLAM 十四講——對極約束求解相機運動（2D-2D）

主要內容

1. 對極約束

　　幾何意義是 ，P， 三者共面，對極約束同時包含了平移和旋轉。

　　基礎矩陣：

　　本質矩陣：

　　對極約束表示：

　　　　其中， 分別表示為相機坐標系下歸一化的平面坐標

2. 本質矩陣的特點（3×3）

　　1）E在不同尺度下是等價的

　　2) 內在性質：奇異值必定是 的形式

　　3） 由於平移+旋轉，共有6個自由度，但因為尺度等價性，E實際上有5個自由度

3 本質矩陣求解——八點法

　　1） E的內在性質是一種非線性性質，在求解線性方程時會帶來麻煩，因此不會用5對點來求解。

　　2） 未知數共有9個，只考慮尺度等價性，所以用8對2D-2D點來求解，線性變換的方法。

　　3） 求解矩陣后，進行奇異值分解分（SVD），得到運動信息

　　4） 分解的時候，會有4中情況，選取一個點進行求解，檢測該點在兩個相機下的深度信息，選取兩個正的深度。

　　5） 線性方程解可能不滿足E的內在性質，將求解出來的矩陣投影到E所在的流形上。

4 單應矩陣H

　　

　　定義與旋轉，平移及平面的參數有關。

　　　　

　　1) 自由度為8的單應矩陣可以通過4對匹配點進行計算（注意，這些特征點不能有三點共線的情況）

　　2） 求解方法：直接線性變換法，對其進行分解得到運動信息，分解方法有數值法和解析法

　　　　4組解，（根據計算點的深度以及平面的法向量信息選取合理的解）

　　3） 意義

　　　　3.1） 描述了兩個平面之間的映射關系，若所有特征點落在同一平面，則可以通過H估計運動，

　　　　　　　　具體應用在無人機攜帶俯視相機或掃地機器人的頂視相機中。

　　　　3.2）當特征點共面或者相機發生純旋轉時，基礎矩陣自由度下降，H可以估計純旋轉的運動信息。

5. 在實際應用，為避免退化現象造成的影響，會同時估計F和H，選擇其中重投影誤差比較小的那個作為最終運動的估計結果。

6. 對極幾何2D-2D的特點

　　1）尺度不確定性

　　　　t歸一化相當於固定了尺度信息，成為單目SLAM的初始化，后面的軌跡和地圖以此單位進行計算。

　　　　  初始化不可避免，初始化完畢以后，計算特征點空間位置，可以利用3D-2D的方法計算相機運動。

　　2） 初始化的純旋轉問題

　　　　　　     純旋轉 → t=0 → E = 0 → 無法求解R,

　　　　or:     用H求解R， 但是t為0,導致無法利用三角測量求解特征點位置

　　　　　　單目SLAM初始化，不能只有純旋轉，必須要有一定程度的平移。

　　3） 多余8對點的情況

　　　　利用最小二乘進行求解矩陣E。

　　　　當可能存在誤匹配的情況時，更傾向於使用隨機采樣一致性（Random Sample Concensus，RANSAS），可以處理帶有錯誤匹配的數據。

7. 代碼中需要注意的點：

　　1） Mat類型的操作：C++ ,賦值初始化，乘法，轉置等運算

　　2）三個矩陣的求解

　　3） 從match信息中得到對應圖像中像素的坐標信息

　　4）根據求得的R t在反向通過對極約束進行驗證， 計算的精度情況等。

 

參考鏈接

隨機抽樣一致性算法（RANSAC）

使用cv::findFundamentalMat要注意的幾點

findHomography()函數詳解

Mat_＜double＞ ( 3,3 )是什么意思

OpenCV矩陣運算總結

代碼

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2 
using namespace std;
using namespace cv;

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估計相機運動
 * **************************************************/

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 );

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: pose_estimation_2d2d img1 img2"<<endl;
        return 1;
    }
    //-- 讀取圖像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], 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 );

    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;
    }
    return 0;
}

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;
    // used in OpenCV3 
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2 
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    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描述子進行匹配，使用 Hamming 距離
    vector<DMatch> match;
    //BFMatcher matcher ( NORM_HAMMING );
    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;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_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 Freiburg2
    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 );
    }

    //-- 計算基礎矩陣
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
    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, CV_RANSAC, 3 );
    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

    //-- 從本質矩陣中恢復旋轉和平移信息.
    recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
    
}

結果及分析

Starting: /home/qifarui/code/slambook-master/ch7/build/pose_estimation_2d2d 1.png 2.png
-- Max dist : 94.000000 
-- Min dist : 4.000000 
一共找到了79組匹配點
fundamental_matrix is 
[4.544437503937326e-06, 0.0001333855576988952, -0.01798499246457619;
 -0.0001275657012959839, 2.266794804637672e-05, -0.01416678429258694;
 0.01814994639952877, 0.004146055871509035, 1]
essential_matrix is 
[0.01097677479889588, 0.2483720528328748, 0.03167429208264108;
 -0.2088833206116968, 0.02908423961947315, -0.674465883831914;
 0.008286777626839029, 0.66140416240827, 0.01676523772760232]
homography_matrix is 
[0.9261214281395963, -0.1445322024422802, 33.26921085290552;
 0.04535424466077615, 0.9386696693994352, 8.570979963061975;
 -1.00619755759245e-05, -3.0081402779533e-05, 1]
R is 
[0.9969387384756405, -0.0515557418857258, 0.05878058527448649;
 0.05000441581116598, 0.9983685317362444, 0.02756507279509838;
 -0.06010582439317147, -0.02454140007064545, 0.9978902793176159]
t is 
[-0.9350802885396324;
 -0.03514646277098749;
 0.352689070059345]
t^R=
[-0.01552350379194682, -0.3512511256306389, -0.04479421344178829;
 0.2954056249626309, -0.04113132612112196, 0.9538388002732133;
 -0.01171927330817152, -0.9353667366876339, -0.02370962657084997]
epipolar constraint = [-0.0005415187894598494]
epipolar constraint = [-0.002158321996443405]
epipolar constraint = [0.0003344241034267809]
epipolar constraint = [-8.762783075558223e-06]
epipolar constraint = [0.0002121321904275941]
epipolar constraint = [0.0001088766988978362]
epipolar constraint = [0.0004246621761395189]
epipolar constraint = [-0.003173128213173212]
epipolar constraint = [-3.716645900272639e-05]
epipolar constraint = [-0.001451851842095554]
epipolar constraint = [-0.0009607717987651782]
epipolar constraint = [-0.0008051993509270439]
epipolar constraint = [-0.001424813547085908]
epipolar constraint = [-0.0004339424745618652]
epipolar constraint = [-0.0004786109668010741]
epipolar constraint = [-0.0001965692841337796]
epipolar constraint = [0.001542309775822037]
epipolar constraint = [0.003107154827523384]
epipolar constraint = [0.0006774648880640624]
epipolar constraint = [-0.001176167495898923]
epipolar constraint = [-0.003987986032579591]
epipolar constraint = [-0.001255263863970228]
epipolar constraint = [-0.001553941958799671]
epipolar constraint = [0.002009914868860929]
epipolar constraint = [-0.0006068096317080607]
epipolar constraint = [-2.769881084775661e-05]
epipolar constraint = [0.001274573010445104]
epipolar constraint = [-0.004169986054276958]
epipolar constraint = [-0.001108104734518528]
epipolar constraint = [-0.0005154295528846109]
epipolar constraint = [-0.001776993209361748]
epipolar constraint = [6.67735429456251e-07]
epipolar constraint = [-0.001853977733712375]
epipolar constraint = [-0.0004823070343765445]
epipolar constraint = [0.000474090447079023]
epipolar constraint = [-0.00296104174875747]
epipolar constraint = [-0.003347655248199201]
epipolar constraint = [-0.000197568098110501]
epipolar constraint = [-0.002824643387693802]
epipolar constraint = [-0.0004311798178931181]
epipolar constraint = [0.001181203683149307]
epipolar constraint = [1.756253096285576e-07]
epipolar constraint = [-0.002137829860220063]
epipolar constraint = [0.001153415526361834]
epipolar constraint = [-0.002120357729634967]
epipolar constraint = [2.741528250471692e-06]
epipolar constraint = [0.0009044582009310256]
epipolar constraint = [-0.002100484436687942]
epipolar constraint = [-0.0003517789230311139]
epipolar constraint = [-2.72046495066805e-05]
epipolar constraint = [-0.003784823354081965]
epipolar constraint = [-0.001588562609431521]
epipolar constraint = [-0.002928516012703969]
epipolar constraint = [-0.001016592267328753]
epipolar constraint = [0.0001241874570146462]
epipolar constraint = [-0.0009797104639974082]
epipolar constraint = [0.001457875224074562]
epipolar constraint = [-1.153738168938045e-05]
epipolar constraint = [-0.002733247428635111]
epipolar constraint = [0.001415721256946334]
epipolar constraint = [0.001790255871245483]
epipolar constraint = [-0.002547929672679236]
epipolar constraint = [-0.006257078862295637]
epipolar constraint = [-0.001874877416209744]
epipolar constraint = [-0.002104542913954163]
epipolar constraint = [1.392530300986516e-06]
epipolar constraint = [-0.004013502582045692]
epipolar constraint = [-0.004726241072653375]
epipolar constraint = [-0.001328682673556458]
epipolar constraint = [3.99592575944796e-07]
epipolar constraint = [-0.00508051172487578]
epipolar constraint = [-0.001977141851277564]
epipolar constraint = [-0.001661334265550506]
epipolar constraint = [0.004285762869538863]
epipolar constraint = [0.004087325193214478]
epipolar constraint = [0.0001482534277866508]
epipolar constraint = [-0.000962530114253686]
epipolar constraint = [-0.00234107601156941]
epipolar constraint = [-0.006138005036133154]
*** Exited normally ***

 

　　可以看到，最后利用求得的R t再帶入到對極約束中去，得到的精度在0.00x 精度級別。