一起做RGB-D SLAM(3)

第三講特徵提取與配準

　　師兄：同學們大家好，又到我們每週一次的“一起做RGB-D SLAM”時間啦！大家還記得上週我們講了什麼東西嗎？

　　小蘿蔔：等一下師兄！我們的博客什麼時候變成每週一更了？

　　師兄：這個，這不是因爲終於到暑假了，我可以專門搞學術了嘛。

　　小蘿蔔：胡說！前兩天我還看到你在超市開挖掘機來着！

　　師兄：這事你就別提了啊……

　　小蘿蔔：我還有照片爲證呢！

　　師兄：你能不能別提這個事了啊……我們趕緊開始講課吧。

　　小蘿蔔：師兄，那可是兒童專區啊，您可是個博士，自重啊！

上講回顧

　　在上一講中，我們介紹瞭如何把2D圖像座標轉換爲3D空間座標。然後，利用推導的數學公式，實現了把圖像轉化爲點雲的程序。在上一講的末尾，我們給出了一道作業題，希望讀者去把這兩件事做成一個函數庫，以供將來調用。不知道大家回去之後做了沒有呢？

　　小蘿蔔：讀者這麼勤奮肯定已經做好了啦！

　　師兄：嗯，所以呢，這一講裏面我們就要用到上面兩個函數了。在講解本講的內容之前，先介紹一下我們是如何封裝上節課代碼的。在代碼根目錄/include 下，我們新建了一個 slamBase.h 文件，存放上一講以及以後講到的各種函數：

　　include/slamBase.h:

 1 /*************************************************************************
 2     > File Name: rgbd-slam-tutorial-gx/part III/code/include/slamBase.h
 3     > Author: xiang gao
 4     > Mail: [email protected]
 5     > Created Time: 2015年07月18日 星期六 15時14分22秒
 6     > 說明：rgbd-slam教程所用到的基本函數（C風格）
 7  ************************************************************************/
 8 # pragma once
 9 
10 // 各種頭文件 
11 // C++標準庫
12 #include <fstream>
13 #include <vector>
14 using namespace std;
15 
16 // OpenCV
17 #include <opencv2/core/core.hpp>
18 #include <opencv2/highgui/highgui.hpp>
19 
20 //PCL
21 #include <pcl/io/pcd_io.h>
22 #include <pcl/point_types.h>
23 
24 // 類型定義
25 typedef pcl::PointXYZRGBA PointT;
26 typedef pcl::PointCloud<PointT> PointCloud;
27 
28 // 相機內參結構
29 struct CAMERA_INTRINSIC_PARAMETERS 
30 { 
31     double cx, cy, fx, fy, scale;
32 };
33 
34 // 函數接口
35 // image2PonitCloud 將rgb圖轉換爲點雲
36 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );
37 
38 // point2dTo3d 將單個點從圖像座標轉換爲空間座標
39 // input: 3維點Point3f (u,v,d)
40 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

　　可以看到，我們把相機參數封裝成了一個結構體，另外還聲明瞭 image2PointCloud 和 point2dTo3d 兩個函數。然後，在 src/ 目錄下新建 slamBase.cpp 文件：

　　src/slamBase.cpp

 1 /*************************************************************************
 2     > File Name: src/slamBase.cpp
 3     > Author: xiang gao
 4     > Mail: [email protected]
 5     > Implementation of slamBase.h
 6     > Created Time: 2015年07月18日 星期六 15時31分49秒
 7  ************************************************************************/
 8 
 9 #include "slamBase.h"
10 
11 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera )
12 {
13     PointCloud::Ptr cloud ( new PointCloud );
14 
15     for (int m = 0; m < depth.rows; m++)
16         for (int n=0; n < depth.cols; n++)
17         {
18             // 獲取深度圖中(m,n)處的值
19             ushort d = depth.ptr<ushort>(m)[n];
20             // d 可能沒有值，若如此，跳過此點
21             if (d == 0)
22                 continue;
23             // d 存在值，則向點雲增加一個點
24             PointT p;
25 
26             // 計算這個點的空間座標
27             p.z = double(d) / camera.scale;
28             p.x = (n - camera.cx) * p.z / camera.fx;
29             p.y = (m - camera.cy) * p.z / camera.fy;
30             
31             // 從rgb圖像中獲取它的顏色
32             // rgb是三通道的BGR格式圖，所以按下面的順序獲取顏色
33             p.b = rgb.ptr<uchar>(m)[n*3];
34             p.g = rgb.ptr<uchar>(m)[n*3+1];
35             p.r = rgb.ptr<uchar>(m)[n*3+2];
36 
37             // 把p加入到點雲中
38             cloud->points.push_back( p );
39         }
40     // 設置並保存點雲
41     cloud->height = 1;
42     cloud->width = cloud->points.size();
43     cloud->is_dense = false;
44 
45     return cloud;
46 }
47 
48 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
49 {
50     cv::Point3f p; // 3D 點
51     p.z = double( point.z ) / camera.scale;
52     p.x = ( point.x - camera.cx) * p.z / camera.fx;
53     p.y = ( point.y - camera.cy) * p.z / camera.fy;
54     return p;
55 }

　　最後，在 src/CMakeLists.txt 中加入以下幾行，將 slamBase.cpp 編譯成一個庫，供將來調用：

ADD_LIBRARY( slambase slamBase.cpp )
TARGET_LINK_LIBRARIES( slambase
    ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )

　　這兩句話是說，把slamBase.cpp編譯成 slamBase 庫，並把該庫裏調到的OpenCV和PCL的部分，和相應的庫鏈接起來。是不是感覺代碼更有條理了呢？今後，我們會在每一講裏介紹新的內容，並把實現的代碼封裝裏這個slamBase庫中。

圖像配準數學部分

　　SLAM是由“定位”（Localization)和“建圖”（Mapping)兩部分構成的。現在來看定位問題。要求解機器人的運動，首先要解決這樣一個問題：給定了兩個圖像，如何知道圖像的運動關係呢？

　　這個問題可以用基於特徵的方法（feature-based）或直接的方法（direct method）來解。雖說直接法已經有了一定的發展，但目前主流的方法還是基於特徵點的方式。在後者的方法中，首先你需要知道圖像裏的“特徵”，以及這些特徵的一一對應關係。

　　假設我們有兩個幀：F1和F2. 並且，我們獲得了兩組一一對應的特徵點：

P = {p 1, p 2, \dots, p N} \in F 1

Q = {q 1, q 2, \dots, q N} \in F 2

. 其中

p和

q都是

R3中的點。

　　我們的目的是求出一個旋轉矩陣R和位移矢量t，使得：

\forall i, p i = R q i + t

　　然而實際當中由於誤差的存在，等號基本是不可能的。所以我們通過最小化一個誤差來求解R,t:

min R, t \sum i = 1 N ∥ p i - (R q i + t) ∥ 2

　　這個問題可以用經典的ICP算法求解。其核心是奇異值分解(SVD)。我們將調用opencv中的函數求解此問題，所以在此就不細講ICP的具體步驟了。有興趣的讀者可以參閱1987年PAMI上的一篇文章，也是最原始的ICP方法：Least-squares fitting of two 3-D point sets。

　　那麼從這個數學問題上來講，我們的關鍵就是要獲取一組一一對應的空間點，這可以通過圖像的特徵匹配來完成。　　

圖像配準編程實現

　　本節中，我們要匹配兩對圖像，並且計算它們的位移關係。它們分別是rgb1,png, rgb2.png, depth1.png, depth2.png. 人眼可以直觀地看到第二個圖是向左轉動了一些。

　　照例，我們先給出完整的程序代碼。讀者可以先把代碼瀏覽一遍，我們再加以詳細的解釋。因爲本節的代碼比較長，我把它摺疊起來，否則排版就太長了。

detectFeatures.cpp

　　代碼的解釋：

　　第一部分： 18行至52行，特徵的檢測與描述子的計算。

　　要在一個圖像裏提取特徵，第一步是計算“關鍵點”，然後，針對這些關鍵點周圍的像素，計算其“描述子”。在OpenCV中，分別由cv::FeatureDetector和cv::DescriptorExtractor來計算。

1 cv::Ptr<cv::FeatureDetector> _detector = cv::FeatureDetector::create( "SIFT" );
2 cv::Ptr<cv::DescriptorExtractor> _descriptor = cv:: DescriptorExtractor::create( "SIFT" );

　　然後，使用 _detector->detect()函數提取關鍵點。值得一提的是，_detector和_descriptor的類型可以由字符串指定。如果你想構建FAST, SURF等特徵，只需改後面的字符串即可。

　　關鍵點是一種cv::KeyPoint的類型。你可以看它的API: http://docs.opencv.org/modules/features2d/doc/common_interfaces_of_feature_detectors.html?highlight=keypoint#KeyPoint 由於比較長我這裏就不貼了。可以看到，KeyPoint結構中帶有 Point2f pt 這個成員變量，指這個關鍵點的像素座標。此外，有的關鍵點還有半徑、角度等參數，畫在圖裏就會像一個個的圓一樣。

有了一組KeyPoint之後，就可以調用：

1 _descriptor->compute( image, keypoint, descriptor )

　　在 keypoint 上計算描述子。描述子是一個cv::Mat的矩陣結構，它的每一行代表一個對應於Keypoint的特徵向量。當兩個keypoint的描述子越相似，說明這兩個關鍵點也就越相似。我們正是通過這種相似性來檢測圖像之間的運動的。

　　第二部分：特徵匹配（53行至88行）

　　接下來，我們對上述的描述子進行匹配。在OpenCV中，你需要選擇一個匹配算法，例如粗暴式（bruteforce），近似最近鄰（Fast Library for Approximate Nearest Neighbour, FLANN）等等。這裏我們構建一個FLANN的匹配算法：

1 vector< cv::DMatch > matches;
2 cv::FlannBasedMatcher matcher;
3 matcher.match( desp1, desp2, matches );

　　匹配完成後，算法會返回一些 DMatch 結構。該結構含有以下幾個成員：

queryIdx 源特徵描述子的索引（也就是第一張圖像）。
trainIdx 目標特徵描述子的索引（第二個圖像）
distance 匹配距離，越大表示匹配越差。

　　有了匹配後，可以用drawMatch函數畫出匹配的結果：

　　小蘿蔔：這真是“亂花漸欲迷人眼，淺草才能沒馬蹄”啊！

　　師兄：你不是機器人麼，怎麼開始變得文藝起來了？

　　不管如何說，僅靠描述子的匹配似乎是太多了些，把許多不相似的東西也匹配起來了。(由於這兩個圖像只有水平旋轉，所以水平的匹配線纔是對的，其他的都是誤匹配）。因此，需要篩選一下這些匹配，例如，把distance太大的給去掉（源文件69到88行）。

　　篩選的準則是：去掉大於最小距離四倍的匹配。

　　小蘿蔔：爲什麼是四倍呢？

　　師兄：這只是個經驗數值啦，就像你平時都買一斤半的棗糕呀，爲什麼不買兩斤呢？

　　小蘿蔔：我喜歡喫棗糕！等這節講完我們去喫棗糕吧！

　　師兄：呃……總、總之，選出來的goodmatch大概是這樣的：

　　篩選之後，匹配就少了許多了，圖像看起來也比較乾淨。

　　第三部分求解PnP

　　求解PnP的核心是調用OpenCV裏的SolvePnPRansac函數。該函數的接口如下（來自OpenCV在線API）：

C++: void solvePnPRansac(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100, OutputArray inliers=noArray(), int flags=ITERATIVE )

Python: cv2.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, iterationsCount[, reprojectionError[, minInliersCount[, inliers[, flags]]]]]]]]) → rvec, tvec, inliers

Parameters:

Parameters:	objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. `vector<Point3f>` can be also passed here. imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. `vector<Point2f>` can be also passed here. cameraMatrix – Input camera matrix $A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}$ . distCoeffs – Input vector of distortion coefficients $(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]])$ of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. rvec – Output rotation vector (see `Rodrigues()` ) that, together with `tvec` , brings points from the model coordinate system to the camera coordinate system. tvec – Output translation vector. useExtrinsicGuess – If true (1), the function uses the provided `rvec` and `tvec` values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them. iterationsCount – Number of iterations. reprojectionError – Inlier threshold value used by the RANSAC procedure. The parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier. minInliersCount – Number of inliers. If the algorithm at some stage finds more inliers than `minInliersCount` , it finishes. inliers – Output vector that contains indices of inliers in `objectPoints` and `imagePoints` . flags – Method for solving a PnP problem (see `solvePnP()` ).

objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector<Point3f> can be also passed here.
imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. vector<Point2f> can be also passed here.
cameraMatrix – Input camera matrix $A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}$ .
distCoeffs – Input vector of distortion coefficients $(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6]])$ of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
rvec – Output rotation vector (see Rodrigues() ) that, together with tvec , brings points from the model coordinate system to the camera coordinate system.
tvec – Output translation vector.
useExtrinsicGuess – If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them.
iterationsCount – Number of iterations.
reprojectionError – Inlier threshold value used by the RANSAC procedure. The parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier.
minInliersCount – Number of inliers. If the algorithm at some stage finds more inliers than minInliersCount , it finishes.
inliers – Output vector that contains indices of inliers in objectPoints and imagePoints .
flags – Method for solving a PnP problem (see solvePnP() ).

　　可以看到，這個函數需要輸入一組匹配好的三維點: objectPoints 和一組二維圖像點: imagePoints. 返回的結果是旋轉向量 rvec 和平移向量tvec。其他的都是算法中的參數。因此，我們需要想辦法構建這兩組輸入點，它們實際上就是從goodmatches裏抽取來的。

　　由於我們已經提取了KeyPoint，知道了它們的像素座標。那麼，利用上一節提供的庫函數，就可以直接轉到三維點啦。代碼如下：（因爲很重要就又貼了一遍）

 1 // 計算圖像間的運動關係
 2     // 關鍵函數：cv::solvePnPRansac()
 3     // 爲調用此函數準備必要的參數
 4     
 5     // 第一個幀的三維點
 6     vector<cv::Point3f> pts_obj;
 7     // 第二個幀的圖像點
 8     vector< cv::Point2f > pts_img;
 9 
10     // 相機內參
11     CAMERA_INTRINSIC_PARAMETERS C;
12     C.cx = 325.5;
13     C.cy = 253.5;
14     C.fx = 518.0;
15     C.fy = 519.0;
16     C.scale = 1000.0;
17 
18     for (size_t i=0; i<goodMatches.size(); i++)
19     {
20         // query 是第一個, train 是第二個
21         cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
22         // 獲取d是要小心！x是向右的，y是向下的，所以y纔是行，x是列！
23         ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
24         if (d == 0)
25             continue;
26         pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );
27 
28         // 將(u,v,d)轉成(x,y,z)
29         cv::Point3f pt ( p.x, p.y, d );
30         cv::Point3f pd = point2dTo3d( pt, C );
31         pts_obj.push_back( pd );
32     }
33 
34     double camera_matrix_data[3][3] = {
35         {C.fx, 0, C.cx},
36         {0, C.fy, C.cy},
37         {0, 0, 1}
38     };
39 
40     // 構建相機矩陣
41     cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
42     cv::Mat rvec, tvec, inliers;
43     // 求解pnp
44     cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );

　　求解完成後，rvec和tvec就含有了位移和旋轉的信息，至此，我們已經成功地計算出兩個圖像的運動啦！

　　小蘿蔔：對了，那個Ransac和inlier是什麼呢？

　　師兄：儘管經過了篩選，我們提供的匹配裏還是存在誤匹配的情況。根據誤匹配計算運動是不靠譜的。這時該怎麼辦呢？OpenCV會利用一種“隨機採樣一致性”（Random Sample Consensus）的思路（見https://en.wikipedia.org/wiki/RANSAC）。意思即爲，在現有的匹配中隨機取一部分，估計其運動。因爲正確的匹配結果肯定是相似的，而誤匹配的結果肯定滿天亂飛。只要把收斂的結果取出來即可。

　　小蘿蔔：這個就叫做“幸福的家庭都是相似的，不幸的家庭各有各的不幸”吧。

　　師兄：你這樣理解也可以。ransac適用於數據噪聲比較大的場合。這對圖像的inlier大概是這樣的：

　　小蘿蔔：似乎仍有一些誤差的樣子呢。

　　師兄：是的，不過可以通過調節我們之前的篩選過程以及之後的ransac參數，得到更好的結果。

本節回顧

　　本節中，我們介紹瞭如何提取、匹配圖像的特徵，並通過這些匹配，使用ransac方法估計圖像的運動。下一節，我們將介紹如何使用剛得到的平移、旋轉向量來拼接點雲。至此，我們就完成了一個只有兩幀的迷你SLAM程序。

　　課後作業：

請把計算圖像運動信息封裝成一個函數，放進slamBase庫中。
由於我們的程序變複雜了，出現了一些內部的參數，如特徵點類型，篩選準則，ransac參數等等。如果我們把這些參數值定義在源代碼裏，那麼每修改一次，就要重新編譯一遍程序。這非常麻煩。所以，我們希望把參數定義在外部文件中，在程序剛開始時讀取此文件。這樣一來，只要更改此文件即可完成參數的調節，不必重新編譯程序了。因此，請你完成一個讀取參數的類，放進slamBase中。

　　小蘿蔔：這一講比前兩節難好多啊，師兄。

　　師兄：是啊，然而我也不得不講這些東西，不然講不清楚呢。

　　小蘿蔔：嗯，那我回去好好複習啦。大家也要加油！

一起做RGB-D SLAM(3)

第三講特徵提取與配準

上講回顧

圖像配準數學部分

圖像配準編程實現

本節回顧

未完待續

使用neovim打造go ide(支持代碼跳轉, 代碼補全, 實時語法檢查)

挑戰程序設計競賽 2.3章習題 poj 3046 Ant Counting

Shell/Python中的用戶名獲取

一起做RGB-D SLAM(2)

C++累積求和函數

一起做RGB-D SLAM(3)

DBoW3問題及解決方案

Ubuntu16安裝刪除libfreenect2

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

一起做RGB-D SLAM(3)

第三講 特徵提取與配準

上講回顧

圖像配準 數學部分

圖像配準 編程實現

本節回顧

未完待續

第三講特徵提取與配準

圖像配準數學部分

圖像配準編程實現