[SLAM]（番外篇）：一起做RGB-D SLAM(5)
本文轉自高翔老師的部落格，建議在學完教程的第二講後，插入學習，做到工程快速入門。
原文連結：https://www.cnblogs.com/gaoxiang12/p/4719156.html
Visual Odometry (視覺里程計)
讀者朋友們大家好，又到了我們開講rgbd slam的時間了。由於前幾天博主在忙開會拍婚紗照等一系列亂七八糟的事情，這一講稍微做的慢了些，先向讀者們道個歉！
上幾講中，我們詳細講了兩張影象間的匹配與運動估計。然而一個實際的機器人總不可能只有兩個影象資料吧？那該多麼寂寞呀。所以，本講開始，我們要處理一個視訊流，包含八百左右的資料啦。這才像是在做SLAM嘛！
小蘿蔔：那我們去哪裡下載這些資料呢？
師兄：可以到我的百度雲裡去：http://yun.baidu.com/s/1i33uvw5
因為有點大（400多M），我就沒有傳到git上。不然執行前四講的程式碼就要下一堆東西啦。開啟這個資料集，你會看到裡頭有 和 兩個資料夾，分別是RGB圖與深度圖。前幾講的資料也是取自這裡的哦。
小蘿蔔：這算不算師兄你在偷懶呢？
師兄：呃，這個，總、總之，我們這裡暫時先用這些資料啦。它們取自nyuv2資料集：http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html 這可是一個國際上認可的，相當有名的資料集哦。如果你想要跑自己的資料，當然也可以，不過需要你進行一些預處理啦。
本講中，我們利用前幾講寫好的程式碼，完成一個視覺里程計(visual odometry)的編寫。什麼是視覺里程計呢？簡而言之，就是把新來的資料與上一幀進行匹配，估計其運動，然後再把運動累加起來的東西。畫成示意圖的話，就是下面這個樣子：
師兄：大家看懂了不？這實際上和濾波器很像，通過不斷的兩兩匹配，估計機器人當前的位姿，過去的就給丟棄了。這個思路比較簡單，實際當中也比較有效，能夠保證區域性運動的正確性。下面我們來實現一下visual odometry。
小蘿蔔：道理我是明白了，可是師兄你這畫風究竟是哪個年代的啊……
準備工作
為了保證程式碼的簡潔，我們要把以前做過的東西封裝成函式，寫在slamBase.cpp中，以便將來呼叫。（不過，由於是演算法性質的內容，就不封成c++的物件了）。
首先工具函式：將cv的旋轉向量與位移向量轉換為變換矩陣，型別為Eigen::Isometry3d；
src/slamBase.cpp
1 // cvMat2Eigen
2 Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )
3 {
4 cv::Mat R;
5 cv::Rodrigues( rvec, R );
6 Eigen::Matrix3d r;
7 cv::cv2eigen(R, r);
8
9 // 將平移向量和旋轉矩陣轉換成變換矩陣
10 Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
11
12 Eigen::AngleAxisd angle(r);
13 Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
14 T = angle;
15 T(0,3) = tvec.at<double>(0,0);
16 T(1,3) = tvec.at<double>(0,1);
17 T(2,3) = tvec.at<double>(0,2);
18 return T;
19 }
另一個函式：將新的幀合併到舊的點雲裡：
1 // joinPointCloud
2 // 輸入：原始點雲，新來的幀以及它的位姿
3 // 輸出：將新來幀加到原始幀後的影象
4 PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera )
5 {
6 PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );
7
8 // 合併點雲
9 PointCloud::Ptr output (new PointCloud());
10 pcl::transformPointCloud( *original, *output, T.matrix() );
11 *newCloud += *output;
12
13 // Voxel grid 濾波降取樣
14 static pcl::VoxelGrid<PointT> voxel;
15 static ParameterReader pd;
16 double gridsize = atof( pd.getData("voxel_grid").c_str() );
17 voxel.setLeafSize( gridsize, gridsize, gridsize );
18 voxel.setInputCloud( newCloud );
19 PointCloud::Ptr tmp( new PointCloud() );
20 voxel.filter( *tmp );
21 return tmp;
22 }
另外，在parameters.txt中，我們增加了幾個引數，以便調節程式的效能：
# part 5
# 資料相關
# 起始與終止索引
start_index=1
end_index=700
# 資料所在目錄
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
# 點雲解析度
voxel_grid=0.02
# 是否實時視覺化
visualize_pointcloud=yes
# 最小匹配數量
min_good_match=10
# 最小內點
min_inliers=5
# 最大運動誤差
max_norm=0.3
前面幾個引數是相當直觀的：指定RGB圖與深度圖所在的目錄，起始與終止的影象索引（也就是第1張到第700張的slam啦）。後面幾個引數，會在後面進行解釋。
實現VO
最後，利用之前寫好的工具函式，實現一個VO:
src/visualOdometry.cpp
1 /*************************************************************************
2 > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
3 > Author: xiang gao
4 > Mail: [email protected]
5 > Created Time: 2015年08月01日 星期六 15時35分42秒
6 ************************************************************************/
7
8 #include <iostream>
9 #include <fstream>
10 #include <sstream>
11 using namespace std;
12
13 #include "slamBase.h"
14
15 // 給定index，讀取一幀資料
16 FRAME readFrame( int index, ParameterReader& pd );
17 // 度量運動的大小
18 double normofTransform( cv::Mat rvec, cv::Mat tvec );
19
20 int main( int argc, char** argv )
21 {
22 ParameterReader pd;
23 int startIndex = atoi( pd.getData( "start_index" ).c_str() );
24 int endIndex = atoi( pd.getData( "end_index" ).c_str() );
25
26 // initialize
27 cout<<"Initializing ..."<<endl;
28 int currIndex = startIndex; // 當前索引為currIndex
29 FRAME lastFrame = readFrame( currIndex, pd ); // 上一幀資料
30 // 我們總是在比較currFrame和lastFrame
31 string detector = pd.getData( "detector" );
32 string descriptor = pd.getData( "descriptor" );
33 CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
34 computeKeyPointsAndDesp( lastFrame, detector, descriptor );
35 PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
36
37 pcl::visualization::CloudViewer viewer("viewer");
38
39 // 是否顯示點雲
40 bool visualize = pd.getData("visualize_pointcloud")==string("yes");
41
42 int min_inliers = atoi( pd.getData("min_inliers").c_str() );
43 double max_norm = atof( pd.getData("max_norm").c_str() );
44
45 for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
46 {
47 cout<<"Reading files "<<currIndex<<endl;
48 FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
49 computeKeyPointsAndDesp( currFrame, detector, descriptor );
50 // 比較currFrame 和 lastFrame
51 RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
52 if ( result.inliers < min_inliers ) //inliers不夠，放棄該幀
53 continue;
54 // 計算運動範圍是否太大
55 double norm = normofTransform(result.rvec, result.tvec);
56 cout<<"norm = "<<norm<<endl;
57 if ( norm >= max_norm )
58 continue;
59 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
60 cout<<"T="<<T.matrix()<<endl;
61
62 //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );
63 cloud = joinPointCloud( cloud, currFrame, T, camera );
64
65 if ( visualize == true )
66 viewer.showCloud( cloud );
67
68 lastFrame = currFrame;
69 }
70
71 pcl::io::savePCDFile( "data/result.pcd", *cloud );
72 return 0;
73 }
74
75 FRAME readFrame( int index, ParameterReader& pd )
76 {
77 FRAME f;
78 string rgbDir = pd.getData("rgb_dir");
79 string depthDir = pd.getData("depth_dir");
80
81 string rgbExt = pd.getData("rgb_extension");
82 string depthExt = pd.getData("depth_extension");
83
84 stringstream ss;
85 ss<<rgbDir<<index<<rgbExt;
86 string filename;
87 ss>>filename;
88 f.rgb = cv::imread( filename );
89
90 ss.clear();
91 filename.clear();
92 ss<<depthDir<<index<<depthExt;
93 ss>>filename;
94
95 f.depth = cv::imread( filename, -1 );
96 return f;
97 }
98
99 double normofTransform( cv::Mat rvec, cv::Mat tvec )
100 {
101 return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
102 }
其實一個VO也就一百行的程式碼，相信大家很快就能讀懂的。我們稍加解釋。
- FRAME readFrame( int index, ParameterReader& pd ) 是讀取幀資料的函式。告訴它我要讀第幾幀的資料，它就會乖乖的把資料給找出來，返回一個FRAME結構體。
- 在得到匹配之後，我們判斷了匹配是否成功，並把失敗的資料丟棄。為什麼這樣做呢？因為之前的演算法，對於任意兩張影象都能做出一個結果。對於無關的影象，就明顯是不對的。所以要去除匹配失敗的情形。
- 如何檢測匹配失敗呢？我們採用了三個方法：
-
- 去掉goodmatch太少的幀，最少的goodmatch定義為：
min_good_match=10 - 去掉solvePnPRASNAC裡，inlier較少的幀，同理定義為：
min_inliers=5 - 去掉求出來的變換矩陣太大的情況。因為假設運動是連貫的，兩幀之間不會隔的太遠：
max_norm=0.3
- 去掉goodmatch太少的幀，最少的goodmatch定義為：
如何知道兩幀之間不隔太遠呢？我們計算了一個度量運動大小的值：∥Δt∥+min(2π−∥r∥,∥r∥)‖Δt‖+min(2π−‖r‖,‖r‖)。它可以看成是位移與旋轉的範數加和。當這個數大於閾值max_norm時，我們就認為匹配出錯了。
經過這三道工序處理後，vo的結果基本能保持正確啦。下面是一個gif圖片：
小蘿蔔：師兄！這效果相當不錯啊！
師兄：嗯，至少有點兒像樣啦，雖然問題還是挺多的。具體有哪些問題呢？我們留到下一講裡再說。各位同學也可以執行一下自己的程式碼，看看結果哦。
tips：
- 當點雲出現時，可按5顯示顏色，然後按r重置視角，快速檢視點雲；
- 可以調節parameters.txt中的voxel_grid值來設定點雲解析度。0.01表示每1cm3cm3的格子裡有一個點。
課後作業
請觀察vo的執行狀態並嘗試不同引數，總結它有哪些侷限性。
本講程式碼： https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 資料鏈接見前面百度盤。
TIPS
- 如果在編譯時期出現Link錯誤，請檢查你是否連結到了PCL的visualization模組。如果實在不確定，就照著github原始碼抄一遍。
- 在運動時期，由於存在兩張影象完全一樣的情況，導致匹配時距離為0。由於本節程式的設定，這種情況會被當成沒有匹配，導致VO丟失。請你自己fix一下這個bug，我在下一節當中也進行了檢查。