.net core(c#)拟合圆测试
- 2019 年 10 月 3 日
- 筆記
说明
很多时候，我们需要运动物体的转弯半径去描述其机器性能。但在大多数的现实条件下，我们只能够获取到运动物体的 GPS 位置点集，并不能直接得到转弯半径或者圆心位置。为此，我们可以利用拟合圆的方式得到圆坐标方程，由此得到转弯半径和圆心位置。
解决过程
关于拟合圆方程的方法有很多，曾经在这篇译文中获益良多代数逼近法、最小二乘法、正交距离回归法来拟合圆及其结果对比（Python）。此系列文中也给出了提及的三种方法的性能及效果对比，最终得出最优的解决方案就是最小二乘法。由于最近的学习中又进一步了解到，可以利用线性代数的方法去求解。本着大学课程中曾学过的《线性代数》知识，所以想着用此方法再加以解决该问题，以作最对比。
接下来，本文就最小二乘法和线性代数的方法求取圆方程作一论述。
准备
引用矩阵计算库MathNet.Numerics。该库是一个强大的科学计算库，遵循 .Net Standard，所以可以跨平台使用。
创建描述圆的类
public class Circle { /// <summary> /// 圆心横坐标 /// </summary> /// <value></value> public double X { get; set; } /// <summary> /// 圆心纵坐标 /// </summary> /// <value></value> public double Y { get; set; } /// <summary> /// 圆半径 /// </summary> /// <value></value> public double R { get; set; } }画图，引用System.Drawing.Common库，以实现跨平台的图像生成。接下来，我们简单的实现一个图像帮助类来进行图像绘制。
public class ImageHelp { private Image _image; public ImageHelp(int width, int height) { _image = new Bitmap(width, height); var graph = Graphics.FromImage(_image); graph.Clear(Color.White); } public void DrawCicle(Circle circle, Brush brush) { var graph = Graphics.FromImage(_image); var count=200; var fitPoints = new Point[count+1]; var step = 2 * Math.PI / count; for (int i = 0; i < count; i++) { //circle var p = new Point(); p.X = (int)(circle.X + Math.Cos(i * step) * circle.R); p.Y = (int)(circle.Y + Math.Sin(i * step) * circle.R); fitPoints[i] = p; } fitPoints[count] = fitPoints[0];//闭合圆 graph.DrawLines(new Pen(brush, 2), fitPoints); graph.Dispose(); } public void DrawPoints(double[] X, double[] Y, Brush brush) { var graph = Graphics.FromImage(_image); for (int i = 0; i < X.Length; i++) { graph.DrawEllipse(new Pen(brush, 2), (int)X[i], (int)Y[i], 6, 6); } graph.Dispose(); } public void SaveImage(string file) { _image.Save(file, System.Drawing.Imaging.ImageFormat.Png); } }模拟点集，由于现实中的数据采集存在着精度、数据记录等众多不确定因素的影像。模拟点集中也将加入一定程度的噪音。以下代码中 x 与 y 中存储着我们的点集数据：
var count = 50; var step = 2 * Math.PI / 100; var rd = new Random(); //参照圆 var x0 = 204.1; var y0 = 213.1; var r0 = 98.4; //噪音绝对差 var diff = (int)(r0 * 0.1); var x = new double[count]; var y = new double[count]; //输出点集 for (int i = 0; i < count; i++) { //circle x[i] = x0 + Math.Cos(i * step) * r0; y[i] = y0 + Math.Sin(i * step) * r0; //noise x[i] += Math.Cos(rd.Next() % 2 * Math.PI) * rd.Next(diff); y[i] += Math.Cos(rd.Next() % 2 * Math.PI) * rd.Next(diff); }最小二乘法
网上有很多的原理解析，上文中提到的译文中也有提及，这里不在过多赘述。直接贴出 c#代码实现：
public Circle LeastSquaresFit(double[] X, double[] Y) { if (X.Length < 3) { return null; } double cent_x = 0.0, cent_y = 0.0, radius = 0.0; 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 = X.Length; double x, y, x2, y2; for (int i = 0; i < N; i++) { x = X[i]; y = Y[i]; x2 = x * x; 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; cent_x = a / (-2); cent_y = b / (-2); radius = Math.Sqrt(a * a + b * b - 4 * c) / 2; var result = new Circle(); result.X = cent_x; result.Y = cent_y; result.R = radius; return result; }线性代数
从标准圆方程(x-c1)^2+(y-c2)^2=r^2中进行方程变换得到2xc1+2yc2+(r^2−c1^2−c2^2)=x^2+y^2，其中，我们c3替换常量值r^2−c1^2−c2^2，即：r^2−c1^2−c2^2=c3。由此，我们得到2xc1+2yc2+c3=x^2+y^2，将点集带入，方程就只剩三个未知数`c1,c2 和 c3。
简单起见，假设我们有四个点{[0,5],[0,-5],[5,0],[-5,0]},代入方程可得到四个方程：
0c1 + 10c2 + c3 = 25 0c1 - 10c2 + c3 = 25 10c1 + 0c2 + c3 = 25 -10c1 + 0c2 + c3 = 25该方程组比较简单，一眼便能看出解。但用线性代数我们可以得到矩阵：
/***************************A**********B******C*/ | 0c1 10c2 1c3| | 0 10 1| |c1| |25| | 0c1 -10c2 1c3| = | 0 -10 1| * |c2| = |25| | 10c1 0c2 1c3| | 10 0 1| |c3| |25| |-10c1 0c2 1c3| |-10 0 1| |25|在矩阵方程中A*B=C，只需求出矩阵B即可得到方程组的解。c#中MathNet.Numerics可以轻松胜任这一工作：
public Circle LinearAlgebraFit(double[] X, double[] Y) { if (X.Length < 3) { return null; } var count = X.Length; var a = new double[count, 3]; var c = new double[count, 1]; for (int i = 0; i < count; i++) { //matrix a[i, 0] = 2 * X[i]; a[i, 1] = 2 * Y[i]; a[i, 2] = 1; c[i, 0] = X[i] * X[i] + Y[i] * Y[i]; } var A = DenseMatrix.OfArray(a); var C = DenseMatrix.OfArray(c); //A*B=C var B = A.Solve(C); double c1 = B.At(0, 0), c2 = B.At(1, 0), r = Math.Sqrt(B.At(2, 0) + c1 * c1 + c2 * c2); var result = new Circle(); result.X = c1; result.Y = c2; result.R = r; return result; }最后总结
Console.WriteLine($"raw c1:{x0}, c2:{y0}, r:{r0}"); var fit = new FitCircle(); var sth = new Stopwatch(); sth.Start(); var lsf = fit.LeastSquaresFit(x, y); Console.WriteLine($"LeastSquaresFit c1:{lsf.X}, c2:{lsf.Y}, r:{lsf.R}, time:{sth.Elapsed}"); sth.Restart(); var laf = fit.LinearAlgebraFit(x, y); Console.WriteLine($"LinearAlgebraFit c1:{laf.X}, c2:{laf.Y}, r:{laf.R}, time:{sth.Elapsed}"); var img = new ImageHelp(512, 512); img.DrawPoints(x, y, Brushes.Red); img.DrawCicle(lsf, Brushes.Green); img.DrawCicle(laf, Brushes.Orange); img.SaveImage("graph.jpeg");控制台输出：
raw c1:204.1, c2:213.1, r:98.4 LeastSquaresFit c1:204.791071061878, c2:210.86075318831, r:100.436594821545, time:00:00:00.0011029 LinearAlgebraFit c1:204.791071061878, c2:210.860753188315, r:100.436594821541, time:00:00:00.1691119从结果中可以看出，两种方法的结果基本一样，在小数点后好几位才出现差别。但是其计算效率却差异巨大，最小二乘法比线性代数快上 100 多倍。
在图中,二者重合（绿色被后面的橙色覆盖）。
在最小二乘法中，只有一个及其简单的 for 循环，很少涉及内存写。但在线性代数中，需要进行矩阵的生成DenseMatrix.OfArray，以及矩阵运算，这二者都需要内存写。再者，矩阵计算有着繁重的计算量，这些都在影响着线性代数拟合圆的效率。最终的胜利还是属于最小二乘法。
