ch7 視覺里程計1
本章目標：
1.理解圖像特征點的意義，並掌握在單副圖像中提取出特征點及多副圖像中匹配特征點的方法
2.理解對極幾何的原理，利用對極幾何的約束，恢復出圖像之間的攝像機的三維運動
3.理解PNP問題，以及利用已知三維結構與圖像的對應關系求解攝像機的三維運動
4.理解ICP問題，以及利用點雲的匹配關系求解攝像機的三維運動
5.理解如何通過三角化獲得二維圖像上對應點的三維結構
本章目的：基於特征點法的vo，將介紹什么是特征點，如何提取和匹配特征點，以及如何根據配對的特征點估計相機運動和場景結構，從而實現一個基本的兩幀間視覺里程計。
特征點：角點、SIFT(尺度不變特征變換，Scale-Invariant Feature Transform)、SURF、、ORB（后三個是人工設計的特征點，具有更多的優點）
特征點的組成：
1.關鍵點：指特征點在圖像里的位置
2.描述子：通常是一個向量，按照某種人為設計的方式，描述了該關鍵點周圍像素的信息。相似的特征應該有相似的描述子（即當兩個特征點的描述子在向量空間上的距離相近，認為這兩個特征點是一樣的）
以ORB特征為代表介紹提取特征的整個過程：
ORB特征：OrientedFAST關鍵點+BRIEF關鍵子
提取ORB特征的步驟：
1.提取FAST角點：找出圖像中的“角點”，計算特征點的主方向，為后續BRIEF描述子增加了旋轉不變特性
FAST角點：主要檢測局部像素灰度變化明顯的地方
特點：速度快
缺點：1).FAST特征點數量很大且不確定，但是我們希望對圖像提取固定數量的特征
2).FAST角點不具有方向信息，並且存在尺度問題
解決方式：1).指定要提取的角點數量N，對原始FAST角點分別計算Harris響應值，然后選取前N個具有最大響應值的角點作為最終的角點集合
2).添加尺度和旋轉的描述
尺度不變性的實現：構建圖像金字塔，並在金字塔的每一層上檢測角點（金字塔：指對圖像進行不同層次的降采樣，以獲得不同分辨
率的圖像）
特征旋轉的實現：灰度質心法（質心：指以圖像塊灰度值作為權重的中心）
2.計算BRIEF描述子：對前一步提取出的特征點周圍圖像區域進行掃描
特點：使用隨機選點的比較，速度非常快，由於使用了二進制表達，存儲起來也十分方便，適用於實時的圖像匹配
在不同圖像之間進行特征匹配的方法：
1.暴力匹配：浮點類型的描述子，使用歐式距離度量
二進制類型的描述子（比如本例中的BRIEF描述子），使用漢明距離度量
缺點：當特征點數量很大時，暴力匹配法的運算量會變得很大
2.快速近似最近鄰（FLANN）：適合匹配特征點數量極多的情況
實踐部分：
1.OpenCV的圖像特征提取、計算和匹配的過程：演示如何提取ORB特征並進行匹配
代碼：
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/features2d/features2d.hpp> 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 //讀取圖像 18 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR); 19 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR); 20 21 //初始化 22 vector<KeyPoint> keypoints_1,keypoints_2;//關鍵點,指特征點在圖像里的位置 23 Mat descriptors_1,descriptors_2;//描述子,通常是向量 24 Ptr<ORB> orb=ORB::create(500,1.2f,8,31,0,2,ORB::HARRIS_SCORE,31,20); 25 26 //第一步：檢測OrientFAST角點位置 27 orb->detect(img_1,keypoints_1); 28 orb->detect(img_2,keypoints_2); 29 30 //第2步：根據角點位置計算BRIEF描述子 31 orb->compute(img_1,keypoints_1,descriptors_1); 32 orb->compute(img_2,keypoints_2,descriptors_2); 33 34 Mat outimg1; 35 drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT); 36 imshow("1.png的ORB特征點",outimg1); 37 Mat outimg2; 38 drawKeypoints(img_2,keypoints_2,outimg2,Scalar::all(-1),DrawMatchesFlags::DEFAULT); 39 imshow("2.png的ORB特征點",outimg2); 40 41 //第3步：對兩幅圖像中的BRIEF描述子進行匹配，使用Hamming距離 42 vector<DMatch> matches; 43 //特征匹配的方法：暴力匹配 44 BFMatcher matcher(NORM_HAMMING); 45 matcher.match(descriptors_1,descriptors_2,matches); 46 // for(auto it=matches.begin();it!=matches.end();++it) 47 // { 48 // cout<<*it<<" "; 49 // } 50 // cout<<endl; 51 52 //第4步：匹配點對篩選 53 distance是min_dist 54 55 double min_dist=10000,max_dist=0; 56 57 //找出所有匹配之間的最小距離和最大距離，即最相似的和最不相似的和最不相似的兩組點之間的距離 58 for(int i=0;i<descriptors_1.rows;++i) 59 { 60 double dist=matches[i].distance; 61 // cout<<dist<<endl; 62 if(dist<min_dist) min_dist=dist; 63 if(dist>max_dist) max_dist=dist; 64 } 65 66 printf("--Max dist:%f\n",max_dist); 67 printf("--Min dist:%f\n",min_dist); 68 69 //當描述子之間的距離大於兩倍的最小距離時，即認為匹配有誤 70 //但有時候最小距離會非常小，設置一個經驗值作為下限 71 vector<DMatch> good_matches; 72 for(int i=0;i<descriptors_1.rows;++i) 73 { 74 if(matches[i].distance<=max(2*min_dist,30.0)) 75 { 76 good_matches.push_back(matches[i]); 77 } 78 } 79 80 //第5步：繪制匹配結果 81 Mat img_match; 82 Mat img_goodmatch; 83 drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_match); 84 drawMatches(img_1,keypoints_1,img_2,keypoints_2,good_matches,img_goodmatch); 85 imshow("所有匹配點對",img_match); 86 imshow("優化后匹配點對",img_goodmatch); 87 waitKey(0); 88 89 return 0; 90 }
實驗結果：1.png中提取到的特征點
2.png中提取到的特征點
匹配結果：
所有點對匹配結果
優化后的匹配點對結果（篩選依據是Hamming距離小於最小距離的兩倍）
結果分析：盡管在這個例子中利用工程經驗優化篩選出正確的匹配，但並不能保證在所有其他圖像中得到的匹配都是正確的，所以，在后面的運動估計中，還要使用去除誤匹配的算法。
2.演示如何利用匹配點對估計相機運動及利用相機運動估計特征點的空間位置
估計相機運動
2.1.如果是單目相機：只能得到2D的像素坐標->根據兩組2D點估計運動，方法：對極幾何
步驟如下：
1).根據配對點的像素位置求出本質矩陣E或基礎矩陣F
2).根據E或F求出R，t。
公式見P143
對E的介紹：
1).E在不同尺度下是等價的
2).本質矩陣的內在性質：E的奇異值一定是[σ,σ,0]T的形式
3).由於尺度等價性，E實際上有5個自由度。
求解方法：八點法(Eight-point -algorithm)。求出E后，對其分解->R,t
單應矩陣（Homograohy）H：
描述了兩個平面之間的映射關系。如果場景中的特征點都落在同一個平面上（牆、地面等），可通過單應性來估計運動
求解方法：直接線性變換法（Direct Linear Transform）。求出H后，對其分解（分解方法：數值法、解析法）->R,t
重要意義：P148
實踐：利用對極約束求解相機運動
1).將利用匹配好的特征點來計算E、F、H
2).分解E得到R，t
代碼如下：
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 7 using namespace cv; 8 using namespace std; 9 10 void find_feature_matches(const Mat& img_1,const Mat& img_2,vector<KeyPoint>& keypoint_1,vector<KeyPoint>& keypoint_2,vector<DMatch>& matches) 11 { 12 //初始化 13 Mat descriptors_1,descriptors_2; 14 //查看opencv版本號:pkg-config --modversion opencv 15 //used in OpenCV3 16 Ptr<FeatureDetector> detector=ORB::create(); 17 Ptr<DescriptorExtractor> descriptor=ORB::create(); 18 //if used in OpenCV2 19 //Ptr<FeatureDetector> detector=ORB::create("ORB"); 20 //Ptr<DescriptorExtractor> descriptor=ORB::create("ORB"); 21 Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming"); 22 23 //第1步：檢測Oriented FAST角點位置 24 detector->detect(img_1,keypoint_1); 25 detector->detect(img_2,keypoint_2); 26 27 //第2步:根據角點位置計算BRIEF描述子 28 descriptor->compute(img_1,keypoint_1,descriptors_1); 29 descriptor->compute(img_2,keypoint_2,descriptors_2); 30 31 //第3步：對兩幅圖像中的BRIED描述子進行匹配，使用Hamming距離 32 vector<DMatch> match; 33 matcher->match(descriptors_1,descriptors_2,match); 34 35 //第4步：匹配點對篩選 36 double min_dist=10000,max_dist=0; 37 38 //找出所有匹配之間的最小距離和最大距離，即是最相似和最不相似的兩組點之間的距離 39 for(int i=0;i<descriptors_1.rows;++i) 40 { 41 double dist=match[i].distance; 42 if(dist<min_dist) min_dist=dist; 43 if(dist>max_dist) max_dist=dist; 44 } 45 46 cout<<"Max dist: "<<max_dist<<endl; 47 cout<<"Min dist: "<<min_dist<<endl; 48 49 //當描述子之間的距離大於兩倍的最小距離時，即認為匹配有誤。 50 //但有時候最小距離非常小，設置一個經驗值30作為下限 51 for(int i=0;i<descriptors_1.rows;++i) 52 { 53 if(match[i].distance<=max(2*min_dist,30.0)) 54 { 55 matches.push_back(match[i]); 56 } 57 } 58 } 59 60 61 void pose_estimation_2d2d(vector<KeyPoint> keypoints_1,vector<KeyPoint> keypoints_2,vector<DMatch> matches,Mat& R,Mat& t) 62 { 63 //相機內參,TUM Freiburg2 64 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1); 65 66 //把匹配點轉換成vector<Point2f>的形式 67 vector<Point2f> points1; 68 vector<Point2f> points2; 69 70 for(int i=0;i<(int)matches.size();++i) 71 { 72 points1.push_back(keypoints_1[matches[i].queryIdx].pt);//用法？ 73 points2.push_back(keypoints_2[matches[i].trainIdx].pt);//用法？ 74 } 75 76 //計算基礎矩陣F 77 Mat fundamental_matrix; 78 //使用8點法計算F 79 fundamental_matrix=findFundamentalMat(points1,points2,CV_FM_8POINT); 80 cout<<"fundamental_matrix ="<<endl<<fundamental_matrix<<endl; 81 82 //計算本質矩陣E 83 Point2d principal_point(325.1,249.7);//相機光心，TUM dataset標定值 84 double focal_length=521;//相機焦距，TUM dataset標定值 85 Mat essential_matrix; 86 essential_matrix=findEssentialMat(points1,points2,focal_length,principal_point); 87 cout<<"essential_matrix = "<<endl<<essential_matrix<<endl; 88 89 //計算單應矩陣 90 Mat homography_matrix; 91 homography_matrix=findHomography(points1,points2,RANSAC,3); 92 cout<<"homography_matrix = "<<endl<<homography_matrix<<endl; 93 94 //從本質矩陣中恢復旋轉和平移信息 95 recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point); 96 cout<<"R ="<<endl<<R<<endl; 97 cout<<"t ="<<endl<<t<<endl; 98 } 99 100 Point2d pixel2cam(const Point2d& p,const Mat& K) 101 { 102 return Point2d 103 ( 104 (p.x-K.at<double>(0,2))/K.at<double>(0,0), 105 (p.y-K.at<double>(1,2))/K.at<double>(1,1) 106 ); 107 } 108 109 int main(int argc,char** argv) 110 { 111 if(argc!=3) 112 { 113 cout<<"usage:pose_estimation_2d2d img1 img2"<<endl; 114 return 1; 115 } 116 117 //讀取圖像 118 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR); 119 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR); 120 121 vector<KeyPoint> keypoints_1,keypoints_2; 122 vector<DMatch> matches; 123 find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches); 124 125 cout<<"一共找到了"<<matches.size()<<"組匹配點對"<<endl; 126 127 //估計兩張圖像間運動 128 Mat R,t; 129 pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t); 130 131 //驗證E=t^R 132 Mat t_x=(Mat_<double>(3,3)<< 133 0, -t.at<double>(2,0), t.at<double>(1,0), 134 t.at<double>(2,0), 0, -t.at<double>(0,0), 135 -t.at<double>(1,0), t.at<double>(0,0), 0 ); 136 // cout<<"t^ = "<<endl<<t_x<<endl; 137 cout<<"t^R ="<<endl<<t_x*R<<endl;//!=E??? 138 139 //驗證對極約束 140 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1); 141 for(DMatch m:matches) 142 { 143 Point2d pt1=pixel2cam(keypoints_1[m.queryIdx].pt,K); 144 Mat y1=(Mat_<double>(3,1)<<pt1.x,pt1.y,1); 145 Point2d pt2=pixel2cam(keypoints_2[m.trainIdx].pt,K); 146 Mat y2=(Mat_<double>(3,1)<<pt2.x,pt2.y,1); 147 Mat d=y2.t()*t_x*R*y1; 148 cout<<"epipolar constraint = "<<d<<endl; 149 } 150 return 0; 151 }
實驗結果：
計算得到的F、F、H分別為：
恢復的R和t：
驗證E=t^R:
這里有個疑問：t^R計算后得到的矩陣和E相差有點大（不知道能不能這樣說），所以，是怎么回事呢？
驗證堆積約束：x2Tt^Rx1=0
……
單目視覺slam：尺度不確定性。即對軌跡和地圖縮放任意倍數，得到的圖像依然是一樣的
固定尺度的做法：1.對兩張圖像的t歸一化
2.令所有的特征點平均深度為1（相比較而言，特征點深度歸一化可控制場景的規模大小，使得計算在數值上更穩定）
單目初始化不能只有純旋轉，必須要有一定程度的平移，才可進行單目的初始化.
估計特征點的空間位置
在獲估計了相機運動之后，需要借助相機的運動估計特征點的空間位置。方法：三角測量（Triangulation）(或三角化）來估計地圖點的深度
三角測量：通過在兩處觀察同一個點的夾角，從而確定該點的距離
代碼如下：
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 7 using namespace std; 8 using namespace cv; 9 10 void find_feature_matches(const Mat& img_1,const Mat& img_2, 11 vector<KeyPoint>& keypoints_1, 12 vector<KeyPoint>& keypoints_2, 13 vector<DMatch>& matches) 14 { 15 //初始化 16 Mat descriptors_1,descriptors_2; 17 Ptr<FeatureDetector> detector=ORB::create(); 18 Ptr<DescriptorExtractor> descriptor=ORB::create(); 19 Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming"); 20 21 //第一步:檢測 Oriented FAST 角點位置 22 detector->detect(img_1,keypoints_1); 23 detector->detect(img_2,keypoints_2); 24 25 //第二步:根據角點位置計算 BRIEF 描述子 26 descriptor->compute(img_1,keypoints_1,descriptors_1); 27 descriptor->compute(img_2,keypoints_2,descriptors_2); 28 29 //第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離 30 vector<DMatch> match; 31 matcher->match(descriptors_1,descriptors_2,match); 32 33 //第四步:匹配點對篩選 34 double min_dist=1000,max_dist=0; 35 36 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 37 for(int i=0;i<descriptors_1.rows;++i) 38 { 39 double dist=match[i].distance; 40 if(dist<min_dist) min_dist=dist; 41 if(dist>max_dist) max_dist=dist; 42 } 43 44 printf("Max dist :%f\n",max_dist); 45 printf("Min dist :%f\n",min_dist); 46 47 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限. 48 for(int i=0;i<descriptors_1.rows;++i) 49 { 50 if(match[i].distance<=max(2*min_dist,30.0)) 51 { 52 matches.push_back(match[i]); 53 } 54 } 55 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 56 } 57 58 //估計兩張圖像間運動 59 void pose_estimation_2d2d(const vector<KeyPoint>& keypoints_1, 60 const vector<KeyPoint>& keypoints_2, 61 const vector<DMatch>& matches, 62 Mat& R,Mat& t) 63 { 64 //相機內參,TUM Freiburg2 65 Mat K=(Mat_<double> (3,3)<<520.9,325.1,0,521.0,249.7,0,0,1); 66 67 //把匹配點轉換為vector<Point2f>的形式 68 vector<Point2f> points1; 69 vector<Point2f> points2; 70 71 for(int i=0;i<(int)matches.size();++i) 72 { 73 points1.push_back(keypoints_1[matches[i].queryIdx].pt);//? 74 points2.push_back(keypoints_2[matches[i].trainIdx].pt);//? 75 } 76 77 //計算基礎矩陣 78 Mat fundamental_matrix; 79 fundamental_matrix=findFundamentalMat(points1,points2,CV_FM_8POINT); 80 cout<<"fundamental_matrix ="<<endl<<fundamental_matrix<<endl; 81 82 //計算本質矩陣 83 Point2d principal_point(325.1,249.7);//相機光心，TUM dataset標定值 84 int focal_length=521;////相機焦距, TUM dataset標定值 85 Mat essential_matrix; 86 essential_matrix=findEssentialMat(points1,points2,focal_length,principal_point); 87 cout<<"essential_matrix = "<<endl<<essential_matrix<<endl; 88 89 //計算單應矩陣 90 Mat homography_matrix; 91 homography_matrix=findHomography(points1,points2,RANSAC,3); 92 cout<<"homography_matrix = "<<homography_matrix<<endl; 93 94 //從本質矩陣中恢復旋轉和平移信息 95 recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point); 96 cout<<"R = "<<endl<<R<<endl; 97 cout<<"t = "<<endl<<t<<endl; 98 } 99 100 Point2f pixel2cam(const Point2d& p,const Mat& K) 101 { 102 return Point2f( 103 (p.x-K.at<double>(0,2))/K.at<double>(0,0), 104 (p.y-K.at<double>(1,2))/K.at<double>(1,1) 105 ); 106 } 107 108 void triangulation(const vector<KeyPoint>& keypoint_1, 109 const vector<KeyPoint>& keypoint_2, 110 const vector<DMatch>& matches, 111 const Mat& R,const Mat& t, 112 vector<Point3d>& points) 113 { 114 Mat T1=(Mat_<float> (3,4)<< 115 1,0,0,0, 116 0,1,0,0, 117 0,0,1,0); 118 Mat T2=(Mat_<float>(3,4)<< 119 R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),t.at<double>(0,0), 120 R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),t.at<double>(1,0), 121 R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),t.at<double>(2,0) 122 ); 123 124 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1); 125 vector<Point2f> pts_1,pts_2; 126 for(DMatch m:matches) 127 { 128 //將像素坐標轉換至相機坐標 129 pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt,K)); 130 pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt,K)); 131 } 132 133 Mat pts_4d; 134 triangulatePoints(T1,T2,pts_1,pts_2,pts_4d); 135 cout<<pts_4d.cols<<endl; 136 //轉換成非齊次坐標 137 for(int i=0;i<pts_4d.cols;++i) 138 { 139 Mat x=pts_4d.col(i); 140 x/=x.at<float>(3,0);//歸一化 141 Point3d p( 142 x.at<float>(0,0), 143 x.at<float>(1,0), 144 x.at<float>(2,0) 145 ); 146 cout<<"p = "<<endl<<p<<endl; 147 points.push_back(p); 148 } 149 } 150 151 152 153 int main(int argc,char** argv) 154 { 155 if(argc!=3) 156 { 157 cout<<"usage:triangulation img1 img2"<<endl; 158 return 1; 159 } 160 161 //讀取圖像 162 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR); 163 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR); 164 165 //尋找匹配點對 166 vector<KeyPoint> keypoints_1,keypoints_2; 167 vector<DMatch> matches; 168 find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches); 169 170 //估計兩張圖像間運動 171 Mat R,t; 172 pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t); 173 174 //三角化 175 vector<Point3d> points; 176 triangulation(keypoints_1,keypoints_2,matches,R,t,points); 177 178 //驗證三角化點與特征點的重投影關系 179 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1); 180 for(int i=0;i<matches.size();++i) 181 { 182 Point2d pt1_cam=pixel2cam(keypoints_1[matches[i].queryIdx].pt,K); 183 Point2d pt1_cam_3d( 184 points[i].x/points[i].z, 185 points[i].y/points[i].z 186 ); 187 cout<<"point in the first camera frame: "<<pt1_cam<<endl; 188 cout<<"point projected from 3D"<<pt1_cam_3d<<",d="<<points[i].z<<endl; 189 190 //第二個圖 191 Point2f pt2_cam=pixel2cam(keypoints_2[matches[i].trainIdx].pt,K); 192 Mat pt2_trans=R*(Mat_<double>(3,1)<<points[i].x,points[i].y,points[i].z)+t; 193 pt2_trans/=pt2_trans.at<double>(2,0); 194 cout<<"point in the second camera a frame: "<<pt2_cam<<endl; 195 cout<<"point projected from second frame: "<<pt2_trans.t()<<endl; 196 cout<<endl; 197 198 } 199 200 201 return 0; 202 }
純旋轉無法使用三角測量，因為對極約束會永遠滿足。
提高三角化精度的方法：1.提高特征點的提取精度，即提高圖像分辨率。缺點：會導致圖像變大，增加計算成本。
2.使平移量增大（平移較大時，在同樣的相機分辨率下，三角化測量會更精確）。缺點：導致圖像的外觀會發生明顯變化，使得特征提取和匹配變得困難。
三角測量的矛盾：增大平移量->匹配失效；平移太小->三角化精度不夠。
2.2.已有3D點及其在相機的投影位置（3D-2D），來估計相機的運動，方法：PnP
PnP:求解3D到2D點對運動的方法。描述了當知道n個3D空間點及其投影位置時，如何估計相機的位姿
優點：無需使用對極約束，又可在很少的匹配點中獲得較好的運動估計，是最重要的一種姿態估計方法。
求解方法：P3P-用3對點估計位姿
DLT-直接線性變換
EPnP(Efficient PnP)
UPnP
Bundle Adjustment-非線性優化的方式，構建最小二乘問題並迭代求解
1).DLT（Direct Linear Transform）
2).P3P
輸入數據：3對3D-2D匹配點。記3D點為A，B，C。2D點為a,b,c。小寫字母對應的點為對應大寫字母代表的點在相應的成像平面上的 投影
注意：A，B，C表示的是在世界坐標系中的坐標，而不是在相機坐標系中的坐標。如果一旦能算出3D點在相機坐標系下的坐標，就能得到3D-3D的對應點，從而把PnP問題轉化為ICP問題。
缺點：1.P3P只利用3個點的信息，當給定的配對點多於3組時，難以利用更多的信息。
2.如果2D點或3D點受噪聲的影響，或存在誤匹配，則算法失效
3).EPnP、UPnP等
相較於P3P來說，優點：1.利用更過的信息
2.用迭代的方式對相機的位姿進行優化，盡可能消除噪聲的影響
缺點：原理更復雜
通常的做法：先使用P3P/EPnP等方法估計相機位姿，然后構建最小二乘優化問題對估計值進行調整（Bundle Adjustment）
線性方法：先求相機位姿，再求空間位置
非線性優化：把相機位姿和空間位置均看作優化變量，放在一起優化
4).Bundle Adjustment
兩個重要的公式見P164（公式7.45）和P165（公式7.47）-分別描述了觀測相機方程關於相機位姿與特征點的兩個導數矩陣。能夠在優化過程中（優化位姿和優化特征點的空間位置）提供重要的梯度方向，指導優化的迭代。
實驗代碼如下：首先用OpenCV提供的EPnP求解PnP問題，然后通過g2o對結果進行優化
第一步：使用EPnP求解位姿
代碼如下：
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 int main ( int argc, char** argv ) 29 { 30 if ( argc != 4 ) 31 { 32 cout<<"usage: pose_estimation_3d2d img1 img2 depth1"<<endl; 33 return 1; 34 } 35 //-- 讀取圖像 36 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 37 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 38 39 vector<KeyPoint> keypoints_1, keypoints_2; 40 vector<DMatch> matches; 41 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 42 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 43 44 // 建立3D點 45 Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道圖像 46 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 47 vector<Point3f> pts_3d; 48 vector<Point2f> pts_2d; 49 for ( DMatch m:matches ) 50 { 51 ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; 52 if ( d == 0 ) // bad depth 53 continue; 54 float dd = d/5000.0; 55 Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); 56 pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) ); 57 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); 58 } 59 60 cout<<"3d-2d pairs: "<<pts_3d.size() <<endl; 61 62 Mat r, t; 63 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法 64 Mat R; 65 cv::Rodrigues ( r, R ); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣 66 67 cout<<"R="<<endl<<R<<endl; 68 cout<<"t="<<endl<<t<<endl; 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 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 123 { 124 return Point2d 125 ( 126 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 127 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 128 ); 129 }
運行結果為：
分析：在有3D信息時，由於引入了新的深度信息，估計的R與之前2D-2D情況下求解的R幾乎是相同的，但是t相差較大。不過由於Kinect采集的深度地圖本身會有一些誤差，導致這里的3D點也不是准確的。所以，下一步要把位姿和所有三維特征點同時優化。
第二步：使用前一步的估計值作為初始值，對結果進行優化：
2.2.1 利用g2o優化庫提供的Bundle Adjustment對結果進行優化。g2o優化的步驟如下：
1）先將優化問題表示成圖：頂點——優化變量
邊——誤差項
（1）定義頂點和邊的類型：如果頂點和邊的類型在g2o中有提供，就不用自己實現，否則，要自己實現。
（2）構建圖
(這一步是利用g2o優化求解的關鍵)
2) 選擇優化算法：GN、LM、DogLeg……
3）調用g2o進行優化，返回結果
我們來分析下這個問題的優化思路：
首先構建最小二乘問題，見P162公式（7.53）。該問題的誤差項表示的是，將像素坐標（觀測到的投影位置）與3D點按照當前估計的位姿進行投影得到的位置相比較得到的誤差（重投影誤差）。
所以該問題中，優化問題是pose（我們選第2個相機的位姿節點，因為第1個相機位姿固定為0，故不用寫到優化變量中。思考：如果把第1個相機的位姿與觀測也考慮進來呢？）
問題轉化為：根據一組3D點和第二個圖像中得2D投影估計第二個相機的位姿。
因為這個優化問題中的邊（即誤差項，EdgeProjectXTZ2UV（投影方程邊））和頂點（即優化變量，VertexSE3Expmap(李代數位姿)、VertexSBAPointXYZ（空間點位置））提供了，所以不用自己實現。
明確了g2o優化的思路和步驟，下面我們就利用g2o優化庫來進行優化，代碼如下：
代碼如下：
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(const vector<Point3f> points_3d, 29 const vector<Point2f> points_2d, 30 const Mat& K, 31 Mat& R,Mat& t 32 ); 33 34 int main ( int argc, char** argv ) 35 { 36 if ( argc != 4 ) 37 { 38 cout<<"usage: pose_estimation_3d2d img1 img2 depth1"<<endl; 39 return 1; 40 } 41 //-- 讀取圖像 42 Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ); 43 Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ); 44 45 vector<KeyPoint> keypoints_1, keypoints_2; 46 vector<DMatch> matches; 47 find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches ); 48 cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl; 49 50 // 建立3D點 51 Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度圖為16位無符號數，單通道圖像 52 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 53 vector<Point3f> pts_3d; 54 vector<Point2f> pts_2d; 55 for ( DMatch m:matches ) 56 { 57 ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ]; 58 if ( d == 0 ) // bad depth 59 continue; 60 float dd = d/5000.0; 61 Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K ); 62 pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) ); 63 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); 64 } 65 66 cout<<"3d-2d pairs: "<<pts_3d.size() <<endl; 67 68 Mat r, t; 69 solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法 70 Mat R; 71 cv::Rodrigues ( r, R ); // r為旋轉向量形式，用Rodrigues公式轉換為矩陣 72 73 cout<<"R="<<endl<<R<<endl; 74 cout<<"t="<<endl<<t<<endl; 75 76 cout<<"calling bundle adjustment"<<endl; 77 78 bundleAdjustment(pts_3d,pts_2d,K,R,t); 79 } 80 81 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 82 std::vector<KeyPoint>& keypoints_1, 83 std::vector<KeyPoint>& keypoints_2, 84 std::vector< DMatch >& matches ) 85 { 86 //-- 初始化 87 Mat descriptors_1, descriptors_2; 88 // used in OpenCV3 89 Ptr<FeatureDetector> detector = ORB::create(); 90 Ptr<DescriptorExtractor> descriptor = ORB::create(); 91 // use this if you are in OpenCV2 92 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 93 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 94 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 95 //-- 第一步:檢測 Oriented FAST 角點位置 96 detector->detect ( img_1,keypoints_1 ); 97 detector->detect ( img_2,keypoints_2 ); 98 99 //-- 第二步:根據角點位置計算 BRIEF 描述子 100 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 101 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 102 103 //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離 104 vector<DMatch> match; 105 // BFMatcher matcher ( NORM_HAMMING ); 106 matcher->match ( descriptors_1, descriptors_2, match ); 107 108 //-- 第四步:匹配點對篩選 109 double min_dist=10000, max_dist=0; 110 111 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 112 for ( int i = 0; i < descriptors_1.rows; i++ ) 113 { 114 double dist = match[i].distance; 115 if ( dist < min_dist ) min_dist = dist; 116 if ( dist > max_dist ) max_dist = dist; 117 } 118 119 printf ( "-- Max dist : %f \n", max_dist ); 120 printf ( "-- Min dist : %f \n", min_dist ); 121 122 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限. 123 for ( int i = 0; i < descriptors_1.rows; i++ ) 124 { 125 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 126 { 127 matches.push_back ( match[i] ); 128 } 129 } 130 } 131 132 Point2d pixel2cam ( const Point2d& p, const Mat& K ) 133 { 134 return Point2d 135 ( 136 ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ), 137 ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 ) 138 ); 139 } 140 141 void bundleAdjustment(const vector<Point3f> points_3d, 142 const vector<Point2f> points_2d, 143 const Mat &K, Mat &R, Mat &t) 144 { 145 // 初始化g2o 146 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;//pose 維度為 6, landmark 維度為 3 147 Block::LinearSolverType* linearSolver=new g2o::LinearSolverCSparse<Block::PoseMatrixType>();//線性方程求解器 148 Block* solver_ptr=new Block(unique_ptr<Block::LinearSolverType>(linearSolver));//矩陣塊求解器 149 g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(unique_ptr<Block>(solver_ptr)); 150 g2o::SparseOptimizer optimizer; 151 optimizer.setAlgorithm(solver); 152 153 //vertex 154 g2o::VertexSE3Expmap* pose=new g2o::VertexSE3Expmap();//camera pose 155 Eigen::Matrix3d R_mat; 156 R_mat<< 157 R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2), 158 R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2), 159 R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2); 160 pose->setId(0); 161 pose->setEstimate(g2o::SE3Quat( 162 R_mat, 163 Eigen::Vector3d(t.at<double>(0,0),t.at<double>(1,0),t.at<double>(2,0)) 164 )); 165 optimizer.addVertex(pose); 166 167 int index=1; 168 for(const Point3f p:points_3d)//landmarks 169 { 170 g2o::VertexSBAPointXYZ* point=new g2o::VertexSBAPointXYZ(); 171 point->setId(index++); 172 point->setEstimate(Eigen::Vector3d(p.x,p.y,p.z)); 173 point->setMarginalized(true);//g2o 中必須設置 marg 參見第十講內容 174 optimizer.addVertex(point); 175 } 176 177 //parameter:camera intrinsics 178 g2o::CameraParameters* camera=new g2o::CameraParameters( 179 K.at<double>(0,0),Eigen::Vector2d(K.at<double>(0,2),K.at<double>(1,2)),0); 180 camera->setId(0); 181 optimizer.addParameter(camera); 182 183 //edges 184 index=1; 185 for(const Point2f p:points_2d) 186 { 187 g2o::EdgeProjectXYZ2UV* edge=new g2o::EdgeProjectXYZ2UV(); 188 edge->setId(index); 189 edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(index))); 190 edge->setVertex(1,pose); 191 edge->setMeasurement(Eigen::Vector2d(p.x,p.y)); 192 edge->setParameterId(0,0); 193 edge->setInformation(Eigen::Matrix2d::Identity()); 194 optimizer.addEdge(edge); 195 index++; 196 } 197 198 chrono::steady_clock::time_point t1=chrono::steady_clock::now(); 199 optimizer.setVerbose(true); 200 optimizer.initializeOptimization(); 201 optimizer.optimize(100); 202 chrono::steady_clock::time_point t2=chrono::steady_clock::now(); 203 chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1); 204 cout<<"optimization costs time: "<<time_used.count()<<endl; 205 206 cout<<endl<<"after optimization:"<<endl; 207 cout<<"T = "<<endl<<Eigen::Isometry3d(pose->estimate()).matrix()<<endl; 208 }
實驗結果：
可以看出，由於同時優化了特征點和相機位姿，最后輸出的位姿變換矩陣T，對比之前直接做PnP的結果，大約在小數點后第3位發生了一 些變化。
2.2.2 利用Ceres優化庫，見《視覺SLAM十四講》課后習題—ch7（更新中……）習題10。
2.3.如果是雙目或RGB-D相機時，或者能獲得距離信息->根據兩組3D點估計運動，方法：ICP(迭代最近點，Iterative Closest Point)
假設有一組配對好的3D點：P={p1,…,pn}, P'={p1',…,pn'}，我們要找一個歐式變換R,t，使得：對於所有i,pi=Rpi'+t.這個問題就用ICP方法 求解。
注意：僅考慮兩組3D點之間得變換時，和相機並沒有關系。
ICP的求解方式：1)利用線性代數的求解（主要是SVD）。2)利用非線性優化的求解方方式（類似與BA）
2.3.1 SVD方法
簡化后的目標優化函數為：
分3個步驟求解：
（1）計算兩組點的質心位置p，p'，然后計算每個點的去質心坐標：令qi=pi-p,qi'=pi'-p
（2）根據以下優化問題計算旋轉矩陣：
（3）根據第2步的R求t：t*=p-Rp'
代碼如下：
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/highgui/highgui.hpp> 4 #include <opencv2/calib3d/calib3d.hpp> 5 #include <opencv2/features2d/features2d.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(const Mat& img_1,const Mat& img_2, 21 vector<KeyPoint>& keypoints_1, 22 vector<KeyPoint>& keypoints_2, 23 vector<DMatch>& matches); 24 25 //像素坐標轉相機歸一化坐標 26 Point2d pixel2cam(const Point2d& p,const Mat& K); 27 28 void pose_estimation_3d3d(const vector<Point3f>& pts1, 29 const vector<Point3f>& pts2, 30 Mat& R,Mat& t); 31 32 33 int main(int argc,char** argv) 34 { 35 if(argc!=5) 36 { 37 cout<<"usage:pose_estimation_3d3d img1 img2 depth1 depth2"<<endl; 38 return 1; 39 } 40 41 //讀取圖像 42 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);//test 43 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR); 44 45 vector<KeyPoint> keypoints_1,keypoints_2; 46 vector<DMatch> matches; 47 find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches); 48 cout<<"一共找到了"<<matches.size()<<"組匹配點"<<endl; 49 50 //建立3D點 51 Mat depth_1=imread(argv[3],CV_LOAD_IMAGE_UNCHANGED);//深度圖為16位無符號數，單通道圖像 52 Mat depth_2=imread(argv[4],CV_LOAD_IMAGE_UNCHANGED);//深度圖為16位無符號數，單通道圖像 53 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1, 54 0,521.0,249.7, 55 0,0,1); 56 vector<Point3f> pts1,pts2; 57 for(DMatch m:matches) 58 { 59 ushort d1=depth_1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)]; 60 ushort d2=depth_2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)]; 61 if(d1==0 || d2==0)//bad depth 62 continue; 63 Point2d p1=pixel2cam(keypoints_1[m.queryIdx].pt,K); 64 Point2d p2=pixel2cam(keypoints_2[m.trainIdx].pt,K); 65 float dd1=float(d1)/5000.0; 66 float dd2=float(d2)/5000.0; 67 pts1.push_back(Point3f(p1.x*dd1,p1.y*dd1,dd1)); 68 pts2.push_back(Point3f(p2.x*dd2,p2.y*dd2,dd2)); 69 } 70 71 cout<<"3d-3d pairs: "<<pts1.size()<<endl; 72 Mat R,t; 73 pose_estimation_3d3d(pts1,pts2,R,t); 74 cout<<"ICP via SVD results: "<<endl; 75 cout<<"R ="<<endl<<R<<endl; 76 cout<<"t ="<<endl<<t<<endl; 77 cout<<"R_inv ="<<endl<<R.t()<<endl; 78 cout<<"t_inv ="<<endl<<-R.t()*t<<endl; 79 return 0; 80 } 81 82 //匹配特征點對 83 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 84 std::vector<KeyPoint>& keypoints_1, 85 std::vector<KeyPoint>& keypoints_2, 86 std::vector< DMatch >& matches ) 87 { 88 //-- 初始化 89 Mat descriptors_1, descriptors_2; 90 // used in OpenCV3 91 Ptr<FeatureDetector> detector = ORB::create(); 92 Ptr<DescriptorExtractor> descriptor = ORB::create(); 93 // use this if you are in OpenCV2 94 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 95 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 96 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 97 //-- 第一步:檢測 Oriented FAST 角點位置 98 detector->detect ( img_1,keypoints_1 ); 99 detector->detect ( img_2,keypoints_2 ); 100 101 //-- 第二步:根據角點位置計算 BRIEF 描述子 102 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 103 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 104 105 //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離 106 vector<DMatch> match; 107 // BFMatcher matcher ( NORM_HAMMING ); 108 matcher->match ( descriptors_1, descriptors_2, match ); 109 110 //-- 第四步:匹配點對篩選 111 double min_dist=10000, max_dist=0; 112 113 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 114 for ( int i = 0; i < descriptors_1.rows; i++ ) 115 { 116 double dist = match[i].distance; 117 if ( dist < min_dist ) min_dist = dist; 118 if ( dist > max_dist ) max_dist = dist; 119 } 120 121 printf ( "-- Max dist : %f \n", max_dist ); 122 printf ( "-- Min dist : %f \n", min_dist ); 123 124 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限. 125 for ( int i = 0; i < descriptors_1.rows; i++ ) 126 { 127 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 128 { 129 matches.push_back ( match[i] ); 130 } 131 } 132 } 133 134 Point2d pixel2cam(const Point2d &p, const Mat &K) 135 { 136 return Point2d( 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 pose_estimation_3d3d(const vector<Point3f> &pts1, const vector<Point3f> &pts2, Mat &R, Mat &t) 143 { 144 Point3f p1,p2;//center of mass 145 int N=pts1.size(); 146 //計算兩組點的質心位置p1,p2 147 for(int i=0;i<N;++i) 148 { 149 p1+=pts1[i]; 150 p2+=pts2[i]; 151 } 152 p1=Point3f(Vec3f(p1)/N);//test 153 p2=Point3f(Vec3f(p2)/N);//test 154 155 //計算每個點的去質心坐標q1,q2 156 vector<Point3f> q1(N),q2(N); 157 for(int i=0;i<N;++i) 158 { 159 q1[i]=pts1[i]-p1; 160 q2[i]=pts2[i]-p2; 161 } 162 163 //compute q1*q2^T,即書中P174的矩陣W 164 Eigen::Matrix3d W=Eigen::Matrix3d::Zero(); 165 for(int i=0;i<N;++i) 166 { 167 W+=Eigen::Vector3d(q1[i].x,q1[i].y,q1[i].z)*Eigen::Vector3d(q2[i].x,q2[i].y,q2[i].z).transpose(); 168 } 169 cout<<"W ="<<endl<<W<<endl<<endl; 170 171 //SVD on W 172 Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,Eigen::ComputeFullU|Eigen::ComputeFullV); 173 Eigen::Matrix3d U=svd.matrixU(); 174 Eigen::Matrix3d V=svd.matrixV(); 175 176 if(U.determinant()*V.determinant()<0) 177 { 178 for(int x=0;x<3;++x) 179 { 180 U(x,2)*=-1; 181 } 182 } 183 184 cout<<"U="<<U<<endl; 185 cout<<"V="<<V<<endl; 186 187 Eigen::Matrix3d R_=U*(V.transpose()); 188 Eigen::Vector3d t_=Eigen::Vector3d(p1.x,p1.y,p1.z)-R_*Eigen::Vector3d(p2.x,p2.y,p2.z); 189 190 191 192 //convert to cv::Mat 193 R=(Mat_<double>(3,3)<< 194 R_(0,0),R_(0,1),R_(0,2), 195 R_(1,0),R_(1,1),R_(1,2), 196 R_(2,0),R_(2,1),R_(2,2)); 197 t=(Mat_<double>(3,1)<<t_(0,0),t_(1,0),t_(2,0)); 198 }
運行結果：
與Section2.2 PnP得到的結果對比，可以看出這里的R,t是第二幀到第一幀的變換，與前面的PnP是相反的。
由對極幾何->PnP->ICP，我們使用了越來越多的信息：沒有深度->有一個圖的深度->有兩個圖的深度。在深度准確的情況下，得到的估計也會越來越准確。
ICP方法也存在一些缺點：由於Kinect的深度圖存在噪聲，並且數據可能會丟失，使得一些深度數據的特征點不得不丟棄。這可能會導致ICP的估計不夠准確。並且，如果特征點丟失的太多，可能會因為特征點太少而無法進行運動估計。
2.3.2 非線性優化方法：以迭代的方式尋找最優值
目標函數：minξ=½∑ni=1||pi-exp(ξ^)pi'||22
ICP問題存在唯一解或無窮多解的情況，在唯一解的情況下，只要能找到極小值解，這個極小值解就是我們要求的全局最小值=>已知匹配點時求解ICP的好處：ICP求解可以選定任意的初始值。
與SVD方法的不同之：在優化中不僅考慮相機的位姿，同時會優化3D點的空間位置。
代碼如下：
1 #include <iostream> 2 #include <opencv2/core/core.hpp> 3 #include <opencv2/highgui/highgui.hpp> 4 #include <opencv2/calib3d/calib3d.hpp> 5 #include <opencv2/features2d/features2d.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_gauss_newton.h> 12 #include <g2o/solvers/eigen/linear_solver_eigen.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(const Mat& img_1,const Mat& img_2, 20 vector<KeyPoint>& keypoints_1, 21 vector<KeyPoint>& keypoints_2, 22 vector<DMatch>& matches); 23 24 //像素坐標轉相機歸一化坐標 25 Point2d pixel2cam(const Point2d& p,const Mat& K); 26 30 31 void bundleAdjustment(const vector<Point3f>& pts1, 32 const vector<Point3f>& pts2, 33 Mat& R,Mat& t); 34 35 //g2o edge 36 class EdgeProjectXYZRGBDPoseOnly:public g2o::BaseUnaryEdge<3,Eigen::Vector3d,g2o::VertexSE3Expmap> 37 { 38 public: 39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point):_point(point){} 41 42 virtual void computeError() 43 { 44 const g2o::VertexSE3Expmap* pose=static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]); 45 //measurement is p,point is p' 46 _error=_measurement-pose->estimate().map(_point); 47 } 48 49 virtual void linearizeOplus() 50 { 51 g2o::VertexSE3Expmap* pose=static_cast<g2o::VertexSE3Expmap*>(_vertices[0]); 52 g2o::SE3Quat T(pose->estimate()); 53 Eigen::Vector3d xyz_trans=T.map(_point); 54 double x=xyz_trans[0]; 55 double y=xyz_trans[1]; 56 double z=xyz_trans[2]; 57 58 // 誤差項對待優化變量的Jacobin 59 _jacobianOplusXi(0,0)=0; 60 _jacobianOplusXi(0,1)=-z; 61 _jacobianOplusXi(0,2)=y; 62 _jacobianOplusXi(0,3)=-1; 63 _jacobianOplusXi(0,4)=0; 64 _jacobianOplusXi(0,5)=0; 65 66 _jacobianOplusXi(1,0)=z; 67 _jacobianOplusXi(1,1)=0; 68 _jacobianOplusXi(1,2)=-x; 69 _jacobianOplusXi(1,3)=0; 70 _jacobianOplusXi(1,4)=-1; 71 _jacobianOplusXi(1,5)=0; 72 73 _jacobianOplusXi(2,0)=-y; 74 _jacobianOplusXi(2,1)=x; 75 _jacobianOplusXi(2,2)=0; 76 _jacobianOplusXi(2,3)=0; 77 _jacobianOplusXi(2,4)=0; 78 _jacobianOplusXi(2,5)=-1; 79 } 80 81 bool read(istream& in) {} 82 bool write(ostream& out) const{} 83 protected: 84 Eigen::Vector3d _point; 85 }; 86 87 88 int main(int argc,char** argv) 89 { 90 if(argc!=5) 91 { 92 cout<<"usage:pose_estimation_3d3d img1 img2 depth1 depth2"<<endl; 93 return 1; 94 } 95 96 //讀取圖像 97 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);//test 98 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR); 99 100 vector<KeyPoint> keypoints_1,keypoints_2; 101 vector<DMatch> matches; 102 find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches); 103 cout<<"一共找到了"<<matches.size()<<"組匹配點"<<endl; 104 105 //建立3D點 106 Mat depth_1=imread(argv[3],CV_LOAD_IMAGE_UNCHANGED);//深度圖為16位無符號數，單通道圖像 107 Mat depth_2=imread(argv[4],CV_LOAD_IMAGE_UNCHANGED);//深度圖為16位無符號數，單通道圖像 108 Mat K=(Mat_<double>(3,3)<<520.9,0,325.1, 109 0,521.0,249.7, 110 0,0,1); 111 vector<Point3f> pts1,pts2; 112 for(DMatch m:matches) 113 { 114 ushort d1=depth_1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)]; 115 ushort d2=depth_2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)]; 116 if(d1==0 || d2==0)//bad depth 117 continue; 118 Point2d p1=pixel2cam(keypoints_1[m.queryIdx].pt,K); 119 Point2d p2=pixel2cam(keypoints_2[m.trainIdx].pt,K); 120 float dd1=float(d1)/5000.0; 121 float dd2=float(d2)/5000.0; 122 pts1.push_back(Point3f(p1.x*dd1,p1.y*dd1,dd1)); 123 pts2.push_back(Point3f(p2.x*dd2,p2.y*dd2,dd2)); 124 } 125 126 cout<<"3d-3d pairs: "<<pts1.size()<<endl; 127 Mat R,t; 128 129 cout<<"calling bundle adjustment"<<endl; 130 bundleAdjustment(pts1,pts2,R,t); 131 return 0; 132 } 133 134 //匹配特征點對 135 void find_feature_matches ( const Mat& img_1, const Mat& img_2, 136 std::vector<KeyPoint>& keypoints_1, 137 std::vector<KeyPoint>& keypoints_2, 138 std::vector< DMatch >& matches ) 139 { 140 //-- 初始化 141 Mat descriptors_1, descriptors_2; 142 // used in OpenCV3 143 Ptr<FeatureDetector> detector = ORB::create(); 144 Ptr<DescriptorExtractor> descriptor = ORB::create(); 145 // use this if you are in OpenCV2 146 // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" ); 147 // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" ); 148 Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" ); 149 //-- 第一步:檢測 Oriented FAST 角點位置 150 detector->detect ( img_1,keypoints_1 ); 151 detector->detect ( img_2,keypoints_2 ); 152 153 //-- 第二步:根據角點位置計算 BRIEF 描述子 154 descriptor->compute ( img_1, keypoints_1, descriptors_1 ); 155 descriptor->compute ( img_2, keypoints_2, descriptors_2 ); 156 157 //-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離 158 vector<DMatch> match; 159 // BFMatcher matcher ( NORM_HAMMING ); 160 matcher->match ( descriptors_1, descriptors_2, match ); 161 162 //-- 第四步:匹配點對篩選 163 double min_dist=10000, max_dist=0; 164 165 //找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離 166 for ( int i = 0; i < descriptors_1.rows; i++ ) 167 { 168 double dist = match[i].distance; 169 if ( dist < min_dist ) min_dist = dist; 170 if ( dist > max_dist ) max_dist = dist; 171 } 172 173 printf ( "-- Max dist : %f \n", max_dist ); 174 printf ( "-- Min dist : %f \n", min_dist ); 175 176 //當描述子之間的距離大於兩倍的最小距離時,即認為匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作為下限. 177 for ( int i = 0; i < descriptors_1.rows; i++ ) 178 { 179 if ( match[i].distance <= max ( 2*min_dist, 30.0 ) ) 180 { 181 matches.push_back ( match[i] ); 182 } 183 } 184 } 185 186 Point2d pixel2cam(const Point2d &p, const Mat &K) 187 { 188 return Point2d( 189 (p.x-K.at<double>(0,2))/K.at<double>(0,0), 190 (p.y-K.at<double>(1,2))/K.at<double>(1,1) 191 ); 192 } 193 194 195 void bundleAdjustment(const vector<Point3f> &pts1, const vector<Point3f> &pts2, Mat &R, Mat &t) 196 { 197 //初始化g2o 198 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>>Block;////pose 維度為 6, landmark 維度為 3 199 Block::LinearSolverType* linearSolver=new g2o::LinearSolverEigen<Block::PoseMatrixType>();//線性方程求解器 200 Block* solver_ptr=new Block(unique_ptr<Block::LinearSolverType>(linearSolver));//矩陣塊求解器 201 g2o::OptimizationAlgorithmGaussNewton* solver=new g2o::OptimizationAlgorithmGaussNewton(unique_ptr<Block>(solver_ptr)); 202 g2o::SparseOptimizer optimizer; 203 optimizer.setAlgorithm(solver); 204 205 //vertex 206 g2o::VertexSE3Expmap* pose=new g2o::VertexSE3Expmap();//camera pose 207 pose->setId(0); 208 pose->setEstimate(g2o::SE3Quat( 209 Eigen::Matrix3d::Identity(), 210 Eigen::Vector3d(0,0,0) 211 )); 212 optimizer.addVertex(pose); 213 214 //edges 215 int index=1; 216 vector<EdgeProjectXYZRGBDPoseOnly*> edges; 217 for(size_t i=0;i<pts1.size();++i) 218 { 219 EdgeProjectXYZRGBDPoseOnly* edge=new EdgeProjectXYZRGBDPoseOnly( 220 Eigen::Vector3d(pts2[i].x,pts2[i].y,pts2[i].z)); 221 edge->setId(index); 222 edge->setVertex(0,dynamic_cast<g2o::VertexSE3Expmap*>(pose)); 223 edge->setMeasurement(Eigen::Vector3d( 224 pts1[i].x,pts1[i].y,pts1[i].z)); 225 edge->setInformation(Eigen::Matrix3d::Identity()*1e4); 226 optimizer.addEdge(edge); 227 ++index; 228 edges.push_back(edge); 229 } 230 231 chrono::steady_clock::time_point t1=chrono::steady_clock::now(); 232 optimizer.setVerbose(true); 233 optimizer.initializeOptimization(); 234 optimizer.optimize(10); 235 chrono::steady_clock::time_point t2=chrono::steady_clock::now(); 236 chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1); 237 cout<<"optimization consts time: "<<time_used.count()<<" seconds."<<endl; 238 239 cout<<"after optimization: "<<endl; 240 cout<<"T= "<<endl<<Eigen::Isometry3d(pose->estimate()).matrix()<<endl; 241 }
運行結果：
本例得到的結果與我們前面SVD方法得到的位姿結果幾乎相同，說明SVD方法已經給出了最優化的解析解
不過，這里有一點疑問，在section 2.3.1的SVD中，先是利用PnP方法求出R，t,作為BA的初始值。而在本例中，SVD和BA是分開的兩個方法，那BA是如何設定初始值的呢？:Eigen::Matrix3d::Identity()，初始值設為單位矩陣