void mStep(Config config)
{
For.AllKernels(config, (_, k) =>
{
var gsum = sumInRegion(config, k, i => g(k)[index(config, i, k)]);
S[k.index] = sumInRegion(config, k, i => g(k)[index(config, i, k)] * Mat2x2.FromVecVec(i.p - m[k.index], i.p - m[k.index])) / gsum;
m[k.index] = sumInRegion(config, k, i => g(k)[index(config, i, k)] * i.p) / gsum;
v[k.index] = sumInRegion(config, k, i => g(k)[index(config, i, k)] * c[i.index]) / gsum;
});
}
Mat2x2 sumInRegion(Config config, Kernel k, Func <Position, Mat2x2> pos2val)
{
var result = new Mat2x2(0, 0, 0, 0);
For.AllPixelsOfRegion(config, k, (_, i) =>
{
result += pos2val(i);
});
return(result);
}
public void SVD(out Mat2x2 U, out Mat2x2 S, out Mat2x2 Vt)
{
// accoding to the web page:
// http://www.lucidarme.me/?p=4624
double a = m11;
double b = m12;
double c = m21;
double d = m22;
double v1 = 2 * a * c + 2 * b * d;
double v2 = a * a + b * b - c * c - d * d;
double theta = 0.5 * (double)Math.Atan2((double)v1, (double)v2);
U = new Mat2x2(
(double)Math.Cos((double)theta),
-(double)Math.Sin((double)theta),
(double)Math.Sin((double)theta),
(double)Math.Cos((double)theta));
double S1 = a * a + b * b + c * c + d * d;
double S2 = (double)Math.Sqrt((double)(v2 * v2 + v1 * v1));
double s1 = (double)Math.Sqrt((double)((S1 + S2) * 0.5));
double s2 = (double)Math.Sqrt((double)((S1 - S2) * 0.5));
S = new Mat2x2(s1, 0, 0, s2);
double u1 = 2 * a * b + 2 * c * d;
double u2 = a * a - b * b + c * c - d * d;
double phi = 0.5 * (double)Math.Atan2((double)u1, (double)u2);
double cp = (double)Math.Cos((double)phi);
double sp = (double)Math.Sin((double)phi);
double ct = (double)Math.Cos((double)theta);
double st = (double)Math.Sin((double)theta);
double s11 = (a * ct + c * st) * cp + (b * ct + d * st) * sp;
double s22 = (a * st - c * ct) * sp + (-b * st + d * ct) * cp;
double sign_s11 = Math.Sign(s11);
double sign_s22 = Math.Sign(s22);
Vt = new Mat2x2(sign_s11 * cp, sign_s11 * sp, -sign_s22 * sp, sign_s22 * cp);
}