public static Vector2 SVD_V(List <Vector2> pts)
{
Matrix22 ATA = new Matrix22();
Vector2 direction;
foreach (Vector2 pt in pts)
{
ATA.m00 += pt.x * pt.x;
ATA.m10 = ATA.m01 += pt.x * pt.y;
ATA.m11 += pt.y * pt.y;
}
ATA.UpdateEigens();
ATA.eigenvector_0.Normalize();
ATA.eigenvector_1.Normalize();
if (ATA.eigenvalue_0 > ATA.eigenvalue_1)
{
direction = ATA.eigenvector_0;
}
else
{
direction = ATA.eigenvector_1;
}
return(direction);
}
public Gaussian_2D(Random rand, Vector2 miu_init)
{
//init with zeros
int rand_num = rand.Next(-10, 10);
miu = miu_init;
Sigma = new Matrix22(800, rand_num, rand_num, 800);;
}
public Matrix22 Inverse()
{
Matrix22 inv_M = new Matrix22();
inv_M.m00 = m11 / Det();
inv_M.m01 = -m01 / Det();
inv_M.m10 = -m10 / Det();
inv_M.m11 = m00 / Det();
return(inv_M);
}
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>
/// Generate a random gaussian.
/// </summary>
/// <param name="rand">
/// Random operator
/// </param>
/// <param name="corner">
/// Which corner to initialize
/// </param>
/// <param name="init">
/// If true, initialize gaussians for prediction;
/// If false, for input data
/// </param>
public Gaussian_2D(Random rand, int corner = 0, bool init = false)
{
//init input datapoints with random value
int rand_num = rand.Next(-10, 10);
if (!init && corner == 0)
{
miu = new Vector2(rand.Next(100, 1700), rand.Next(50, 900));
Sigma = new Matrix22(rand.Next(100, 900), rand_num,
rand_num, rand.Next(100, 900));
}
//init new gaussians at one of the four corners
else
{
switch (corner)
{
case 1:
miu = new Vector2(200, 200);
break;
case 2:
miu = new Vector2(1700, 900);
break;
case 3:
miu = new Vector2(200, 900);
break;
case 4:
miu = new Vector2(1700, 200);
break;
case 5:
miu = new Vector2(850, 200);
break;
case 6:
miu = new Vector2(850, 900);
break;
case 7:
miu = new Vector2(200, 1100);
break;
case 8:
miu = new Vector2(1700, 1100);
break;
}
//init covariance
Sigma = new Matrix22(2000, rand_num,
rand_num, 2000);
}
}
/// <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);
}
public Gaussian_2D()
{
//init with zeros
miu = new Vector2(0, 0);
Sigma = new Matrix22(0, 0, 0, 0);
}
