視覺里程計（VO）的實現(一)

代碼基於高翔博士（《視覺SLAM十四講》作者）博客，並在在ubuntu 16.0.4 +opencv3.4.1 環境下適配。
原博客網址：https://www.cnblogs.com/gaoxiang12/p/4659805.html

1.具體說明：

1. ORB算法適配
前輩使用的應該是opencv2版本，所以使用ORB算法的代碼爲:

cv::Ptr<cv::FeatureDetector> detector;
cv::Ptr<cv::DescriptorExtractor> descriptor;
etector = cv::FeatureDetector::create("ORB");
descriptor = cv::DescriptorExtractor::create("ORB"); 
vector< cv::KeyPoint > kp1, kp2; //關鍵點
detector->detect( rgb1, kp1 );  //提取關鍵點
detector->detect( rgb2, kp2 ); 

詳情參見前輩github：
https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/blob/master/part III/src/detectFeatures.cpp

opencv3版本下的適配代碼爲：

cv::Ptr<cv::ORB> orb = cv::ORB::create();
vector< cv::KeyPoint > kp1, kp2; //關鍵點
orb->detect(rgb1, kp1);
orb->detect(rgb2, kp2);

我的github：https://github.com/raopei/mySlam
2. 修改錯誤參數
使用 cv::solvePnPRansac函數求解相機位姿時參數出錯。（沒有驗證opencv2版本下是否正確，在opencv 3.4.1下報錯）
前輩代碼：

cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );

其中inliers參數前一個參數是有誤的（我使用的opencv版本下報錯）

OpenCV(3.4.1) Error: Assertion failed (npoints >= 4 && npoints == std::max(ipoints.checkVector(2, 5), ipoints.checkVector(2, 6))) in solvePnPRansac, file /home/raopei/音樂/opencv-3.4.1/modules/calib3d/src/solvepnp.cpp, line 253
terminate called after throwing an instance of 'cv::Exception'

轉到函數cv::solvePnPRansac的聲明下發現1.0和inliers參數之間的100錯誤的，應該設置成（0,1）之間的數字。如：

cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 0.99, inliers );

3.數據路徑問題
原代碼採用相對路徑：

cv::Mat rgb1 = cv::imread( "./data/rgb1.png");

經過測試在我所用的環境下無法讀入圖像。
更改爲絕對路徑後正常。如:

cv::Mat rgb1 = cv::imread( "/home/raopei/mySlam/data/rgb1.png");

運行截圖：