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

2021-09-06 10:32:05 阅读：262 来源： 互联网

标签：RANSAC idx c++ 拟合 y1 x1 pts z1 3d

机械视觉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

标签：RANSAC,idx,c++,拟合,y1,x1,pts,z1,3d
来源： https://blog.csdn.net/weixin_45341885/article/details/120127451