第五講 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
前面幾個參數是相當直觀的:指定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∥)。它可以看成是位移與旋轉的範數加和。當這個數大於閾值max_norm時,我們就認爲匹配出錯了。
經過這三道工序處理後,vo的結果基本能保持正確啦。下面是一個gif圖片:
小蘿蔔:師兄!這效果相當不錯啊!
師兄:嗯,至少有點兒像樣啦,雖然問題還是挺多的。具體有哪些問題呢?我們留到下一講裏再說。各位同學也可以運行一下自己的代碼,看看結果哦。
tips:
- 當點雲出現時,可按5顯示顏色,然後按r重置視角,快速查看點雲;
- 可以調節parameters.txt中的voxel_grid值來設置點雲分辨率。0.01表示每1cm3的格子裏有一個點。
課後作業
請觀察vo的運行狀態並嘗試不同參數,總結它有哪些侷限性。
本講代碼: https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 數據鏈接見前面百度盤。