/// <summary>
/// Normalizes a set of homogeneous points so that the origin is located
/// at the centroid and the mean distance to the origin is sqrt(2).
/// </summary>
public static PointH[] Normalize(this PointH[] points, out MatrixH T)
{
float n = points.Length;
float xmean = 0, ymean = 0;
for (int i = 0; i < points.Length; i++)
{
points[i].X = points[i].X / points[i].W;
points[i].Y = points[i].Y / points[i].W;
points[i].W = 1;
xmean += points[i].X;
ymean += points[i].Y;
}
xmean /= n; ymean /= n;
float scale = 0;
foreach (PointH point in points)
{
float x = point.X - xmean;
float y = point.Y - ymean;
scale += (float)Math.Sqrt(x * x + y * y);
}
scale = (float)(Sqrt2 * n / scale);
T = new MatrixH
(
scale, 0, -scale * xmean,
0, scale, -scale * ymean,
0, 0, 1
);
return(T.TransformPoints(points));
}
//Compute inliers using the Symmetric Transfer Error,
private int[] Distance(MatrixH H, double t)
{
int n = _pointSet1.Length;
// Compute the projections (both directions)
PointF[] p1 = H.TransformPoints(_pointSet1);
PointF[] p2 = H.Inverse().TransformPoints(_pointSet2);
// Compute the distances
double[] d2 = new double[n];
for (int i = 0; i < n; 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(d2.Find(z => z < t));
}