固定半径圆拟合

背景

当圆弧覆盖的角度小于90度时，使用最小二乘拟合或者taubin拟合出的圆会偏小，从而使拟合出的圆心与实际偏差过大。

原理

在已知圆弧的理论半径时，可以将圆半径作为一个约束，拟合圆心。
固定半径的圆拟合可以看做以下优化问题：

求u函数最小时，a和b的取值。
u分别对a和b求偏导，
当

时，u取最小值。
即需要解以下二元非线性方程。

由于该方程没有解析解，所以需要使用牛顿法迭代得到数值解。

代码

针对多元函数求最小值问题，可以使用GSL数学运算库。
编码实现如下：

#include <iostream>
#include <vector>
#include <math.h>

#include <iostream>
#include <fstream>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>
using namespace std;

#define PI (3.1415927f)


struct POINT
{
	double x;
	double y;
};

struct fitParam
{
	vector<POINT> points;
	double radius;
};

bool circleLeastFit(const std::vector<POINT> &points, double &center_x, double &center_y, double &radius)
{
	center_x = 0.0f;
	center_y = 0.0f;
	radius = 0.0f;
	if (points.size() < 3)
	{
		return false;
	}

	double sum_x = 0.0f, sum_y = 0.0f;
	double sum_x2 = 0.0f, sum_y2 = 0.0f;
	double sum_x3 = 0.0f, sum_y3 = 0.0f;
	double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

	int N = points.size();
	for (int i = 0; i < N; i++)
	{
		double x = points[i].x;
		double y = points[i].y;
		double x2 = x * x;
		double y2 = y * y;
		sum_x += x;
		sum_y += y;
		sum_x2 += x2;
		sum_y2 += y2;
		sum_x3 += x2 * x;
		sum_y3 += y2 * y;
		sum_xy += x * y;
		sum_x1y2 += x * y2;
		sum_x2y1 += x2 * y;
	}

	double C, D, E, G, H;
	double a, b, c;

	C = N * sum_x2 - sum_x * sum_x;
	D = N * sum_xy - sum_x * sum_y;
	E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
	G = N * sum_y2 - sum_y * sum_y;
	H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

	center_x = a / (-2);
	center_y = b / (-2);
	radius = sqrt(a * a + b * b - 4 * c) / 2;
	return true;
}

double L1_distance(const gsl_vector * v, void * params)
{
	fitParam *param = (fitParam*)params;
	int N = param->points.size();

	double a, b, r;
	a = gsl_vector_get(v, 0);
	b = gsl_vector_get(v, 1);
	r = param->radius;

	double sum = 0;
	for (int i = 0; i < N; i++)
	{
		const POINT p = param->points[i];
		double xi = p.x - a;
		double yi = p.y - b;
		double dist = sqrt(xi * xi + yi * yi) - r;
		sum += fabs(dist);
	}
	return sum;
}




void CircleFitFixRadius(const fitParam &param,double &center_x,double &center_y)
{
	gsl_multimin_function function;
	gsl_multimin_fminimizer * fminimizer;

	gsl_vector *start_point;   // 迭代初始值
	gsl_vector *step_size;     // 迭代步长

	int max_iter = 100; // 默认最大迭代 100 步
	function.n = 2;
	function.f = L1_distance;
	function.params = (void *)&param;

	start_point = gsl_vector_alloc(function.n);
	step_size = gsl_vector_alloc(function.n);
	fminimizer = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex, function.n);

	gsl_vector_set(start_point, 0, center_x);
	gsl_vector_set(start_point, 1, center_y);
	gsl_vector_set(step_size, 0, 1);
	gsl_vector_set(step_size, 1, 1);

	gsl_multimin_fminimizer_set(fminimizer, &function, start_point, step_size);

	int iter = 0;
	int status;
	do
	{
		iter++;
		status = gsl_multimin_fminimizer_iterate(fminimizer);
		if (status == GSL_ENOPROG) // 表示无法找到更好的解了
		{
			break;
		}
		double size = gsl_multimin_fminimizer_size(fminimizer);
		status = gsl_multimin_test_size(size, 1e-2);
	} while (status == GSL_CONTINUE && iter < max_iter);

	gsl_vector * out = gsl_multimin_fminimizer_x(fminimizer);

	center_x = gsl_vector_get(out, 0);
	center_y = gsl_vector_get(out, 1);
}

int main()
{
	fitParam param;
	vector<POINT> points;
	ofstream outfile("out.txt", ios::trunc);
	double R0 = 45.0;
	double X0 = 0;
	double Y0 = 0;
	//生成1/8圆
	for (int i = 22; i < 67; i++)
	{
		POINT p;
		p.x = X0 + R0 * cos(i / 180.0*PI) + (rand() % 1000 - 500) / 1000.0;
		p.y = Y0 + R0 * sin(i / 180.0*PI) + (rand() % 1000 - 500) / 1000.0;
		param.points.push_back(p);
		outfile << p.x << " " << p.y << endl;
	}
	outfile.close();

	ofstream outfileR("outR.txt", ios::trunc);

	//使用最小二乘拟合不带半径约束
	double xc, yc, rc;
	circleLeastFit(param.points, xc, yc, rc);
	std::cout << "X:" <<xc << ",Y:" << yc << ",R:" << rc << endl;
	outfileR << xc << " " << yc << " " << rc << endl;
	//将最小二乘得到结果作为迭代算法初始值，半径使用理论值
	param.radius = R0;
	CircleFitFixRadius(param, xc, yc);
	std::cout << "X:" << xc << ",Y:" << yc << endl;
	outfileR << xc << " " << yc << " " << param.radius << endl;
	outfileR.close();
	return 0;
}

需要注意的是，针对短圆弧，有可能陷入局部最优，所以要选取一个好的迭代初值。
本文的方法是先使用最小二乘拟合出大概的半径和圆心位置。然后半径用理论半径替换，作为优化的迭代初值。

效果

使用MATLAB分析结果

close all
clear
load('out.txt')
x = out(:,1);
y = out(:,2);
plot(x,y,".k")
hold on
grid on

load('outR.txt')
x1 = outR(1,1);
y1 = outR(1,2);
R = outR(1,3);
rectangle('position',[x1-R,y1-R,R*2,R*2],'curvature',[1,1],'EdgeColor','r','LineStyle','--');
plot(x1,y1,"*r");


x1 = outR(2,1);
y1 = outR(2,2);
R = outR(2,3);
rectangle('position',[x1-R,y1-R,R*2,R*2],'curvature',[1,1],'EdgeColor','b','LineStyle','-');
plot(x1,y1,"*b");

legend('拟合点','最小二乘拟合圆心','理论半径拟合圆心');

执行效果如下：

可以看到在1/8圆时，仍能得到较好的结果。