RGB-D SLAM學習總結(3)
第三講 特徵提取和匹配
本講主要實現影象的特徵提取，影象間的匹配，以及相機轉換矩陣的求解。
高博的部落格中提供了兩幀影象做測試，就是這兩幀影象。。。千萬不要另存為。。。
由於具體程式碼已經有詳細介紹，這裡只往slamBase裡新增方法。
另外在使用的slambase標頭檔案裡的方法時，一定也要像連結庫的標頭檔案一樣將其連結到編譯的可執行檔案中，就像這樣：
ADD_EXECUTABLE(detect_features detectFeatures.cpp) TARGET_LINK_LIBRARIES(detect_features slambase ${OpenCV_LIBS} ${PCL_LIBRARIES})
首先在slamBase.h中新增：
#include <opencv2/features2d/features2d.hpp> #include <opencv2/nonfree/nonfree.hpp> #include <opencv2/calib3d/calib3d.hpp> // 幀結構 struct FRAME { int frameID; cv::Mat rgb, depth; //該幀對應的彩色圖與深度圖 cv::Mat desp; //特徵描述子 vector<cv::KeyPoint> kp; //關鍵點 }; // PnP 結果 struct RESULT_OF_PNP { cv::Mat rvec, tvec; int inliers; }; // computeKeyPointsAndDesp 同時提取關鍵點與特徵描述子 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ); // estimateMotion 計算兩個幀之間的運動 // 輸入：幀1和幀2, 相機內參 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );
特別地，我們還添加了一個引數讀取類（非常值得學習的方法~），用來讀取一些引數：
// 引數讀取類 class ParameterReader { public: ParameterReader( string filename="../parameters.txt" ) { ifstream fin( filename.c_str() ); if (!fin) { cerr<<"parameter file does not exist."<<endl; return; } while(!fin.eof()) { string str; getline( fin, str ); if (str[0] == '#') { // 以‘＃’開頭的是註釋 continue; } int pos = str.find("="); if (pos == -1) continue; string key = str.substr( 0, pos ); string value = str.substr( pos+1, str.length() ); data[key] = value; if ( !fin.good() ) break; } } string getData( string key ) { map<string, string>::iterator iter = data.find(key); if (iter == data.end()) { cerr<<"Parameter name "<<key<<" not found!"<<endl; return string("NOT_FOUND"); } return iter->second; } public: map<string, string> data; };
它讀取的就是根目錄下的parameters.txt裡定義的引數：
# 這是一個引數檔案
# part 4 裡定義的引數
detector=SIFT
descriptor=SIFT
# 定義匹配距離的閾值，小於4倍最小距離則認為是好的匹配
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;那麼，相機內參也可以通過讀取此檔案得到，我們繼續向slamBase.h中新增方法（一個行內函數）去獲得相機內參：
inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
ParameterReader pd;
CAMERA_INTRINSIC_PARAMETERS camera;
camera.fx = atof( pd.getData( "camera.fx" ).c_str());
camera.fy = atof( pd.getData( "camera.fy" ).c_str());
camera.cx = atof( pd.getData( "camera.cx" ).c_str());
camera.cy = atof( pd.getData( "camera.cy" ).c_str());
camera.scale = atof( pd.getData( "camera.scale" ).c_str() );
return camera;
}這樣以後需要相機內參時直接呼叫getDefaultCamera()即可得到。
相應地，在slamBase.cpp中新增：
// computeKeyPointsAndDesp 同時提取關鍵點與特徵描述子
void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )
{
cv::Ptr<cv::FeatureDetector> _detector;
cv::Ptr<cv::DescriptorExtractor> _descriptor;
// 在使用SIFT/SURF特徵時要註冊演算法，不然create會返回空指標
cv::initModule_nonfree();
_detector = cv::FeatureDetector::create( detector.c_str() );
_descriptor = cv::DescriptorExtractor::create( descriptor.c_str() );
if (!_detector || !_descriptor)
{
cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl;
return;
}
_detector->detect( frame.rgb, frame.kp );
_descriptor->compute( frame.rgb, frame.kp, frame.desp );
return;
}
// estimateMotion 計算兩個幀之間的運動
// 輸入：幀1和幀2
// 輸出：rvec 和 tvec
RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera )
{
RESULT_OF_PNP result;
static ParameterReader pd;
vector< cv::DMatch > matches;
//cv::FlannBasedMatcher matcher;
cv::BFMatcher matcher;
matcher.match( frame1.desp, frame2.desp, matches );
//cout<<"find total "<<matches.size()<<" matches."<<endl;
vector< cv::DMatch > goodMatches;
double minDis = 9999;
double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );
for ( size_t i=0; i<matches.size(); i++ )
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
}
//cout << "min dis = " << minDis << endl;
if(minDis<10)minDis=10;
for ( size_t i=0; i<matches.size(); i++ )
{
if (matches[i].distance < good_match_threshold*minDis)
goodMatches.push_back( matches[i] );
}
//cout<<"good matches: "<<goodMatches.size()<<endl;
if (goodMatches.size() <= 5)
{
result.inliers = -1;
return result;
}
// 第一個幀的三維點
vector<cv::Point3f> pts_obj;
// 第二個幀的影象點
vector< cv::Point2f > pts_img;
// 相機內參
for (size_t i=0; i<goodMatches.size(); i++)
{
// query 是第一個, train 是第二個
cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;
// 獲取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ];
if (d == 0)
continue;
pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) );
// 將(u,v,d)轉成(x,y,z)
cv::Point3f pt ( p.x, p.y, d );
cv::Point3f pd = point2dTo3d( pt, camera );
pts_obj.push_back( pd );
}
if(pts_obj.size()==0 || pts_img.size()==0)
{
result.inliers = -1;
return result;
}
double camera_matrix_data[3][3] = {
{camera.fx, 0, camera.cx},
{0, camera.fy, camera.cy},
{0, 0, 1}
};
//cout<<"solving pnp"<<endl;
// 構建相機矩陣
cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
cv::Mat rvec, tvec, inliers;
// 求解pnp
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );
result.rvec = rvec;
result.tvec = tvec;
result.inliers = inliers.rows;
return result;
}特別注意的是：
添加了這兩段程式碼確保solvePnPRansac不報錯(不能傳入空的3D點和影象點)以及幀的可用性，實際上對於新來的幀，如果與參考幀匹配過少，那就表示這一幀是錯誤的(拍得不好)，不能新增進關鍵幀序列中。
if (goodMatches.size() <= 5)
{
result.inliers = -1;
return result;
}
if(pts_obj.size()==0 || pts_img.size()==0)
{
result.inliers = -1;
return result;
}這樣特徵提取和匹配就完成啦~同時我們也獲得了相機的運動資訊。
solvePnPRansac方法補充說明：
- Ransac介紹：
轉自：http://blog.csdn.net/u010128736/
隨機抽樣一致演算法（RANdom SAmple Consensus,RANSAC）,採用迭代的方式從一組包含離群的被觀測資料中估算出數學模型的引數。維基百科中的RANSAC該演算法最早由Fischler和Bolles於1981年提出。RANSAC演算法假設資料中包含正確資料和異常資料（或稱為噪聲）。正確資料記為內點（inliers），異常資料記為外點（outliers）。同時RANSAC也假設，給定一組正確的資料，存在可以計算出符合這些資料的模型引數的方法。該演算法核心思想就是隨機性和假設性，隨機性是根據正確資料出現概率去隨機選取抽樣資料，根據大數定律，隨機性模擬可以近似得到正確結果。假設性是假設選取出的抽樣資料都是正確資料，然後用這些正確資料通過問題滿足的模型，去計算其他點，然後對這次結果進行一個評分。
通過例項對演算法基本思想進行描述：
（1）首先，我們知道要得到一個直線模型，我們需要兩個點唯一確定一個直線方程。所以第一步我們隨機選擇兩個點。
（2）通過這兩個點，我們可以計算出這兩個點所表示的模型方程y=ax+b。
（3）我們將所有的資料點套到這個模型中計算誤差。
（4）我們找到所有滿足誤差閾值的點，第4幅圖中可以看到，只有很少的點支援這個模型。
（5）然後我們再重複（1）~（4）這個過程，直到達到一定迭代次數後，我們選出那個被支援的最多的模型，作為問題的解。
- PnP方法介紹：
參考：《視覺SLAM十四講》7.7
PnP(Perspective-n-Point)是求解3D到2D點對運動的方法。它描述了當知道n個3D空間點及其投影位置時，如何估計相機的位姿。PnP有很多種求解方法，例如，用3對點估計位姿的P3P、直接線性變換(DLT)、EPnP(Efficient PnP)、UPnP，等等。此外，還能用非線性優化的方法，構建最小二乘問題並迭代求解，也就是萬金油式的Bundle Adjustment。
- solvePnPRansac函式介紹：
參考：opencv線上API
solvePnPRansac(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100, OutputArray inliers=noArray(), int flags=ITERATIVE )
| objectPoints | 3D空間點。 |
| imagePoints | 3D空間點的投影位置。 |
| cameraMatrix | 相機內參。 |
| distCoeffs | 相機畸變係數，4,5,8個係數構成的矩陣或為零失真矩陣。 |
| rvec | 返回旋轉量。 |
| tvec | 返回平移量。 |
| useExtrinsicGuess |
如果為真(1)，則函式使用提供的rvec和tvec值分別作為旋轉和平移向量的初始近似值，並進一步優化它們。 emmmm...沒懂這個值。 |
| iterationsCount | 迭代次數。 |
| reprojectionError | RANSAC過程中使用的內閾值。引數值是觀測點和計算點投影之間的最大允許距離，將其視為內點。 |
| minInliersCount | 內點數目。如果在某個階段的演算法找到比這個值更多的內點，它就結束了。 |
| inliers | 輸出向量，包含物件點和影象點中的內點索引。 |
| flags | 使用的PnP方法，見下表。 |
| CV_ITERATIVE | 迭代法是基於Levenberg Marquardt優化的。在這種情況下，該函式找到這樣的姿態，該姿態使重新投影誤差最小化，即所觀察的影象點與3D投影點之間的平方距離之和（BA — 最小二乘）。 |
| CV_P3P | 方法基於X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”。在這種情況下，函式需要精確的四組物件和影象點。 |
| CV_EPNP | F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”一文中介紹了此方法。 |