散点拟合圆---最小二乘法

最近工作中遇到一个拟合圆的问题，通过找到的轮廓点（存在缺失的情况），需要找出圆心及半径。这里采用最小二乘法进行拟合，并记录一下具体的推导过程。最小二乘法是解决曲线拟合问题最常用的方法，其基本思路是：令

$$f(x) = \alpha_1 \phi_1(x) + \alpha_2 \phi_2(x) + ... + \alpha_m \phi_m(x)$$

其中$\phi_k(x)$是事先选择的一组线性无关函数，$\alpha_k$是待定系数 ，拟合准则是使$y_i(i = 1, 2, ..., n)$与$f(x_i)$的距离$\delta_i$的平方和最小。注意：最小二乘拟合对于离群点很敏感，如果存在离群点可以采用RANSAC(Random Sample Consensus)算法进行拟合。

最小二乘法拟合圆推导流程

1、确定待定系数

设圆的圆心为$(A, B)$，半径为$R$，则圆的方程可以写成

$$\begin{align} R^2 &= (x-A)^2 + (y - B)^2 \tag{1}\\ &= x^2 - 2Ax + A^2 + y^2 - 2By + B^2 \tag{2} \end{align}$$

令$a = -2A, b = -2B, c = A^2 + B^2 - R^2$，则圆的曲线方程可写成(3)式：

$$\begin{align} x^2 + y^2 +ax + by + c = 0 \tag{3} \end{align}$$

只需要求出参数$a, b, c$即可得到圆心及半径：

$$\begin{align} A &= -\frac{a}{2} \tag{4}\\ B &= -\frac{b}{2} \tag{5}\\ R &= \frac{1}{2}\sqrt{a^2 + b^2 - 4c} \tag{6} \end{align}$$

所以这里待定系数为$a, b, c$。

2、确定距离和

样本集$(X_i, Y_i), i = {1, 2, 3, ..., N}$中第$i$个样本到圆心的距离记为$d_i$,

$${d_i}^2 = (X_i - A)^2 + (Y_i - B)^2 \tag{7}$$

则其与半径之间的误差为：

$$\delta_i = {d_i}^2 - R^2 = {X_i}^2 + {Y_i}^2 + aX_i + bY_i + c \tag{8}$$

所以所有样本的误差和$Q(a, b, c)$为

$$\begin{align} Q(a, b, c) &= \sum_{i = 1}^{N}{\delta_i}^2 \tag{9} \\ &= \sum_{i = 1}^{N}{({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c)}^2 \tag{10} \end{align}$$

现在问题变成求参数$a,b,c$使得误差和$Q(a, b, c)$最小。

3、求解

$Q(a, b, c)$的每一项都大于等于0，所以函数存在大于或等于零的极小值，极大值为正无穷大。
$Q(a, b, c)$分别对参数$a,b,c$求偏导，然后令偏导数为0，即可求出极值点。

$$\begin{align} \frac{{\rm d}Q(a, b, c)}{{\rm d}a} &= \sum_{i = 1}^{N} 2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c)X_i = 0 \tag{11} \\ \frac{{\rm d}Q(a, b, c)}{{\rm d}b} &= \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c)Y_i = 0 \tag{12} \\ \frac{{\rm d}Q(a, b, c)}{{\rm d}c} &= \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c) = 0 \tag{13} \end{align}$$

$式(11) * N - 式(13) * \sum_{i = 1}^{N} X_i$消去c

$$\begin{align} 0 &= N\sum_{i = 1}^{N} 2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c)X_i - \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c) * \sum_{i = 1}^{N}X_i \tag{14} \\ &= N\sum_{i = 1}^{N} 2({X_i}^2 + {Y_i}^2 + aX_i + bY_i)X_i - \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i) * \sum_{i = 1}^{N}X_i \tag{15}\\ &= (N\sum_{i = 1}^{N}{X_i}^2 - \sum_{i = 1}^{N}X_i \sum_{i = 1}^{N}X_i)a + (N\sum_{i = 1}^{N}X_i Y_i - \sum_{i = 1}^{N}X_i \sum_{i = 1}^{N}Y_i)b + N\sum_{i = 1}^{N}{X_i}^3 + N\sum_{i = 1}^{N}X_i{Y_i}^2 - \sum_{i = 1}^{N}({X_i}^2 + {Y_i}^2)\sum_{i = 1}^{N}X_i \tag{16} \end{align}$$

$式(12) * N - 式(13) * \sum_{i = 1}^{N} Y_i$得

$$\begin{align} 0 &= N\sum_{i = 1}^{N} 2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c)Y_i - \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i + c) * \sum_{i = 1}^{N}Y_i \tag{17} \\ &= N\sum_{i = 1}^{N} 2({X_i}^2 + {Y_i}^2 + aX_i + bY_i)Y_i - \sum_{i = 1}^{N}2({X_i}^2 + {Y_i}^2 + aX_i + bY_i) * \sum_{i = 1}^{N}Y_i \tag{18}\\ &= (N\sum_{i = 1}^{N}{X_i}{Y_i} - \sum_{i = 1}^{N}X_i \sum_{i = 1}^{N}Y_i)a + (N\sum_{i = 1}^{N}{Y_i}^2 - \sum_{i = 1}^{N}Y_i \sum_{i = 1}^{N}Y_i)b + N\sum_{i = 1}^{N}{Y_i}^3 + N\sum_{i = 1}^{N}{X_i}^2{Y_i} - \sum_{i = 1}^{N}({X_i}^2 + {Y_i}^2)\sum_{i = 1}^{N}Y_i \tag{19} \end{align}$$

令

$$\begin{align} C &= N\sum_{i = 1}^{N} {X_i}^2 - \sum_{i = 1}^{N}X_i \sum_{i = 1}^{N}X_i \tag{20} \\ D &= N\sum_{i = 1}^{N} {X_i}{Y_i} - \sum_{i = 1}^{N}X_i \sum_{i = 1}^{N}Y_i \tag{21} \\ E &= N\sum_{i = 1}^{N} {X_i}^3 + N\sum_{i = 1}^{N} {X_i}{Y_i}^2 - \sum_{i = 1}^{N}({X_i}^2 + {Y_i}^2) \sum_{i = 1}^{N}X_i \tag{22} \\ G &= N\sum_{i = 1}^{N} {Y_i}^2 - \sum_{i = 1}^{N}Y_i \sum_{i = 1}^{N}Y_i \tag{23} \\ H &= N\sum_{i = 1}^{N} {X_i}^2Y_i + N\sum_{i = 1}^{N}{Y_i}^3 - \sum_{i = 1}^{N}({X_i}^2 + {Y_i}^2) \sum_{i = 1}^{N}Y_i \tag{24} \end{align}$$

可得：

$$\begin{align} 0 &= Ca + Db + E \tag{25} \\ 0 &= Da + Gb + H \tag{26} \\ a &= \frac{HD - EG} {CG - D^2} \tag{27} \\ b &= \frac{HC - ED} {D^2 - GC} \tag{28} \\ c &= -\frac{\sum_{i = 1}^{N}({X_i}^2 + {Y_i}^2) + a\sum_{i = 1}^{N}X_i + b\sum_{i = 1}^{N}Y_i}{N} \tag{29} \end{align}$$

再结合式(4)(5)(6)即可求得圆心和半径。

代码

• Fitter.h

点击展开

class Fitter
{
public:
	static double FitCircle(const std::vector<Point2>& pointArray, Point2 & center, double& radius);
};

• Fitter.cpp

点击展开

#include <limits>
#include <cmath>

#include "Fitter.h"

double Fitter::FitCircleByLeastSquare(const std::vector<Point2>& pointArray, Point2& center, double& radius)
{
	int N = pointArray.size();
	if (N < 3) {
		return std::numeric_limits<double>::max();
	}

	double sumX = 0.0; 
	double sumY = 0.0;
	double sumX2 = 0.0;
	double sumY2 = 0.0;
	double sumX3 = 0.0;
	double sumY3 = 0.0;
	double sumXY = 0.0;
	double sumXY2 = 0.0;
	double sumX2Y = 0.0;

	for (int pId = 0; pId < N; ++pId) {
		sumX += pointArray[pId].x;
		sumY += pointArray[pId].y;

		double x2 = pointArray[pId].x * pointArray[pId].x;
		double y2 = pointArray[pId].y * pointArray[pId].y;
		sumX2 += x2;
		sumY2 += y2;

		sumX3 += x2 * pointArray[pId].x;
		sumY3 += y2 * pointArray[pId].y;
		sumXY += pointArray[pId].x * pointArray[pId].y;
		sumXY2 += pointArray[pId].x * y2;
		sumX2Y += x2 * pointArray[pId].y;
 	}

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

	C = N * sumX2 - sumX * sumX;
	D = N * sumXY - sumX * sumY;
	E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
	G = N * sumY2 - sumY * sumY;
	H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

	a = (H * D - E * G) / (C * G - D * D);
	b = (H * C - E * D) / (D * D - G * C);
	c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

	center.x = -a / 2.0;
	center.y = -b / 2.0;
	radius = sqrt(a * a + b * b - 4 * c) / 2.0;

	double err = 0.0;
	double e;
	double r2 = radius * radius;
	for (int pId = 0; pId < N; ++pId){
		e = (pointArray[pId] - center).GetSquareSum() - r2;
		if (e > err) {
			err = e;
		}
	}
	return err;
}

• test.cpp

点击展开

#include "Fitter.h"

int main(int argc, char * argv[])
{
	std::vector<core::Point2> pts;
	pts.push_back(core::Point2(0.0, 1.0));
	pts.push_back(core::Point2(1.0, 0.0));
	pts.push_back(core::Point2(0.0, -1.0));
	pts.push_back(core::Point2(-1.0, 0.0));

	core::Point2 center;
	double r;
	double err = Fitter().FitCircleByLeastSquare(pts, center, r);

	return 0;
}

参考文档

圆拟合算法
最小二乘法拟合圆公式推导及其实现
最小二乘法拟合圆