最小二乘法拟合圆原理与在点云数据中的应用

最小二乘法拟合圆原理

我把原理和公式手推了一遍，如下：
参考blog：
最小二乘法拟合圆公式推导及其实现
最小二乘法拟合圆

C++代码实现with PCL库

bool circleFit(pcl::PointCloud<pcl::PointXYZRGB>::Ptr points_cloud, double *circle_X, double *circle_Y, double *circle_R)
{
	int N = points_cloud->size();
	if (N < 3)return false;

	double sum_X1 = 0.0;
	double sum_Y1 = 0.0;
	double sum_X2 = 0.0;
	double sum_Y2 = 0.0;
	double sum_X3 = 0.0;
	double sum_Y3 = 0.0;
	double sum_X1Y1 = 0.0;
	double sum_X1Y2 = 0.0;
	double sum_X2Y1 = 0.0;

	for (int i = 0; i < N; i++)
	{
		double x = points_cloud->points[i].x;
		double y = points_cloud->points[i].y;
		sum_X1 += x;
		sum_X2 += x*x;
		sum_X3 += x*x*x;
		sum_Y1 += y;
		sum_Y2 += y*y;
		sum_Y3 += y*y*y;
		sum_X1Y1 += x*y;
		sum_X1Y2 += x*y*y;
		sum_X2Y1 += x*x*y;
	}
	double C = N*sum_X2 - sum_X1*sum_X1;
	double D = N*sum_X1Y1 - sum_X1*sum_Y1;
	double E = N*sum_X3 + N*sum_X1Y2 - (sum_X2 + sum_Y2)*sum_X1;
	double G = N*sum_Y2 - sum_Y1*sum_Y1;
	double H = N*sum_X2Y1 + N*sum_Y3 - (sum_X2 + sum_Y2)*sum_Y1;

	double a = (H*D - E*G) / (C*G - D*D);
	double b = (H*C - E*D) / (D*D - G*C);
	double c = -(sum_X2 + sum_Y2 + a*sum_X1 + b*sum_Y1) / N;

	*circle_X = -0.5*a;
	*circle_Y = -0.5*b;
	*circle_R = 0.5*sqrt(a*a + b*b - 4 * c);
	return true;
}

在点云模型中的应用

拟合经过边缘检测的鞋子点云，鞋头部分与鞋跟部分