Explanation
Many times, we need to exercise the turning radius of the object to describe the performance of the machine. But in most real-world conditions, we can only get the GPS location point set of moving objects, and can not directly turning radius or the center position. For this purpose, we can use a circular manner to obtain the coordinates of the circle fitted to the equation, thereby obtaining the turning radius and center location.
Resolution process
There are round about method of fitting equation lot, had learned a lot in this translation algebraic approximation, least squares method, orthogonal distance regression to fit round their results compare (Python) . This paper also gives a series of performance and the effect of contrast three methods mentioned, the ultimate solution is to obtain the optimal method of least squares. Due to a recent study further learned that you can make use of linear algebraic methods to solve it. In the curricula of universities have learned knowledge "linear algebra", so think of using this method to resolve this problem together, in order to make the most contrast.
Next, the article is obtained and on the method of least squares linear algebraic equations for a circle discussed.
ready
Reference matrix calculation library MathNet.Numerics . The library is a powerful scientific computing library, follow the .Net Standard, so you can cross-platform use.
Create a class description circle
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; }
}Drawing, reference System.Drawing.Common library, in order to achieve image generation cross-platform. Next, we simply help achieve a class image for image rendering.
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);
}
}Analog set point, since the data acquisition reality there are many uncertain factors in the accuracy of the image data recording and the like. Analog focus point will also add a certain degree of noise. X and y in the following code stored in our data set of points:
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);
}Least squares method
There are many online analytical principle, the translation mentioned above also mentioned here is not described in detail here. Direct posted c # code to achieve:
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;
}Linear Algebra
From the standard equation of a circle (x-c1)^2+(y-c2)^2=r^2for conversion equation obtained 2xc1+2yc2+(r^2−c1^2−c2^2)=x^2+y^2these, we c3replace the constant value r^2−c1^2−c2^2, namely: r^2−c1^2−c2^2=c3. Thus, we get 2xc1+2yc2+c3=x^2+y^2the point set into the equation left with three unknowns `c1, c2 and c3.
For simplicity, suppose we have four points {[0,5],[0,-5],[5,0],[-5,0]}, four equations into the equation can be obtained:
0c1 + 10c2 + c3 = 25
0c1 - 10c2 + c3 = 25
10c1 + 0c2 + c3 = 25
-10c1 + 0c2 + c3 = 25The equation is simple, a solution able to be seen. But we can get using linear algebra matrix:
/***************************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|In the matrix equation A*B=C, the matrix is obtained only Bto the solution of equations obtained. c # in MathNet.Numericscan easily do the job:
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;
}Concluded
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");Console output:
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.1691119As can be seen from the results, the results of the two methods is basically the same, difference only appeared in quite a few decimal places. But the difference was enormous computational efficiency, least squares linear algebra than 100 times faster.
In the drawing, both the coincidence (green orange covered later).
In the least-squares method, and only a simple for loop, rarely involve memory write. However, linear algebra, matrix needs to be generated DenseMatrix.OfArray, and matrix operations, both of which require memory write. Further, matrix calculation with the computationally intensive, which are affecting the efficiency of the circle fitted linear algebra. The final victory still belongs to the least squares method.