注：这是我在知乎写的文章，现搬运至此。原文链接：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的理解还有待加强。