SLAM：理論與實踐
阿新 • • 發佈：2021-01-12
第一講，第二講：概述與預備知識
1，課程內容與預備知識
1.1，SLAM是什麼怎麼學：
1,SLAM：Simultaneous Localization and Mapping
2,參考書：MVG多檢視幾何，機器人學狀態估計State Estimation For Robotics（如何估計相機的運動）
3,視覺SLAM十四講
1.2，基礎知識：
數學：高等數學，線代，矩陣論，概率論
程式設計：等知識
2，SLAM是什麼
2.1，作用：
1,定位：提取點，來確定相機的位置，運動軌跡。
2,建圖：根據單目相機提取的點，來構建稀疏-半稠密的重建，用rgb-d相機可進行三維稠密的重建
2.2，應用：
1,手持裝置定位
2,自動駕駛定位
3,AR增強現實
2.3，感測器：
內質：感受機器人本體的資訊，IMU，鐳射，相機，編碼器
外質：安裝在環境中，二維碼，GPS，導軌（受環境的限制）
2.4，相機：
分類：
單目相機Monocular：
雙目相機Stereo：
深度相機RGB-D：
其他，全景，魚眼相機
相機的特點：二維投影形式記錄了三維世界的資訊，丟失了距離的緯度
3，視覺SLAM數學表述與框架
3.1，框架
視覺里程計：前端，連續的相鄰影象間進行移動軌跡的估計
後端：更長時間內，由於誤差的累計，會出現偏移，是估計，優化問題，符合長時間領域的觀測。
迴環檢測loop closing：由於誤差的累積，可能到同一位置時，路徑有差別，要檢測出軌跡回到某個地方。
建圖：
方法：特徵點法，直接法
後端：
3.2,SLAM問題的數學描述
4，實踐：Linux下的C++程式設計基礎
5，作業
1,熟悉linux
1.1 如何在Ubuntu 中安裝軟體（命令⾏界⾯）？它們通常被安裝在什麼地⽅？
1）apt-get 方式的安裝；
普通安裝：sudo apt-get install XXX （包名）
修復安裝：sudo apt-get -f install XXX
重新安裝：sudo apt-get -f reinstall XXX
（例如安裝g++編譯環境，可在終端中輸入命令：sudo apt-get install g++，附加說明可
通過g++ -v命令檢視g++版本）
2）dpkg方式安裝
sudo dpkg -i package_name.deb
安裝後軟體預設位置 /usr/share
1.2 linux 的環境變數是什麼？我如何定義新的環境變數？
環境變數：環境變數是在作業系統中一個具有特定名字的物件，它包含了一個或多個應用程式將使用到的資訊；它分為永久生效和shell臨時生效兩種。
1)對所有使用者生效的永久性變數（系統級）:
這類變數對系統內的所有使用者都生效，所有使用者都可以使用這類變數。作用範圍是整個系統。
設定方式： 用vim 或者gedit 開啟/etc/profile 檔案，然後新增我們想要的環境變數,用export指令新增環境變數。這個檔案只有在root（超級使用者）下才能修改。我們可以在etc目錄下使用ls -l檢視這個檔案的使用者及許可權。以gedit為例：
$ gedit /etc/profile
$ export 新增環境變數
注：新增完成後新的環境變數不會立即生效，需要source /etc/profile 該檔案才會生效。否則只能在下次重進此使用者時才能生效。記住要在終端source一下。。。
2)對當前使用者生效的永久性變數（使用者級）:
只針對當前使用者，和上面的一樣，只不過不需要在etc 下面進行新增，直接在.bash_profile檔案最下面用export新增就好了。
這裡 .bashrc和.bash_profile原則上來說設定此類環境變數時在這兩個檔案任意一個裡面新增都是可以的。
~/.bash_profile是互動式login方式進入bash shell執行。
~/ .bashrc是互動式non-login方式進入bash shell執行。
二者設定大致相同。
.bash_profile檔案只會在使用者登入的時候讀取一次；而.bashrc在每次開啟終端進行一次新的會話時都會讀取。
3)臨時有效的環境變數（只對當前shell有效）:
此類環境變數只對當前的shell有效。當我們退出登入或者關閉終端再重新開啟時，這個環境變數就會消失，是臨時的，可以直接使用export指令新增。
$ export 新增環境變數
`1.3 linux 根⽬錄下⾯的⽬錄結構是什麼樣的？⾄少說出3 個⽬錄的⽤途。
根目錄一般結構如下：（以本人筆記本目前ubuntu16.04系統為例，截圖如下）
1）/bin 使用者二進位制檔案
包含二進位制可執行檔案，系統所有使用者可執行檔案都在這個資料夾裡，例如：ls，cp，ping等。
2）/sbin 系統二進位制檔案
包含二進位制可執行檔案，但只能由系統管理員執行，對系統進行維護。
3）/etc 配置檔案
包含所有程式配置檔案，也包含了用於啟動/停止單個程式的啟動和關閉shell指令碼。
4）/dev 裝置檔案
包含終端所有裝置，USB或連線到系統的任何裝置。例如：/dev/tty1、/dev/usbmon0
5）/proc 程序資訊
包含系統程序的相關資訊。
這是一個虛擬的檔案系統，包含有關正在執行的程序的資訊。例如：/proc/{pid}目錄中包含的與特定pid相關的資訊。
6）/var 變數檔案
可以找到內容可能增長的檔案。這包括 - 系統日誌檔案（/var/log）;包和資料庫檔案（/var/lib）;電子郵件（/var/mail）;列印佇列（/var/spool）;鎖檔案（/var/lock）;多次重新啟動需要的臨時檔案（/var/tmp）;
7）/tmp 臨時檔案
包含系統和使用者建立的臨時檔案。當系統重新啟動時，這個目錄下的檔案都將被刪除。
8）/usr 使用者程式
包含二進位制檔案、庫檔案、文件和二級程式的原始碼。
/usr/bin 中包含使用者程式的二進位制檔案。如果你在/bin 中找不到使用者二進位制檔案，到/usr/bin目錄看看。例如：at、awk、cc、less、scp。
/usr/sbin 中包含系統管理員的二進位制檔案。如果你在/sbin 中找不到系統二進位制檔案，
到/usr/sbin目錄看看。例如：atd、cron、sshd、useradd、userdel。
/usr/lib中包含了/usr/bin和/usr/sbin用到的庫。
/usr/local 中包含了從源安裝的使用者程式。例如，當你從源安裝Apache，它會在/usr/local/apache2中。
9）/home HOME目錄
所有使用者用來存檔他們的個人檔案。
10）/boot 引導載入程式檔案
包含引導載入程式相關的檔案。核心的initrd、vmlinux、grub檔案位於/boot下。
11）/lib 系統庫
包含支援位於/bin和/sbin下的二進位制檔案的庫檔案，庫檔名為 ld或lib.so.*
12）/opt 可選的附加應用程式
opt代表opitional；包含從個別廠商的附加應用程式。附加應用程式應該安裝在/opt/或者/opt/的子目錄下。
13）/mnt 掛載目錄
臨時安裝目錄，系統管理員可以掛載檔案系統。
14）/media 可移動媒體裝置
用於掛載可移動裝置的臨時目錄。
舉例來說，掛載CD-ROM的/media/cdrom，掛載軟盤驅動器的/media/floppy;
15）/srv 服務資料
srv代表服務。包含伺服器特定服務相關的資料。例如，/srv/cvs包含cvs相關的資料。
1.4 假設我要給a.sh 加上可執⾏許可權，該輸⼊什麼命令？
chmod 777 +檔名： 將檔案設定成對擁有者、組成員、其他人可讀、可寫、可執行。
chmod a+x +檔名：將檔案在原來的配置上增加可執行許可權。
1.5假設我要將a.sh ⽂件的所有者改成xiang:xiang，該輸⼊什麼命令？
chown xiang：xiang a.sh
第三講：三維空間的剛體運動
第四講：李群與李代數
第五講：第六講：相機模型與非線性優化
第七講：特徵點法視覺里程計
章節總括：代數的方式去求解，優化的方式去求解，有了2D的資訊能夠推出3D的資訊，深度的資訊，三角化
1，特徵點提取與匹配
1，由於是RGBD影象，2d-2d 2d-3d 3d-3d的程式碼都可以用。
2，ORB是一個類
ORB特徵點提取，匹配程式碼：feature_extraction.cpp程式碼
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> //features2d模組支援很多很多的特徵，包括orb。 4 #include <opencv2/highgui/highgui.hpp> 5 6 using namespace std; 7 using namespace cv; 8 9 int main ( int argc, char** argv ) 10 { 11 if ( argc != 3 ) 12 { 13 cout<<"usage: feature_extraction img1 img2"<<endl; 14 return 1; 15 } 16 //-- 讀取影象 17 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 18 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 19 20 //-- 初始化 21 std::vector<KeyPoint> keypoints_1, keypoints_2; 22 Mat descriptors_1, descriptors_2; 23 Ptr<FeatureDetector> detector = ORB::create(); 24 Ptr<DescriptorExtractor> descriptor = ORB::create(); 25 // Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name); 26 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name); 27 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 28 29 //-- 第一步:檢測 Oriented FAST 角點位置 30 detector->detect ( img_1,keypoints_1 ); 31 detector->detect ( img_2,keypoints_2 ); 32 33 //-- 第二步:根據角點位置計算 BRIEF 描述子 34 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 35 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 36 37 Mat outimg1; 38 drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); 39 imshow("ORB特徵點",outimg1); 40 41 //-- 第三步:對兩幅影象中的BRIEF描述子進行匹配，使用 Hamming 距離 42 vector<DMatch> matches; 43 //BFMatcher matcher ( NORM_HAMMING ); 44 matcher->match ( descriptors_1, descriptors_2, matches ); 45 46 //-- 第四步:匹配點對篩選 47 double min_dist=10000, max_dist=0; 48 49 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 50 for ( int i = 0; i < descriptors_1.rows; i++ ) 51 { 52 double dist = matches[i].distance; 53 if ( dist < min_dist ) min_dist = dist; 54 if ( dist > max_dist ) max_dist = dist; 55 } 56 57 // 僅供娛樂的寫法 58 min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; 59 max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance; 60 61 printf ( "-- Max dist : %f \n", max_dist ); 62 printf ( "-- Min dist : %f \n", min_dist ); 63 64 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設定一個經驗值30作為下限. 65 std::vector< DMatch > good_matches; 66 for ( int i = 0; i < descriptors_1.rows; i++ ) 67 { 68 if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ) 69 { 70 good_matches.push_back ( matches[i] ); 71 } 72 } 73 74 //-- 第五步:繪製匹配結果 75 Mat img_match; 76 Mat img_goodmatch; 77 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match ); 78 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch ); 79 imshow ( "所有匹配點對", img_match ); 80 imshow ( "優化後匹配點對", img_goodmatch ); 81 waitKey(0); 82 83 return 0; 84 }
2，2D-2D 對極幾何
從單應矩陣恢復R，t；
求得R，t利用三角化計算特徵點的3D位置，即深度
給E算R 和 t
2D-2D對極幾何程式碼：pose_estimation_2d2d.cpp
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 4 #include <opencv2/highgui/highgui.hpp> 5 #include <opencv2/calib3d/calib3d.hpp> 6 // #include "extra.h" // use this if in OpenCV2 7 using namespace std; 8 using namespace cv; 9 10 /**************************************************** 11 * 本程式演示瞭如何使用2D-2D的特徵匹配估計相機運動 12 * **************************************************/ 13 14 void find_feature_matches ( 15 const Mat& img_1, const Mat& img_2, 16 std::vector<KeyPoint>& keypoints_1, 17 std::vector<KeyPoint>& keypoints_2, 18 std::vector< DMatch >& matches ); 19 20 void pose_estimation_2d2d ( 21 std::vector<KeyPoint> keypoints_1, 22 std::vector<KeyPoint> keypoints_2, 23 std::vector< DMatch > matches, 24 Mat& R, Mat& t ); 25 26 // 畫素座標轉相機歸一化座標 27 Point2d pixel2cam ( const Point2d& p, const Mat& K ); 28 29 int main ( int argc, char** argv ) 30 { 31 if ( argc != 3 ) 32 { 33 cout<<"usage: pose_estimation_2d2d img1 img2"<<endl; 34 return 1; 35 } 36 //-- 讀取影象 37 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 38 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 39 40 vector<KeyPoint> keypoints_1, keypoints_2; 41 vector<DMatch> matches; 42 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 43 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 44 45 //-- 估計兩張影象間運動 46 Mat R,t; 47 pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t ); 48 49 //-- 驗證E=t^R*scale 50 Mat t_x = ( Mat_<double> ( 3,3 ) << 51 0, -t.at<double> ( 2,0 ), t.at<double> ( 1,0 ), 52 t.at<double> ( 2,0 ), 0, -t.at<double> ( 0,0 ), 53 -t.at<double> ( 1.0 ), t.at<double> ( 0,0 ), 0 ); 54 55 cout<<"t^R="<<endl<<t_x*R<<endl; 56 57 //-- 驗證對極約束 58 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 59 for ( DMatch m: matches ) 60 { 61 Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K ); 62 Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 ); 63 Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K ); 64 Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 ); 65 Mat d = y2.t() * t_x * R * y1; 66 cout << "epipolar constraint = " << d << endl; 67 } 68 return 0; 69 } 70 71 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 72 std::vector<KeyPoint>& keypoints_1, 73 std::vector<KeyPoint>& keypoints_2, 74 std::vector< DMatch >& matches ) 75 { 76 //-- 初始化 77 Mat descriptors_1, descriptors_2; 78 // used in OpenCV3 79 Ptr<FeatureDetector> detector = ORB::create(); 80 Ptr<DescriptorExtractor> descriptor = ORB::create(); 81 // use this if you are in OpenCV2 82 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 83 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 84 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 85 //-- 第一步:檢測 Oriented FAST 角點位置 86 detector->detect ( img_1,keypoints_1 ); 87 detector->detect ( img_2,keypoints_2 ); 88 89 //-- 第二步:根據角點位置計算 BRIEF 描述子 90 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 91 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 92 93 //-- 第三步:對兩幅影象中的BRIEF描述子進行匹配，使用 Hamming 距離 94 vector<DMatch> match; 95 //BFMatcher matcher ( NORM_HAMMING ); 96 matcher->match ( descriptors_1, descriptors_2, match ); 97 98 //-- 第四步:匹配點對篩選 99 double min_dist=10000, max_dist=0; 100 101 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 102 for ( int i = 0; i < descriptors_1.rows; i++ ) 103 { 104 double dist = match[i].distance; 105 if ( dist < min_dist ) min_dist = dist; 106 if ( dist > max_dist ) max_dist = dist; 107 } 108 109 printf ( "-- Max dist : %f \n", max_dist ); 110 printf ( "-- Min dist : %f \n", min_dist ); 111 112 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設定一個經驗值30作為下限. 113 for ( int i = 0; i < descriptors_1.rows; i++ ) 114 { 115 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 116 { 117 matches.push_back ( match[i] ); 118 } 119 } 120 } 121 122 123 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 124 { 125 return Point2d 126 ( 127 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 128 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 129 ); 130 } 131 132 133 void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1, 134 std::vector<KeyPoint> keypoints_2, 135 std::vector< DMatch > matches, 136 Mat& R, Mat& t ) 137 { 138 // 相機內參,TUM Freiburg2 139 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 140 141 //-- 把匹配點轉換為vector<Point2f>的形式 142 vector<Point2f> points1; 143 vector<Point2f> points2; 144 145 for ( int i = 0; i < ( int ) matches.size(); i++ ) 146 { 147 points1.push_back ( keypoints_1[matches[i].queryIdx].pt ); 148 points2.push_back ( keypoints_2[matches[i].trainIdx].pt ); 149 } 150 151 //-- 計算基礎矩陣 152 Mat fundamental_matrix; 153 fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); 154 cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl; 155 156 //-- 計算本質矩陣 157 Point2d principal_point ( 325.1, 249.7 ); //相機光心, TUM dataset標定值 158 double focal_length = 521; //相機焦距, TUM dataset標定值 159 Mat essential_matrix; 160 essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point ); 161 cout<<"essential_matrix is "<<endl<< essential_matrix<<endl; 162 163 //-- 計算單應矩陣 164 Mat homography_matrix; 165 homography_matrix = findHomography ( points1, points2, RANSAC, 3 ); 166 cout<<"homography_matrix is "<<endl<<homography_matrix<<endl; 167 168 //-- 從本質矩陣中恢復旋轉和平移資訊. 169 recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point ); 170 cout<<"R is "<<endl<<R<<endl; 171 cout<<"t is "<<endl<<t<<endl; 172 173 }
3，3D-2D PnP
在雙目或者Rgbd，從3D到2D位姿的估計，已知3D點的空間位置，和相機上的投影點，求相機的旋轉和平移（外參）
代數的解法：
1,DLT：直接線性變換
非代數的解法：非線性優化的方式
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 4 #include <opencv2/highgui/highgui.hpp> 5 #include <opencv2/calib3d/calib3d.hpp> 6 #include <Eigen/Core> 7 #include <Eigen/Geometry> 8 #include <g2o/core/base_vertex.h> 9 #include <g2o/core/base_unary_edge.h> 10 #include <g2o/core/block_solver.h> 11 #include <g2o/core/optimization_algorithm_levenberg.h> 12 #include <g2o/solvers/csparse/linear_solver_csparse.h> 13 #include <g2o/types/sba/types_six_dof_expmap.h> 14 #include <chrono> 15 16 using namespace std; 17 using namespace cv; 18 19 void find_feature_matches ( 20 const Mat& img_1, const Mat& img_2, 21 std::vector<KeyPoint>& keypoints_1, 22 std::vector<KeyPoint>& keypoints_2, 23 std::vector< DMatch >& matches ); 24 25 // 畫素座標轉相機歸一化座標 26 Point2d pixel2cam ( const Point2d& p, const Mat& K ); 27 28 void bundleAdjustment ( 29 const vector<Point3f> points_3d, 30 const vector<Point2f> points_2d, 31 const Mat& K, 32 Mat& R, Mat& t 33 ); 34 35 int main ( int argc, char** argv ) 36 { 37 if ( argc != 5 ) 38 { 39 cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl; 40 return 1; 41 } 42 //-- 讀取影象 43 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 44 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 45 46 vector<KeyPoint> keypoints_1, keypoints_2; 47 vector<DMatch> matches; 48 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 49 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 50 51 // 建立3D點 52 Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道影象 53 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 54 vector<Point3f> pts_3d; 55 vector<Point2f> pts_2d; 56 for ( DMatch m:matches ) 57 { 58 ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; 59 if ( d == 0 ) // bad depth 60 continue; 61 float dd = d/5000.0; 62 Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); 63 pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) ); 64 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); 65 } 66 67 cout<<"3d-2d pairs: "<<pts_3d.size() <<endl; 68 69 Mat r, t; 70 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 呼叫OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法 71 Mat R; 72 cv::Rodrigues ( r, R ); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣 73 74 cout<<"R="<<endl<<R<<endl; 75 cout<<"t="<<endl<<t<<endl; 76 77 cout<<"calling bundle adjustment"<<endl; 78 79 bundleAdjustment ( pts_3d, pts_2d, K, R, t ); 80 } 81 82 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 83 std::vector<KeyPoint>& keypoints_1, 84 std::vector<KeyPoint>& keypoints_2, 85 std::vector< DMatch >& matches ) 86 { 87 //-- 初始化 88 Mat descriptors_1, descriptors_2; 89 // used in OpenCV3 90 Ptr<FeatureDetector> detector = ORB::create(); 91 Ptr<DescriptorExtractor> descriptor = ORB::create(); 92 // use this if you are in OpenCV2 93 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 94 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 95 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 96 //-- 第一步:檢測 Oriented FAST 角點位置 97 detector->detect ( img_1,keypoints_1 ); 98 detector->detect ( img_2,keypoints_2 ); 99 100 //-- 第二步:根據角點位置計算 BRIEF 描述子 101 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 102 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 103 104 //-- 第三步:對兩幅影象中的BRIEF描述子進行匹配，使用 Hamming 距離 105 vector<DMatch> match; 106 // BFMatcher matcher ( NORM_HAMMING ); 107 matcher->match ( descriptors_1, descriptors_2, match ); 108 109 //-- 第四步:匹配點對篩選 110 double min_dist=10000, max_dist=0; 111 112 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 113 for ( int i = 0; i < descriptors_1.rows; i++ ) 114 { 115 double dist = match[i].distance; 116 if ( dist < min_dist ) min_dist = dist; 117 if ( dist > max_dist ) max_dist = dist; 118 } 119 120 printf ( "-- Max dist : %f \n", max_dist ); 121 printf ( "-- Min dist : %f \n", min_dist ); 122 123 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設定一個經驗值30作為下限. 124 for ( int i = 0; i < descriptors_1.rows; i++ ) 125 { 126 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 127 { 128 matches.push_back ( match[i] ); 129 } 130 } 131 } 132 133 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 134 { 135 return Point2d 136 ( 137 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 138 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 139 ); 140 } 141 142 void bundleAdjustment ( 143 const vector< Point3f > points_3d, 144 const vector< Point2f > points_2d, 145 const Mat& K, 146 Mat& R, Mat& t ) 147 { 148 // 初始化g2o 149 typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 維度為 6, landmark 維度為 3 150 Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 線性方程求解器 151 Block* solver_ptr = new Block ( linearSolver ); // 矩陣塊求解器 152 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); 153 g2o::SparseOptimizer optimizer; 154 optimizer.setAlgorithm ( solver ); 155 156 // vertex 157 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose 158 Eigen::Matrix3d R_mat; 159 R_mat << 160 R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), 161 R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), 162 R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 ); 163 pose->setId ( 0 ); 164 pose->setEstimate ( g2o::SE3Quat ( 165 R_mat, 166 Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) ) 167 ) ); 168 optimizer.addVertex ( pose ); 169 170 int index = 1; 171 for ( const Point3f p:points_3d ) // landmarks 172 { 173 g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ(); 174 point->setId ( index++ ); 175 point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) ); 176 point->setMarginalized ( true ); // g2o 中必須設定 marg 參見第十講內容 177 optimizer.addVertex ( point ); 178 } 179 180 // parameter: camera intrinsics 181 g2o::CameraParameters* camera = new g2o::CameraParameters ( 182 K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0 183 ); 184 camera->setId ( 0 ); 185 optimizer.addParameter ( camera ); 186 187 // edges 188 index = 1; 189 for ( const Point2f p:points_2d ) 190 { 191 g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); 192 edge->setId ( index ); 193 edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) ); 194 edge->setVertex ( 1, pose ); 195 edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) ); 196 edge->setParameterId ( 0,0 ); 197 edge->setInformation ( Eigen::Matrix2d::Identity() ); 198 optimizer.addEdge ( edge ); 199 index++; 200 } 201 202 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 203 optimizer.setVerbose ( true ); 204 optimizer.initializeOptimization(); 205 optimizer.optimize ( 100 ); 206 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 207 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 ); 208 cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl; 209 210 cout<<endl<<"after optimization:"<<endl; 211 cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl; 212 }
4，3D-3D ICP
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 4 #include <opencv2/highgui/highgui.hpp> 5 #include <opencv2/calib3d/calib3d.hpp> 6 #include <Eigen/Core> 7 #include <Eigen/Geometry> 8 #include <Eigen/SVD> 9 #include <g2o/core/base_vertex.h> 10 #include <g2o/core/base_unary_edge.h> 11 #include <g2o/core/block_solver.h> 12 #include <g2o/core/optimization_algorithm_gauss_newton.h> 13 #include <g2o/solvers/eigen/linear_solver_eigen.h> 14 #include <g2o/types/sba/types_six_dof_expmap.h> 15 #include <chrono> 16 17 using namespace std; 18 using namespace cv; 19 20 void find_feature_matches ( 21 const Mat& img_1, const Mat& img_2, 22 std::vector<KeyPoint>& keypoints_1, 23 std::vector<KeyPoint>& keypoints_2, 24 std::vector< DMatch >& matches ); 25 26 // 畫素座標轉相機歸一化座標 27 Point2d pixel2cam ( const Point2d& p, const Mat& K ); 28 29 void pose_estimation_3d3d ( 30 const vector<Point3f>& pts1, 31 const vector<Point3f>& pts2, 32 Mat& R, Mat& t 33 ); 34 35 void bundleAdjustment( 36 const vector<Point3f>& points_3d, 37 const vector<Point3f>& points_2d, 38 Mat& R, Mat& t 39 ); 40 41 // g2o edge 42 class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap> 43 { 44 public: 45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {} 47 48 virtual void computeError() 49 { 50 const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] ); 51 // measurement is p, point is p' 52 _error = _measurement - pose->estimate().map( _point ); 53 } 54 55 virtual void linearizeOplus() 56 { 57 g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]); 58 g2o::SE3Quat T(pose->estimate()); 59 Eigen::Vector3d xyz_trans = T.map(_point); 60 double x = xyz_trans[0]; 61 double y = xyz_trans[1]; 62 double z = xyz_trans[2]; 63 64 _jacobianOplusXi(0,0) = 0; 65 _jacobianOplusXi(0,1) = -z; 66 _jacobianOplusXi(0,2) = y; 67 _jacobianOplusXi(0,3) = -1; 68 _jacobianOplusXi(0,4) = 0; 69 _jacobianOplusXi(0,5) = 0; 70 71 _jacobianOplusXi(1,0) = z; 72 _jacobianOplusXi(1,1) = 0; 73 _jacobianOplusXi(1,2) = -x; 74 _jacobianOplusXi(1,3) = 0; 75 _jacobianOplusXi(1,4) = -1; 76 _jacobianOplusXi(1,5) = 0; 77 78 _jacobianOplusXi(2,0) = -y; 79 _jacobianOplusXi(2,1) = x; 80 _jacobianOplusXi(2,2) = 0; 81 _jacobianOplusXi(2,3) = 0; 82 _jacobianOplusXi(2,4) = 0; 83 _jacobianOplusXi(2,5) = -1; 84 } 85 86 bool read ( istream& in ) {} 87 bool write ( ostream& out ) const {} 88 protected: 89 Eigen::Vector3d _point; 90 }; 91 92 int main ( int argc, char** argv ) 93 { 94 if ( argc != 5 ) 95 { 96 cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl; 97 return 1; 98 } 99 //-- 讀取影象 100 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 101 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 102 103 vector<KeyPoint> keypoints_1, keypoints_2; 104 vector<DMatch> matches; 105 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 106 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 107 108 // 建立3D點 109 Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道影象 110 Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道影象 111 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 112 vector<Point3f> pts1, pts2; 113 114 for ( DMatch m:matches ) 115 { 116 ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; 117 ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ]; 118 if ( d1==0 || d2==0 ) // bad depth 119 continue; 120 Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); 121 Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K ); 122 float dd1 = float ( d1 ) /5000.0; 123 float dd2 = float ( d2 ) /5000.0; 124 pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) ); 125 pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) ); 126 } 127 128 cout<<"3d-3d pairs: "<<pts1.size() <<endl; 129 Mat R, t; 130 pose_estimation_3d3d ( pts1, pts2, R, t ); 131 cout<<"ICP via SVD results: "<<endl; 132 cout<<"R = "<<R<<endl; 133 cout<<"t = "<<t<<endl; 134 cout<<"R_inv = "<<R.t() <<endl; 135 cout<<"t_inv = "<<-R.t() *t<<endl; 136 137 cout<<"calling bundle adjustment"<<endl; 138 139 bundleAdjustment( pts1, pts2, R, t ); 140 141 // verify p1 = R*p2 + t 142 for ( int i=0; i<5; i++ ) 143 { 144 cout<<"p1 = "<<pts1[i]<<endl; 145 cout<<"p2 = "<<pts2[i]<<endl; 146 cout<<"(R*p2+t) = "<< 147 R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t 148 <<endl; 149 cout<<endl; 150 } 151 } 152 153 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 154 std::vector<KeyPoint>& keypoints_1, 155 std::vector<KeyPoint>& keypoints_2, 156 std::vector< DMatch >& matches ) 157 { 158 //-- 初始化 159 Mat descriptors_1, descriptors_2; 160 // used in OpenCV3 161 Ptr<FeatureDetector> detector = ORB::create(); 162 Ptr<DescriptorExtractor> descriptor = ORB::create(); 163 // use this if you are in OpenCV2 164 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 165 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 166 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); 167 //-- 第一步:檢測 Oriented FAST 角點位置 168 detector->detect ( img_1,keypoints_1 ); 169 detector->detect ( img_2,keypoints_2 ); 170 171 //-- 第二步:根據角點位置計算 BRIEF 描述子 172 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 173 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 174 175 //-- 第三步:對兩幅影象中的BRIEF描述子進行匹配，使用 Hamming 距離 176 vector<DMatch> match; 177 // BFMatcher matcher ( NORM_HAMMING ); 178 matcher->match ( descriptors_1, descriptors_2, match ); 179 180 //-- 第四步:匹配點對篩選 181 double min_dist=10000, max_dist=0; 182 183 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 184 for ( int i = 0; i < descriptors_1.rows; i++ ) 185 { 186 double dist = match[i].distance; 187 if ( dist < min_dist ) min_dist = dist; 188 if ( dist > max_dist ) max_dist = dist; 189 } 190 191 printf ( "-- Max dist : %f \n", max_dist ); 192 printf ( "-- Min dist : %f \n", min_dist ); 193 194 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設定一個經驗值30作為下限. 195 for ( int i = 0; i < descriptors_1.rows; i++ ) 196 { 197 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 198 { 199 matches.push_back ( match[i] ); 200 } 201 } 202 } 203 204 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 205 { 206 return Point2d 207 ( 208 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 209 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 210 ); 211 } 212 213 void pose_estimation_3d3d ( 214 const vector<Point3f>& pts1, 215 const vector<Point3f>& pts2, 216 Mat& R, Mat& t 217 ) 218 { 219 Point3f p1, p2; // center of mass 220 int N = pts1.size(); 221 for ( int i=0; i<N; i++ ) 222 { 223 p1 += pts1[i]; 224 p2 += pts2[i]; 225 } 226 p1 = Point3f( Vec3f(p1) / N); 227 p2 = Point3f( Vec3f(p2) / N); 228 vector<Point3f> q1 ( N ), q2 ( N ); // remove the center 229 for ( int i=0; i<N; i++ ) 230 { 231 q1[i] = pts1[i] - p1; 232 q2[i] = pts2[i] - p2; 233 } 234 235 // compute q1*q2^T 236 Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); 237 for ( int i=0; i<N; i++ ) 238 { 239 W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose(); 240 } 241 cout<<"W="<<W<<endl; 242 243 // SVD on W 244 Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV ); 245 Eigen::Matrix3d U = svd.matrixU(); 246 Eigen::Matrix3d V = svd.matrixV(); 247 cout<<"U="<<U<<endl; 248 cout<<"V="<<V<<endl; 249 250 Eigen::Matrix3d R_ = U* ( V.transpose() ); 251 Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z ); 252 253 // convert to cv::Mat 254 R = ( Mat_<double> ( 3,3 ) << 255 R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ), 256 R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ), 257 R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 ) 258 ); 259 t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) ); 260 } 261 262 void bundleAdjustment ( 263 const vector< Point3f >& pts1, 264 const vector< Point3f >& pts2, 265 Mat& R, Mat& t ) 266 { 267 // 初始化g2o 268 typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose維度為 6, landmark 維度為 3 269 Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 線性方程求解器 270 271 Block* solver_ptr = new Block( linearSolver ); // 矩陣塊求解器 272 273 g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); 274 275 g2o::SparseOptimizer optimizer; 276 optimizer.setAlgorithm( solver ); 277 278 // vertex 279 g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose 280 pose->setId(0); 281 pose->setEstimate( g2o::SE3Quat( 282 Eigen::Matrix3d::Identity(), 283 Eigen::Vector3d( 0,0,0 ) 284 ) ); 285 optimizer.addVertex( pose ); 286 287 // edges 288 int index = 1; 289 vector<EdgeProjectXYZRGBDPoseOnly*> edges; 290 for ( size_t i=0; i<pts1.size(); i++ ) 291 { 292 EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly( 293 Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) ); 294 edge->setId( index ); 295 edge->setVertex( 0, dynamic_cast<g2o::VertexSE3Expmap*> (pose) ); 296 edge->setMeasurement( Eigen::Vector3d( 297 pts1[i].x, pts1[i].y, pts1[i].z) ); 298 edge->setInformation( Eigen::Matrix3d::Identity()*1e4 ); 299 optimizer.addEdge(edge); 300 index++; 301 edges.push_back(edge); 302 } 303 304 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 305 optimizer.setVerbose( true ); 306 optimizer.initializeOptimization(); 307 optimizer.optimize(10); 308 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 309 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1); 310 cout<<"optimization costs time: "<<time_used.count()<<" seconds."<<endl; 311 312 cout<<endl<<"after optimization:"<<endl; 313 cout<<"T="<<endl<<Eigen::Isometry3d( pose->estimate() ).matrix()<<endl; 314 315 }
5，三角化與深度估計
第八講：直接法視覺里程計
1,光流法（不需要特徵點）
估計的是畫素點
2,直接法 ,光流法稍加修改。
估計的是相機是怎麼運動的
第九講：實踐
第十講，第十一講：後端優化
第十三講：迴環檢測
第十四講：展望
#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, 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; }