注：這是我在知乎寫的文章，現搬運至此。原文鏈接：https://zhuanlan.zhihu.com/p/79386524
在基於特征的SLAM中，特征匹配是一個非常關鍵的問題，為了防止錯誤匹配對后端的估計造成影響，工程師們研究出了很多魯棒估計算法，在視覺SLAM中，目前比較流行兩種方式，一種是在SLAM后端優化中添加核函數防止錯誤匹配權重過大，另一種就是本文要介紹的RANSAC，中文翻譯為隨機采樣一致性算法。本文的 RANSAC 算法是利用DLT求解PNP實現的最基礎版本，outlier剔除效果不錯，但是迭代次數過多，編寫的代碼和對RANSAC的理解還有許多不足，僅供參考。
RANSAC 原理簡單，知乎上也有很多老鐵介紹過，在此再簡單說下，要想更加具體的了解可以參閱《機器人學中的狀態估計》五章第3節。
最基礎的RANSAC包括五個步驟：
- 從所有原始數據中隨機選取一個最小子集(如果求解PNP問題，那么顯然可以選取3個點(P3P)。但是我使用的是dlt方式，所以選取的是6個點,這種方式僅供參考，不過應該沒什么問題，OpenCV中solvePnPRansac()也可以選擇Epnp解法，需要4個點對)。
- 使用這個子集擬合一個模型(本文中就是通過DLT求解PNP問題)。
- 根據上一步求解出的模型去判斷所有數據，將數據分為outlier(外點)和inlier(內點)，如果內點太少，那么返回第一步。其中本文求解的是PNP問題，使用的是重投影誤差來判斷內外點。
- 根據划分出的所有內點，重新擬合模型。
- 根據最新擬合出的模型，判斷內外點數目，如果達到最終要求，那么跳出循環，如果內點太少，那么返回第一步。如果內點數目有增加但是還沒有達到最終要求，那么返回第二步。
我的代碼編寫的比較簡單，其中DLT部分參考了此博客：[PnP] PnP問題之DLT解法
我使用cuSift這個庫提取sift特征點，然后使用庫中自帶的暴力匹配，先看原始結果：
再看使用我們的RANSAC剔除后的結果：
可以看出來，雖說我代碼編的不好，不過還是有一定效果的,因為我使用了cuSift庫,這些代碼在大部分人的電腦上應該是運行不了的，所以代碼僅供參考。
首先隨機選取6個點，這可是真真的隨機，沒有加任何額外處理。
1 deque<int> getMaxScorePoints(SiftData& sift,vector<double>& pt3DDepth , int nums = 6){ 2 deque<int> ans; 3 while(ans.size()<nums){ 4 struct timeval tv; 5 gettimeofday(&tv,NULL); 6 srand((int)(tv.tv_usec)); // 產生隨機種子 7 int i = rand()%sift.numPts; 8 if(ans.size()<nums){ 9 ans.push_back(i); 10 } 11 return ans; 12 }
然后我們進行RANSAC：
1 void RANSAC(SiftData& sift,vector<double>& pt3DDepth,int nums = 6){ 2 auto start = cv::getTickCount(); 3 Eigen::Matrix3d R ; 4 Eigen::Vector3d t ; 5 /// 連續3次滿足條件，那么迭代終止,這里還有需要改進的空間 6 for(int it=0;it<3;++it) { 7 deque<int> goodPoints; 8 /// 如果是第一次迭代,那么隨機選取6個點 9 if(it==0) 10 goodPoints = getMaxScorePoints(sift, pt3DDepth,nums); 11 else { 12 /// state數組記錄所有特征點的狀態,內點為true，外點為false 13 /// 某次迭代后，使用所有內點重新估計模型 14 for(int i=0;i<sift.numPts;++i){ 15 if (state[i]) 16 goodPoints.push_back(i); 17 } 18 } 19 std::cout << "goodPoints size:" << goodPoints.size() << endl; 20 /// 對內點使用DLT解法求解,R,t為結果 21 solvePnpByDLT(sift,pt3DDepth,goodPoints,R,t); 22 /// 計算重投影誤差，剔除外點 23 double goodPoint = 0.0; 24 for (int i=0;i<sift.numPts;++i) { 25 auto siftPoint = sift.h_data[i]; 26 /// 我們觀測的像素坐標 27 const double &u = siftPoint.match_xpos; 28 const double &v = siftPoint.match_ypos; 29 /// 計算3d點的世界坐標 30 const double &z = pt3DDepth[i]; 31 const double &x = (siftPoint.xpos - cx) * z / fx; 32 const double &y = (siftPoint.ypos - cy) * z / fy; 33 /// 求得的是特征點世界坐標系下的坐標，要轉化到相機坐標系下 34 Eigen::Vector3d ans(x,y,z); 35 ans = R.inverse()*(ans-t); 36 /// 重投影 37 double u_reprojection = (fx * ans(0) / ans(2)) + cx; 38 double v_reprojection = (fy * ans(1) / ans(2)) + cy; 39 /// 誤差 40 double dis = sqrt((u - u_reprojection)*(u - u_reprojection) 41 + (v - v_reprojection)*(v - v_reprojection)); 42 /// 誤差小於35.....這個閾值好像有點大，因為DLT估計的姿態特別不准確... 43 /// 根據結果設置state的狀態 44 if (abs(dis) > 35) state[i] = false; 45 else { 46 goodPoint += 1.0; 47 state[i] = true; 48 } 49 } 50 cout<<"good count:"<<goodPoint<<endl; 51 /// 如果本次估計的結果滿足一個比率，那么繼續 52 /// 否則重新迭代 53 if(goodPoint / sift.numPts<ee[it]){ 54 cout<<"估計錯誤，重新估算!"<<endl; 55 it = -1; 56 for(int i=0;i<sift.numPts;++i){ 57 auto p = sift.h_data[i]; 58 if (pt3DDepth[i] < 0.0 || p.score<0.9 || p.ambiguity>0.9) { 59 state[i] = false; 60 } else state[i] = true; 61 62 } 63 } 64 } 65 cout << "R:" << endl << R << endl; 66 cout << "t:" << endl << t << endl; 67 double time = (cv::getTickCount() - start) / cv::getTickFrequency(); 68 std::cout<<"Finish ......t =:"<<time*1000<<"ms"<<std::endl; 69 }
其中的solvePnpByDLT(),參考了這位老鐵寫的文章：[PnP] PnP問題之DLT解法，感謝分享！我就不貼代碼了。
總結與討論
1.RANSAC原理很簡單，代碼編寫更簡單，但是從我的測試來看，如果我不對比配的特征點做任何過濾，真正的隨機取點，那么算法的收斂速度完全就取決於這隨機的6個點的好壞，收斂非常之慢，往往迭代幾百次才收斂。如果我們希望加快收斂速度，這個所謂的“隨機”應該還是有很多方式的，最起碼如果6個特征點非常集中，那么我們肯定要重新取點，因為過於集中的特征點會帶來非常差的估計結果，這次迭代必定是發散的。不過這樣似乎就不叫隨機了。當然，這僅僅代表我自己的觀點，讀者還要選擇性的吸收。我對RANSAC的理解還有待加強。