[SLAM]（番外篇）：一起做RGB-D SLAM(4)
阿新 • • 發佈：2018-12-03
本文轉自高翔老師的部落格，建議在學完教程的第二講後，插入學習，做到工程快速入門。
原文連結：https://www.cnblogs.com/gaoxiang12/p/4669490.html
上講回顧
上一講中，我們理解了如何利用影象中的特徵點，估計相機的運動。最後，我們得到了一個旋轉向量與平移向量。那麼讀者可能會問：這兩個向量有什麼用呢？在這一講裡，我們就要使用這兩個向量，把兩張影象的點雲給拼接起來，形成更大的點雲。
首先，我們把上一講的內容封裝進slamBase庫中，程式碼如下：
include/slamBase.h
1 // 幀結構 2 struct FRAME 3 { 4 cv::Mat rgb, depth; //該幀對應的彩色圖與深度圖 5 cv::Mat desp; //特徵描述子 6 vector<cv::KeyPoint> kp; //關鍵點 7 }; 8 9 // PnP 結果 10 struct RESULT_OF_PNP 11 { 12 cv::Mat rvec, tvec; 13 int inliers; 14 }; 15 16 // computeKeyPointsAndDesp 同時提取關鍵點與特徵描述子 17 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ); 18 19 // estimateMotion 計算兩個幀之間的運動 20 // 輸入：幀1和幀2, 相機內參 21 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );
我們把關鍵幀和PnP的結果都封成了結構體，以便將來別的程式呼叫。這兩個函式的實現如下：
src/slamBase.cpp
1 // computeKeyPointsAndDesp 同時提取關鍵點與特徵描述子 2 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ) 3 { 4 cv::Ptr<cv::FeatureDetector> _detector; 5 cv::Ptr<cv::DescriptorExtractor> _descriptor; 6 7 cv::initModule_nonfree(); 8 _detector = cv::FeatureDetector::create( detector.c_str() ); 9 _descriptor = cv::DescriptorExtractor::create( descriptor.c_str() ); 10 11 if (!_detector || !_descriptor) 12 { 13 cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl; 14 return; 15 } 16 17 _detector->detect( frame.rgb, frame.kp ); 18 _descriptor->compute( frame.rgb, frame.kp, frame.desp ); 19 20 return; 21 } 22 23 // estimateMotion 計算兩個幀之間的運動 24 // 輸入：幀1和幀2 25 // 輸出：rvec 和 tvec 26 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera ) 27 { 28 static ParameterReader pd; 29 vector< cv::DMatch > matches; 30 cv::FlannBasedMatcher matcher; 31 matcher.match( frame1.desp, frame2.desp, matches ); 32 33 cout<<"find total "<<matches.size()<<" matches."<<endl; 34 vector< cv::DMatch > goodMatches; 35 double minDis = 9999; 36 double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() ); 37 for ( size_t i=0; i<matches.size(); i++ ) 38 { 39 if ( matches[i].distance < minDis ) 40 minDis = matches[i].distance; 41 } 42 43 for ( size_t i=0; i<matches.size(); i++ ) 44 { 45 if (matches[i].distance < good_match_threshold*minDis) 46 goodMatches.push_back( matches[i] ); 47 } 48 49 cout<<"good matches: "<<goodMatches.size()<<endl; 50 // 第一個幀的三維點 51 vector<cv::Point3f> pts_obj; 52 // 第二個幀的影象點 53 vector< cv::Point2f > pts_img; 54 55 // 相機內參 56 for (size_t i=0; i<goodMatches.size(); i++) 57 { 58 // query 是第一個, train 是第二個 59 cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt; 60 // 獲取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！ 61 ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ]; 62 if (d == 0) 63 continue; 64 pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) ); 65 66 // 將(u,v,d)轉成(x,y,z) 67 cv::Point3f pt ( p.x, p.y, d ); 68 cv::Point3f pd = point2dTo3d( pt, camera ); 69 pts_obj.push_back( pd ); 70 } 71 72 double camera_matrix_data[3][3] = { 73 {camera.fx, 0, camera.cx}, 74 {0, camera.fy, camera.cy}, 75 {0, 0, 1} 76 }; 77 78 cout<<"solving pnp"<<endl; 79 // 構建相機矩陣 80 cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data ); 81 cv::Mat rvec, tvec, inliers; 82 // 求解pnp 83 cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers ); 84 85 RESULT_OF_PNP result; 86 result.rvec = rvec; 87 result.tvec = tvec; 88 result.inliers = inliers.rows; 89 90 return result; 91 }
此外，我們還實現了一個簡單的引數讀取類。這個類讀取一個引數的文字檔案，能夠以關鍵字的形式提供文字檔案中的變數：
include/slamBase.h
1 // 引數讀取類
2 class ParameterReader
3 {
4 public:
5 ParameterReader( string filename="./parameters.txt" )
6 {
7 ifstream fin( filename.c_str() );
8 if (!fin)
9 {
10 cerr<<"parameter file does not exist."<<endl;
11 return;
12 }
13 while(!fin.eof())
14 {
15 string str;
16 getline( fin, str );
17 if (str[0] == '#')
18 {
19 // 以‘＃’開頭的是註釋
20 continue;
21 }
22
23 int pos = str.find("=");
24 if (pos == -1)
25 continue;
26 string key = str.substr( 0, pos );
27 string value = str.substr( pos+1, str.length() );
28 data[key] = value;
29
30 if ( !fin.good() )
31 break;
32 }
33 }
34 string getData( string key )
35 {
36 map<string, string>::iterator iter = data.find(key);
37 if (iter == data.end())
38 {
39 cerr<<"Parameter name "<<key<<" not found!"<<endl;
40 return string("NOT_FOUND");
41 }
42 return iter->second;
43 }
44 public:
45 map<string, string> data;
46 };
它讀的引數檔案是長這個樣子的：
# 這是一個引數檔案
# 去你妹的yaml! 我再也不用yaml了！簡簡單單多好！
# part 4 裡定義的引數
detector=SIFT
descriptor=SIFT
good_match_threshold=4
# camera
camera.cx=325.5;
camera.cy=253.5;
camera.fx=518.0;
camera.fy=519.0;
camera.scale=1000.0;
嗯，引數檔案裡，以“變數名=值”的形式定義變數。以井號開頭的是註釋啦！是不是很簡單呢？
小蘿蔔：師兄你為何對yaml有一股強烈的怨念？
師兄：哎，不說了……總之實現簡單的功能，就用簡單的東西，特別是從教程上來說更應該如此啦。
現在，如果我們想更改特徵型別，就只需在parameters.txt檔案裡進行修改，不必編譯原始碼了。這對接下去的各種除錯都會很有幫助。
拼接點雲
點雲的拼接，實質上是對點雲做變換的過程。這個變換往往是用變換矩陣(transform matrix)來描述的：
T=[R3×3O1×3t3×11]∈R4×4T=[R3×3t3×1O1×31]∈R4×4
該矩陣的左上部分是一個3×33×3的旋轉矩陣，它是一個正交陣。右上部分是3×13×1的位移向量。左下是3×13×1的縮放向量，在SLAM中通常取成0，因為環境裡的東西不太可能突然變大變小（又沒有縮小燈）。右下角是個1. 這樣的一個陣可以對點或者其他東西進行齊次變換：
⎡⎣⎢⎢⎢y1y2y31⎤⎦⎥⎥⎥=T⋅⎡⎣⎢⎢⎢x1x2x31⎤⎦⎥⎥⎥[y1y2y31]=T⋅[x1x2x31]
由於變換矩陣結合了旋轉和縮放，是一種較為經濟實用的表達方式。它在機器人和許多三維空間相關的科學中都有廣泛的應用。PCL裡提供了點雲的變換函式，只要給定了變換矩陣，就能對移動整個點雲：
pcl::transformPointCloud( input, output, T );小蘿蔔：所以我們現在就是要把OpenCV裡的旋轉向量、位移向量轉換成這個矩陣嘍？
師兄：對！OpenCV認為旋轉矩陣RR，雖然有3×33×3那麼大，自由變數卻只有三個，不夠節省空間。所以在OpenCV裡使用了一個向量來表達旋轉。向量的方向是旋轉軸，大小則是轉過的弧度.
小蘿蔔：但是我們又把它變成了矩陣啊，這不就沒有意義了嗎！
師兄：呃，這個，確實如此。不管如何，我們先用羅德里格斯變換（Rodrigues）將旋轉向量轉換為矩陣，然後“組裝”成變換矩陣。程式碼如下：
src/joinPointCloud.cpp
1 /*************************************************************************
2 > File Name: src/jointPointCloud.cpp
3 > Author: Xiang gao
4 > Mail: [email protected]
5 > Created Time: 2015年07月22日 星期三 20時46分08秒
6 ************************************************************************/
7
8 #include<iostream>
9 using namespace std;
10
11 #include "slamBase.h"
12
13 #include <opencv2/core/eigen.hpp>
14
15 #include <pcl/common/transforms.h>
16 #include <pcl/visualization/cloud_viewer.h>
17
18 // Eigen !
19 #include <Eigen/Core>
20 #include <Eigen/Geometry>
21
22 int main( int argc, char** argv )
23 {
24 //本節要拼合data中的兩對影象
25 ParameterReader pd;
26 // 宣告兩個幀，FRAME結構請見include/slamBase.h
27 FRAME frame1, frame2;
28
29 //讀取影象
30 frame1.rgb = cv::imread( "./data/rgb1.png" );
31 frame1.depth = cv::imread( "./data/depth1.png", -1);
32 frame2.rgb = cv::imread( "./data/rgb2.png" );
33 frame2.depth = cv::imread( "./data/depth2.png", -1 );
34
35 // 提取特徵並計算描述子
36 cout<<"extracting features"<<endl;
37 string detecter = pd.getData( "detector" );
38 string descriptor = pd.getData( "descriptor" );
39
40 computeKeyPointsAndDesp( frame1, detecter, descriptor );
41 computeKeyPointsAndDesp( frame2, detecter, descriptor );
42
43 // 相機內參
44 CAMERA_INTRINSIC_PARAMETERS camera;
45 camera.fx = atof( pd.getData( "camera.fx" ).c_str());
46 camera.fy = atof( pd.getData( "camera.fy" ).c_str());
47 camera.cx = atof( pd.getData( "camera.cx" ).c_str());
48 camera.cy = atof( pd.getData( "camera.cy" ).c_str());
49 camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
50
51 cout<<"solving pnp"<<endl;
52 // 求解pnp
53 RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
54
55 cout<<result.rvec<<endl<<result.tvec<<endl;
56
57 // 處理result
58 // 將旋轉向量轉化為旋轉矩陣
59 cv::Mat R;
60 cv::Rodrigues( result.rvec, R );
61 Eigen::Matrix3d r;
62 cv::cv2eigen(R, r);
63
64 // 將平移向量和旋轉矩陣轉換成變換矩陣
65 Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
66
67 Eigen::AngleAxisd angle(r);
68 cout<<"translation"<<endl;
69 Eigen::Translation<double,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2));
70 T = angle;
71 T(0,3) = result.tvec.at<double>(0,0);
72 T(1,3) = result.tvec.at<double>(0,1);
73 T(2,3) = result.tvec.at<double>(0,2);
74
75 // 轉換點雲
76 cout<<"converting image to clouds"<<endl;
77 PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
78 PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );
79
80 // 合併點雲
81 cout<<"combining clouds"<<endl;
82 PointCloud::Ptr output (new PointCloud());
83 pcl::transformPointCloud( *cloud1, *output, T.matrix() );
84 *output += *cloud2;
85 pcl::io::savePCDFile("data/result.pcd", *output);
86 cout<<"Final result saved."<<endl;
87
88 pcl::visualization::CloudViewer viewer( "viewer" );
89 viewer.showCloud( output );
90 while( !viewer.wasStopped() )
91 {
92
93 }
94 return 0;
95 }
重點在於59至73行，講述了這個轉換的過程。
變換完畢後，我們就得到了拼合的點雲啦：
怎麼樣？是不是有點成就感了呢？
接下來的事……
至此，我們已經實現了一個只有兩幀的SLAM程式。然而，也許你還不知道，這已經是一個視覺里程計(Visual Odometry)啦！只要不斷地把進來的資料與上一幀對比，就可以得到完整的運動軌跡以及地圖了呢！
小蘿蔔：這聽著已經像是SLAM了呀！
師兄：嗯，要做完整的SLAM，還需要一些東西。以兩兩匹配為基礎的里程計有明顯的累積誤差，我們需要通過迴環檢測來消除它。這也是我們後面幾講的主要內容啦！
小蘿蔔：那下一講我們要做點什麼呢？
師兄：我們先講講關鍵幀的處理，因為把每個影象都放進地圖，會導致地圖規模增長地太快，所以需要關鍵幀技術。然後呢，我們要做一個SLAM後端，就要用到g2o啦！
課後作業
由於引數檔案可以很方便地調節，請你試試不同的特徵點型別，看看哪種型別比較符合你的心意。為此，最好在原始碼中加入顯示匹配圖的程式碼哦！
未完待續