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