public void Estimate(List <Vector2> pts)
{
// http://www.dtcenter.org/sites/default/files/community-code/met/docs/write-ups/circle_fit.pdf
float a;
float b;
float x2;
float y2;
float uc;
float vc;
Matrix22 m1;
Matrix22 m2;
Matrix22 m3;
float sum_x2 = 0;
float sum_y2 = 0;
float sum_xy = 0;
float sum_x3 = 0;
float sum_y3 = 0;
float sum_xy2 = 0;
float sum_yx2 = 0;
Vector2 origin = Matrix.Mean(pts);
List <Vector2> centered_pts = Matrix.Minus(pts, origin);
foreach (Vector2 pt in centered_pts)
{
x2 = pt.x * pt.x;
y2 = pt.y * pt.y;
sum_x2 += x2;
sum_y2 += y2;
sum_xy += pt.x * pt.y;
sum_x3 += x2 * pt.x;
sum_y3 += y2 * pt.y;
sum_xy2 += pt.x * y2;
sum_yx2 += pt.y * x2;
}
a = (sum_x3 + sum_xy2) / 2;
b = (sum_y3 + sum_yx2) / 2;
// Solve 2-dimensional linear equation using Cramer’s rule:
// https://www.geeksforgeeks.org/system-linear-equations-three-variables-using-cramers-rule/
m1 = new Matrix22(sum_x2, sum_xy, sum_xy, sum_y2);
m2 = new Matrix22(a, sum_xy, b, sum_y2);
m3 = new Matrix22(sum_x2, a, sum_xy, b);
uc = m2.Det() / m1.Det();
vc = m3.Det() / m1.Det();
Center = new Vector2(uc, vc).Add(origin);
R = (float)Math.Sqrt(uc * uc + vc * vc + (sum_x2 + sum_y2) / centered_pts.Count);
}
/// <summary>
/// Calculate multivariate normal probablity density function.
/// Hardcoded for Vector2 and Matrix22 datatype.
/// Use double precision to increase robustness.
/// </summary>
/// <param name="pt"></param>
/// <param name="miu"></param>
/// <param name="covariance"></param>
/// <returns></returns>
private static double MultiNormalPDF(Vector2 pt, Gaussian_2D gaussian)
{
Vector2 miu = gaussian.miu;
Matrix22 covariance = gaussian.Sigma;
Vector2 x_minus_miu = pt.Minus(miu);
Matrix22 inv_cov = covariance.Inverse();
//Break Matrix22 into two rows (Vector2) for dot product
Vector2 row1 = new Vector2(inv_cov.m00, inv_cov.m01);
Vector2 row2 = new Vector2(inv_cov.m10, inv_cov.m11);
float dot_row1 = row1.Dot(x_minus_miu);
float dot_row2 = row2.Dot(x_minus_miu);
//Assemble two rows
Vector2 temp_v = new Vector2(dot_row1, dot_row2);
double numerator = Math.Exp(-x_minus_miu.Dot(temp_v) / 2);
double denom = 2 * Math.PI * Math.Sqrt(covariance.Det());
return(numerator / denom);
}
