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);

实现：

/**
 * @description:	RANSAC算法拟合平面
 * @param cloud		输入点云
 * @param max_iter	最大迭代次数
 * @param threshold	距离阈值
 */
void ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int max_iter, float threshold)
{
	srand(time(0)); 
	int size_old = 3;
	double a_final, b_final, c_final, d_final; 

	while (--max_iter) 
	{
		std::vector<int> index;
		for (int k = 0; k < 3; ++k)
		{
			index.push_back(rand() % cloud->size());
		}		
		auto idx = index.begin();
		double x1 = cloud->points[*idx].x, y1 = cloud->points[*idx].y, z1 = cloud->points[*idx].z; 
		++idx;
		double x2 = cloud->points[*idx].x, y2 = cloud->points[*idx].y, z2 = cloud->points[*idx].z;
		++idx;
		double x3 = cloud->points[*idx].x, y3 = cloud->points[*idx].y, z3 = cloud->points[*idx].z;

		double a = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
		double b = (z2 - z1)*(x3 - x1) - (x2 - x1)*(z3 - z1);
		double c = (x2 - x1)*(y3 - y1) - (y2 - y1)*(x3 - x1);
		double d = -(a*x1 + b*y1 + c*z1);

		for (auto iter = cloud->begin(); iter != cloud->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 - cloud->begin());
		}

		if (index.size() > size_old)
		{
			size_old = index.size();
			a_final = a; b_final = b; c_final = c; d_final = d;
		}
	}
    std::cout << a_final << " " << b_final << " " << c_final << " " << d_final << std::endl;
}

代码传送门：https://github.com/taifyang/PCL-algorithm