public MatrixH Inverse()
{
// m = 1 / [a(ei-fh) - b(di-fg) + c(dh-eg)]
//
// (ei-fh) (ch-bi) (bf-ce)
// inv(A) = m x (fg-di) (ai-cg) (cd-af)
// (dh-eg) (bg-ah) (ae-bd)
//
float a = this.elements[0], b = this.elements[1], c = this.elements[2];
float d = this.elements[3], e = this.elements[4], f = this.elements[5];
float g = this.elements[6], h = this.elements[7];
float m = 1f / (a * (e - f * h) - b * (d - f * g) + c * (d * h - e * g));
float na = m * (e - f * h);
float nb = m * (c * h - b);
float nc = m * (b * f - c * e);
float nd = m * (f * g - d);
float ne = m * (a - c * g);
float nf = m * (c * d - a * f);
float ng = m * (d * h - e * g);
float nh = m * (b * g - a * h);
float nj = m * (a * e - b * d);
return new MatrixH(na, nb, nc, nd, ne, nf, ng, nh, nj);
}
/// <summary>
/// Compute inliers using the Symmetric Transfer Error,
/// </summary>
///
private int[] distance(MatrixH H, double t)
{
// Compute the projections (both directions)
PointF[] p1 = H.TransformPoints(pointSet1);
PointF[] p2 = H.Inverse().TransformPoints(pointSet2);
// Compute the distances
for (int i = 0; i < pointSet1.Length; i++)
{
// Compute the distance as
float ax = pointSet1[i].X - p2[i].X;
float ay = pointSet1[i].Y - p2[i].Y;
float bx = pointSet2[i].X - p1[i].X;
float by = pointSet2[i].Y - p1[i].Y;
d2[i] = (ax * ax) + (ay * ay) + (bx * bx) + (by * by);
}
// Find and return the inliers
return(Matrix.Find(d2, z => z < t));
}
