1. 程式人生 > >一起做RGB-D SLAM (5)

一起做RGB-D SLAM (5)

第五講 Visual Odometry (視覺里程計)

 2016.11 更新

  • 把原文的SIFT替換成了ORB,這樣你可以在沒有nonfree模組下使用本程式了。
  • 去掉了cv::cv2Eigen函式,因為有些讀者找不到這個函式。
  • 檢查了minDis為零的情況。
  • 之前t的訪問時,行和列顛倒了,會對結果產生一定影響,現在修正了。
  • 請以現在的github上原始碼為準。

  讀者朋友們大家好,又到了我們開講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結構體。
  • 在得到匹配之後,我們判斷了匹配是否成功,並把失敗的資料丟棄。為什麼這樣做呢?因為之前的演算法,對於任意兩張影象都能做出一個結果。對於無關的影象,就明顯是不對的。所以要去除匹配失敗的情形。
  • 如何檢測匹配失敗呢?我們採用了三個方法:
    1. 去掉goodmatch太少的幀,最少的goodmatch定義為:
      min_good_match=10
    2. 去掉solvePnPRASNAC裡,inlier較少的幀,同理定義為:
      min_inliers=5
    3. 去掉求出來的變換矩陣太大的情況。因為假設運動是連貫的,兩幀之間不會隔的太遠:
      max_norm=0.3

  如何知道兩幀之間不隔太遠呢?我們計算了一個度量運動大小的值:‖Δt‖+min(2π−‖r‖,‖r‖)

。它可以看成是位移與旋轉的範數加和。當這個數大於閾值max_norm時,我們就認為匹配出錯了。

  經過這三道工序處理後,vo的結果基本能保持正確啦。下面是一個gif圖片:

  小蘿蔔:師兄!這效果相當不錯啊!

  師兄:嗯,至少有點兒像樣啦,雖然問題還是挺多的。具體有哪些問題呢?我們留到下一講裡再說。各位同學也可以執行一下自己的程式碼,看看結果哦。

 tips:

  1. 當點雲出現時,可按5顯示顏色,然後按r重置視角,快速檢視點雲;
  2. 可以調節parameters.txt中的voxel_grid值來設定點雲解析度。0.01表示每1cm3
  1. 的格子裡有一個點。

課後作業

  請觀察vo的執行狀態並嘗試不同引數,總結它有哪些侷限性。

  本講程式碼: https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 資料鏈接見前面百度盤。

TIPS

  • 如果在編譯時期出現Link錯誤,請檢查你是否連結到了PCL的visualization模組。如果實在不確定,就照著github原始碼抄一遍。
  • 在運動時期,由於存在兩張影象完全一樣的情況,導致匹配時距離為0。由於本節程式的設定,這種情況會被當成沒有匹配,導致VO丟失。請你自己fix一下這個bug,我在下一節當中也進行了檢查。