RANSAC算法拟合平面实现(附代码c++)

机械视觉3D检测项目中遇到的一些问题：

项目场景：

工业视觉双目条纹检测：

项目场景：示例:条纹投影检测芯片引脚高度

问题描述：

在做双目视觉，条纹投影获取点云图后，会遇到噪声点比较多的问题，并且这些噪声点是由于反光引起的，所以没有办法有效剔除噪声点，就会影响我们计算3D点的准确度。 在获取到点云数据后，我进行了滤波，然后将我认为没有异常的点拟合了一个平面，然后再去求我想要的某个point(x,y,z)到平面的距离，这就计算出了某个点相对于平面的距离，这也是项目中用来求高度比较通用的算法。

例如：噪声点剔除不完全，某些极大噪声点会拉斜拟合的平面。

原因分析：

出错原因：是因为我想剔除噪声点，但是很难找到能满足剔除所有噪声点的条件。

比如我设置了，高度剔除，像素剔除，但是这都不能满足噪声点剔除条件，并且噪声点是完全随机的，不可避免的。

从本质上，就不该采用剔除噪声点去拟合平面，这时我找到了RANSAC算法，可谓是完全避开噪声点，

此算法只用三个点去拟合平面，并且设置一个阈值距离，看看有多少个点可以满足这个距离，就记下这个number，然后可以迭代n次，这n次中，number最多的那次拟合出的平面，就可以保证是你最想要的平面。

由于只用了三个点来拟合平面，所有完全不担心噪声点来拉高平面。

活不多说，上代码。（c++）

解决方案：

RANSAC算法：

原理：---------------基于ransac算法平面检测：

1、确定迭代次数；

2、在迭代次数内：

2.1 随机选择三个点组成平面（判断三个点是否共线）；

2.2 构造坐标矩阵；

2.3 求平面方程；

2.4 求所有点到平面的距离；

2.5 统计inlier个数（距离小于阈值）；

3、迭代选择inlier个数最多的平面。

已知三个点坐标为P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3)，求过他们的平面方程：

设方程为A(x - x1) + B(y - y1) + C(z - z1) = 0 (点法式) (也可设为过另外两个点)，则有

A = (y3 - y1)(z3 - z1) - (z2 -z1)(y3 - y1);

B = (x3 - x1)(z2 - z1) - (x2 - x1)(z3 - z1);

C = (x2 - x1)(y3 - y1) - (x3 - x1)(y2 - y1);

//"ransac.h"
void ransac( std::vector<cv::Point3f>& pts_3d, int max_iter, float threshold);

//需要配置opencv，用到了opencv的Point3f
#include"ransac.h"
void ransac( std::vector<cv::Point3f>& pts_3d, int max_iter, float threshold)
{
  srand(time(0)); //随机种子
  int size_old = 3;
  double a, b, c, d; //平面法向量系数
  while (--max_iter) //设置循环的次数
  {
    vector<int> index;
    for (int k = 0; k < 3; ++k)
    {
      index.push_back(rand() % pts_3d.size()); //随机选取三个点 
    }   
    auto idx = index.begin();
    double x1 = pts_3d.at(*idx).x, y1 = pts_3d.at(*idx).y, z1 = pts_3d.at(*idx).z; 
    ++idx;
    double x2 = pts_3d.at(*idx).x, y2 = pts_3d.at(*idx).y, z2 = pts_3d.at(*idx).z; 
    ++idx;
    double x3 =pts_3d.at(*idx).x, y3 = pts_3d.at(*idx).y, z3 = pts_3d.at(*idx).z; 
    a = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
    b = (z2 - z1)*(x3 - x1) - (x2 - x1)*(z3 - z1);
    c = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1);
    d = -(a*x1 + b*y1 + c*z1);
    for (auto iter = pts_3d.begin(); iter != pts_3d.end(); ++iter)
    { 
      double dis = fabs(a*iter->x + b*iter->y + c*iter->z + d) / sqrt(a*a + b*b + c*c);//点到平面的距离公式
      if (dis < threshold)  index.push_back(iter - pts_3d.begin());
    }
    //更新集合
    if (index.size() > size_old)
    {
      size_old = index.size();
    }
    index.clear();
  }
  cout << a << " " << b << " " << c << " " << d << endl;  
}

附上一张图片看看效果，

绿色线：RANSA拟合结果;红色线:最小二乘法拟合结果;蓝色线:期望的理想结果

图片转载自：https://blog.csdn.net/leonardohaig/article/details/104570965?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163089018316780255218394%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=163089018316780255218394&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allfirst_rank_ecpm_v1~rank_v29-10-104570965.pc_search_ecpm_flag&utm_term=ransac%E7%AE%97%E6%B3%95%E6%8B%9F%E5%90%88%E5%B9%B3%E9%9D%A2&spm=1018.2226.3001.4187

代码参考：https://blog.csdn.net/taifyang/article/details/117636554?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163089058616780274150003%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=163089058616780274150003&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allfirst_rank_ecpm_v1~rank_v29-6-117636554.pc_search_ecpm_flag&utm_term=Ransac%E6%8B%9F%E5%90%88%E5%B9%B3%E9%9D%A2&spm=1018.2226.3001.4187