Robotics Lab3 ——圖像特征匹配、跟蹤與相機運動估計

圖像特征匹配

圖像特征點

• 攜帶攝像頭的機器人在運動過程中，會連續性地獲取多幀圖像，輔助其感知周圍環境和自身運動。時間序列上相連的兩幅或多幅圖像，通常存在相同的景物，只是它們在圖像中的位置不同。而位置的變換恰恰暗含了相機的運動，這時就需要相鄰圖像間的相似性匹配。
• 選取一大塊圖像區域進行運動估計是不可取的。已知圖像在計算機內部是以數字矩陣的形式存儲的，[如灰度圖的每個元素代表了單個像素的灰度值]。而對於點的提取和匹配較為方便，且和數字值對應，所以引入圖像特征點輔助估計相機的運動。

• 理解：以灰度值存儲的圖像矩陣，單個圖像像素即為圖像中的點，但不一定是圖像中的特征點，因為灰度值受光照、形變、物體材質等的影響嚴重，在不同圖像間的變化大，不夠穩定。而圖像的特征點一定是具有代表性的點，其在相鄰的圖像間保持穩定，一般選擇圖像的角點作為特征點，具有穩定性、易辨認的特征。
• 特征點組成：特征點包括關鍵點(Keypoint)和描述子(Descriptor)兩部分。對特征點作處理時，涉及關鍵點的提取和描述子的計算。其中，關鍵點表示該特征點在圖像中的位置、朝向、大小等信息；描述子通常為一個向量，描述該關鍵點周圍像素的信息，是加以對比的信息。如果兩個特征點的描述子向量之間的距離較小，則可以認為這兩幅圖像中的特征點是同一個。
• 人工特征點：朴素的角點在相機移動或旋轉時會發生變化，因此人為地設計了更加穩定的局部圖像特征：SIFT，SURF，FAST，ORB等。
  1. SIFT充分考慮了光照、尺度、旋轉等信息，計算量很大。
  2. FAST沒有描述子，不具有方向性，計算速度極快。
  3. ORB：關鍵點(Oriented FAST)為改進的FAST角點，其計算了特征點的主方向，為其描述子增加了旋轉不變特性。描述子(BRIEF)采用二進制，計算速度快。

特征點提取

特征點的提取是與關鍵點和描述子緊緊關聯的；

這里以ORB為例展示特征點的提取過程。

• FAST關鍵點：灰度圖中，局部像素灰度值變化明顯的地方極有可能是一個物體的邊緣，角點存在於其中，為與其鄰域像素值差別較大的像素點。角點的檢測(即是否為特征點的判斷)分為以下五個步驟(針對整幅圖像)——來自視覺SLAM十四講：

1. 在圖像中選取像素 p，假設它的亮度為 Ip。
2. 設置一個閾值 T (比如 Ip 的 20%)。
3. 以像素 p 為中心, 選取半徑為 3 的圓上的 16 個像素點。
4. 假如選取的圓上，有連續的 N 個點的亮度大於 Ip + T 或小於 Ip - T，那么像素 p

可以被認為是特征點 (N 通常取 12，即為 FAST-12。其它常用的 N 取值為 9 和 11，
他們分別被稱為 FAST-9， FAST-11)。

5. 循環以上四步，對每一個像素執行相同的操作 。

下圖較為清晰的描述了角點的檢測：

​ 為了提高算法效率，可以跳躍式檢測每個像素鄰域圓上的像素值，如第1,5,9,13個。

​ 為了避免角點集中，需采用非極大值抑制，即在一定區域內僅保留響應極大值的角點。

• Oriented FAST關鍵點：由於FAST角點存在一定的問題，ORB關鍵點對其進行了改進。

  1. FAST特征點數量大且不確定。

  我們往往希望對圖像提取固定數量的特征，這樣易於計算和匹配。改進的方式為：指定最終要提取的角點數量N，對原始提取的FAST角點分別計算Harris響應值，然后選取前N個具有最大響應值的角點。

  1. FAST角點不具有方向信息，而且選取半徑為3的鄰域圓也存在尺度問題。

  ORB關鍵點添加了對尺度和旋轉的描述。其中，尺度不變性通過構建圖像金字塔 [ 同一圖像的不同分辨率表示 ]，在每一層進行角點檢測；

  旋轉不變性采用灰度質心法實現，質心是指以圖像塊灰度值作為權重的中心，其具體步驟如下——來自視覺SLAM十四講：

  1. 在一個小的圖像塊 B 中，定義圖像塊的矩為：

     

  這里的矩實際為HU矩中的原點矩。其一般用來識別圖像中大的物體，能夠較好地描述物體的形狀，要求圖像的紋理特征不能太復雜。

  1. 通過矩可以找到圖像塊的質心：
     

  2. 連接圖像塊的幾何中心 O 與質心 C，得到一個方向向量，於是特征點的方向可以定義為：
     

  這里θ即為旋轉的角度描述。

  知識點拓展：圖像的幾何矩

  • 在數學中，矩的本質是數學期望，即：

  \[\int x · f(x) dx \]

  ​ 其中f(x)代表x的概率密度。

  • 在灰度圖像中，每個像素點的值可以看成該處的密度。對某個像素點的值求期望即得到了圖像在該點處的矩。矩有原點矩(零階矩)、一階矩、二階矩；由零階矩和一階矩可以計算某個形狀的重心，而二階矩可以計算形狀的方向。

    \[零階矩：M_{00} = \sum_{x,y} I(x,y) \]
    \[一階矩：M_{10} = \sum_{x,y} x·I(x,y)；M_{01} = \sum_{x,y}y·I(x,y) \]
    \[推出圖像的重心(質心)坐標：x_c = \frac{M_{10}}{M_{00}}；y_c = \frac{M_{01}}{M_{00}} \]

    這種質心計算方法的優點為：對噪聲不敏感。

  \[二階矩：M_{20} = \sum x^2 · I(x,y)；M_{02} = \sum y^2 · I(x,y)；M_{11}=\sum x·y·I(x,y) \]

  \[ 則物體的形狀方向：θ = \frac{1}{2}arctan(\frac{2b}{a-c})，θ∈[-90°,90°] \]

  \[ 其中，a=\frac{M_{20}}{M_{00}}-x_c^2；b=\frac{M_{11}}{M_{00}}-x_cy_c；c=\frac{M_{02}}{M_{00}}-y_c^2； \]

  圖像的矩具有旋轉、平移、尺度等的不變性，又稱為不變矩。

• BRIEF描述子：一種二進制描述子，向量由多個0和1組成，這里0和1表示關鍵點附近的兩個像素(如p,q)的大小關系——p>q，取1；p<q，取0。而p和q的選取是按照某種概率分布隨機選取。

  優點：速度快，存儲方便；適用於實時的圖像匹配。

• 改進BRIEF描述子(rBRIEF)：加入旋轉因子，改進旋轉不變性；改進特征點描述子的相關性(即可區分性)，對誤匹配率影響較大。

特征匹配

特征匹配實際上描述的是連續的兩張圖像中景物的對應關系，以此來估計相機的運動。

• 匹配方法：

  1. 暴力匹配。即圖像1中取特征點集合A，圖像2中取特征點集合B，對A中的每一個特征點，依次與B中所有的特征點測量兩者描述子的距離，然后排序，取距離最近的作為匹配的兩個特征點。

     這里，描述子距離表示了兩張圖片兩個特征點之間的相似程度，在實際應用中可以取不同的距離度量范數。如：對於浮點類型，使用歐氏距離；對於二進制類型，使用漢明距離[即兩個二進制串之間不同位數的個數]。

     缺點：計算量隨特征點數量增大呈遞增趨勢。

  2. 快速近似最近鄰(FLANN)。集成於OpenCV，適合於匹配點數量極多的情況。

雙目圖像中ORB特征點的提取和匹配代碼分析

注釋寫在了源代碼里。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: feature_extraction img1 img2"<<endl;
        return 1;
    }
    //-- 讀取圖像，以RGB格式讀取
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    //-- 初始化
	/*
	* 1. FeatureDetector特征檢測器類，通過創建該類對象來使用多種特征檢測方法。【用於檢測指定特征點的角點位置】
	     OpenCV2.4.3中提供了10種特征檢測方法：FAST，SIFT，SURF，ORB，HARRIS，SimpleBlob，STAR，MSER，GFTT，Dense。
	     這里使用ORB特征檢測方法 
	* 2. DescriptorExtractor特征描述子提取類，提供了一些特征描述子提取的算法。
	     其可以針對圖像關鍵點，計算其特征描述子，其可以被表達成密集(dense)，即固定維數的向量。
		 其計算方式為每隔固定個像素計算。
		 一個特征描述子是一個向量，特征描述子的集合為Mat格式，每一行是一個關鍵點的特征描述子。
		 OpenCV支持四種類型的特征描述子提取方法：SIFT，SURF，ORB，BRIEF。
	* 3. DescriptorMatcher特征匹配類，提供了一些特征點匹配的方法。
	     該類主要包含圖像對之間的匹配以及圖像和一個圖像集之間的匹配。
		 OpenCV2中的特征匹配方法都繼承自該類。
		 對於誤匹配情況，提供了KNNMatch方法。
	*/
    std::vector<KeyPoint> keypoints_1, keypoints_2;  // 聲明存放兩張圖片各自關鍵點的向量
    Mat descriptors_1, descriptors_2;           // 兩張圖片特征點的描述子，形式為向量
    Ptr<FeatureDetector> detector = ORB::create();    // 創建FeatureDetector類型的指針detector
    Ptr<DescriptorExtractor> descriptor = ORB::create();    // ORB特征描述子提取指針
    // Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );   // 匹配時計算描述子之間的距離使用漢明距離

    //-- 第一步:檢測 Oriented FAST 角點位置，使用detect方法，將其存入Keypoints變量中
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根據角點位置計算 BRIEF 描述子，使用compute方法，將其存入descriptors變量中
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

	/*
	* 繪制第一幅圖像的ORB特征點，注意這里要把圖片的名稱全改為英文才能正常顯示。
	*/
    Mat outimg1;
    drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    imshow("ORB features.jpg",outimg1);

    //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
	/*
	* Brute-Force(BF)匹配器，取第一個集合里一個特征的描述子並與第二個集合里所有特征的描述子計算距離，返回最近的特征點。
	  其有兩個方法：BFMatcher.match()和BFMatcher.knnMatch()，前者返回最匹配的特征點(僅一個)，后者返回k個最匹配的特征點，k由用戶指定。
	*/
    vector<DMatch> matches;   // DMatch類型的向量。
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, matches );   // 使用match方法，獲取最匹配的兩個特征點(匹配的特征點可以有多對，均存放在matche中)

    //-- 第四步:匹配點對篩選
    double min_dist=10000, max_dist=0;

	
    //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離
	/*
	* descriptors的每行為一個特征點的描述子向量；圖像1中有多少個特征點，matches中存儲多少個匹配的圖像2的特征點相關類型
	*/
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = matches[i].distance;   // 得出每對匹配的描述子間的距離
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }
    
    // 僅供娛樂的寫法
    min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
    max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

	
	/*
	* 對於誤匹配情況采取的措施
	*/
    //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限.
    std::vector< DMatch > good_matches;
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            good_matches.push_back ( matches[i] );
        }
    }

    //-- 第五步:繪制匹配結果，數量分別為matches和good_matches
    Mat img_match;
    Mat img_goodmatch;
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
    imshow ( "all match features.jpg", img_match );  // 注意這里圖片名要改成英文
    imshow ( "good match features.jpg", img_goodmatch );
    waitKey(0);

    return 0;
}

相機姿態運動求解

基於特征點法的視覺里程計

• 視覺SLAM主要分為視覺前端和優化后端，前端即視覺里程計(VO)，其作用是根據相鄰圖像的信息，估計相機運動，提供初始值給后端。
• 根據是否需要提取特征，VO分為特征點法和直接法。

• 特征點法的缺點：1. 關鍵點的提取與描述子的計算非常耗時；2.容易忽略除特征點以外的所有信息；3.相機有時候會運動到特征缺失的地方，往往這些地方沒有明顯的紋理信息。

ICP問題

用於3D-3D的位姿估計，即兩張圖像為3D圖像(如RGB-D相機獲取的圖像)，其中的3D特征點集合已經提取和匹配好。

• ICP即為迭代最近點，僅考慮兩組3D點之間的變換，此時和相機無關。特征點方法正好為我們提供了兩張圖像之間良好的匹配關系，ICP可以據此進行運動估計。

• 利用線性代數方法的求解(SVD)：由兩張圖片，得到兩組匹配好的3D特征點集合，這兩組一一對應的點之間具有歐氏變換關系，即集合1中的點能由集合2中的點通過變換矩陣得到，即有R和T表示兩個點之間的關系。

  匹配好的特征點對會存在一定的誤差，定義誤差項，由誤差項構建最小二乘問題，即表示誤差平方和，求一個最優化問題，這里是求使誤差平方和達到極小的R，t。

  【理解：因為ICP問題求解的目的是得出相機的位姿估計，可以理解為當拍攝到第二幀圖像時相機的位置與拍攝第一幀圖像時相機原位置的變換關系，而變換關系可以用歐氏矩陣R和t描述，而由匹配的特征點對可以描述多個相機位姿，這里就涉及一個最優化問題，即構建最小二乘，求解使相機位姿相對最精確的R和t。】

  [注：但是具體的求解數學公式沒看懂。]

• 利用非線性優化方法的求解：以迭代的方式找最優值。[同樣數學式沒看懂。]

ICP法相機姿態估計代碼分析

源代碼提供了SVD和非線性優化兩種方法。

添加了一點關鍵注釋。

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 );   // 獲得兩幅圖像中匹配好的特征點信息

// 像素坐標轉相機歸一化坐標
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

void bundleAdjustment(
    const vector<Point3f>& points_3d,
    const vector<Point3f>& points_2d,
    Mat& R, Mat& t
);

// g2o edge
/*
* g2o是一個圖優化庫，圖優化的本質仍然是非線性優化，而用圖的方式表示，可以使問題可視化。
*/
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<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;

    // 建立3D點
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度圖為16位無符號數，單通道圖像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度圖為16位無符號數，單通道圖像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );   // 理解這里的K為相機內參
    vector<Point3f> pts1, pts2;

    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );   // 通過相機內參將兩幅圖像特征點的像素坐標轉化為相機坐標
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /5000.0;   // 深度值尺度縮放
        float dd2 = float ( d2 ) /5000.0;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );   // 得到特征點的3D坐標
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );   // 求得最優的R和t
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment( pts1, pts2, R, t );    // 非線性優化方法求解ICP問題

    // verify p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t
            <<endl;
        cout<<endl;
    }
}

還有feature_extraction.cpp中的特征點提取、匹配和跟蹤的代碼，這個上邊已經分析過了。

基於光流法的視覺里程計

繪制了光流法的原理圖