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);
}