|
public static Normalize ( this points, |
||
| points | this | |
| transformation | ||
| return | System.Drawing.PointF[] | |
/// <summary>
/// Matches two sets of points using RANSAC.
/// </summary>
///
/// <returns>The homography matrix matching x1 and x2.</returns>
///
public MatrixH Estimate(PointF[] points1, PointF[] points2)
{
// Initial argument checks
if (points1.Length != points2.Length)
{
throw new ArgumentException("The number of points should be equal.");
}
if (points1.Length < 4)
{
throw new ArgumentException("At least four points are required to fit an homography");
}
// Normalize each set of points so that the origin is
// at centroid and mean distance from origin is sqrt(2).
MatrixH T1, T2;
this.pointSet1 = Tools.Normalize(points1, out T1);
this.pointSet2 = Tools.Normalize(points2, out T2);
d2 = new double[points1.Length];
// Compute RANSAC and find the inlier points
MatrixH H = ransac.Compute(points1.Length, out inliers);
if (inliers == null || inliers.Length < 4)
{
//throw new Exception("RANSAC could not find enough points to fit an homography.");
return(null);
}
// Compute the final homography considering all inliers
H = homography(inliers);
// Denormalize
H = T2.Inverse() * (H * T1);
return(H);
}
public void NormalizeTest()
{
PointH[] points = new PointH[]
{
new PointH(1, 2),
new PointH(5, 2),
new PointH(12, 2),
new PointH(1, 2),
new PointH(10, 2),
};
MatrixH T;
PointH[] actual = Tools.Normalize(points, out T);
// Centroids should be at the origin
double cx = 0, cy = 0;
for (int i = 0; i < actual.Length; i++)
{
cx += actual[i].X / actual[i].W;
cy += actual[i].Y / actual[i].W;
}
Assert.AreEqual(cx / actual.Length, 0, 0.0000001);
Assert.AreEqual(cy / actual.Length, 0, 0.0000001);
// Average distance from the origin should be sqrt(2)
double d = 0;
for (int i = 0; i < actual.Length; i++)
{
double x = actual[i].X / actual[i].W;
double y = actual[i].Y / actual[i].W;
d += System.Math.Sqrt(x * x + y * y);
}
Assert.AreEqual(d / actual.Length, System.Math.Sqrt(2), 0.00001);
}
/// <summary>
/// Matches two sets of points using RANSAC.
/// </summary>
///
/// <returns>The fundamental matrix relating x1 and x2.</returns>
///
public float[,] Estimate(PointF[] points1, PointF[] points2)
{
// Initial argument checks
if (points1.Length != points2.Length)
{
throw new ArgumentException("The number of points should be equal.");
}
if (points1.Length < 8)
{
throw new ArgumentException("At least eight points are required.");
}
// Normalize each set of points so that the origin is
// at centroid and mean distance from origin is sqrt(2).
float[,] T1, T2;
this.pointSet1 = Tools.Normalize(points1, out T1);
this.pointSet2 = Tools.Normalize(points2, out T2);
// Compute RANSAC and find the inlier points
float[,] F = ransac.Compute(points1.Length, out inliers);
if (inliers == null || inliers.Length < 8)
{
return(null);
}
// Compute the final fundamental
// matrix considering all inliers
F = fundamental(inliers);
// Denormalize
F = T2.Transpose().Multiply(F.Multiply(T1));
return(F);
}
/// <summary>
/// Creates an homography matrix matching points
/// from a set of points to another.
/// </summary>
public static MatrixH Homography(PointH[] points1, PointH[] points2)
{
// Initial argument checkings
if (points1.Length != points2.Length)
{
throw new ArgumentException("The number of points should be equal.");
}
if (points1.Length < 4)
{
throw new ArgumentException("At least four points are required to fit an homography");
}
int N = points1.Length;
MatrixH T1, T2; // Normalize input points
points1 = Tools.Normalize(points1, out T1);
points2 = Tools.Normalize(points2, out T2);
// Create the matrix A
double[,] A = new double[3 * N, 9];
for (int i = 0; i < N; i++)
{
PointH X = points1[i];
double x = points2[i].X;
double y = points2[i].Y;
double w = points2[i].W;
int r = 3 * i;
A[r, 0] = 0;
A[r, 1] = 0;
A[r, 2] = 0;
A[r, 3] = -w * X.X;
A[r, 4] = -w * X.Y;
A[r, 5] = -w * X.W;
A[r, 6] = y * X.X;
A[r, 7] = y * X.Y;
A[r, 8] = y * X.W;
r++;
A[r, 0] = w * X.X;
A[r, 1] = w * X.Y;
A[r, 2] = w * X.W;
A[r, 3] = 0;
A[r, 4] = 0;
A[r, 5] = 0;
A[r, 6] = -x * X.X;
A[r, 7] = -x * X.Y;
A[r, 8] = -x * X.W;
r++;
A[r, 0] = -y * X.X;
A[r, 1] = -y * X.Y;
A[r, 2] = -y * X.W;
A[r, 3] = x * X.X;
A[r, 4] = x * X.Y;
A[r, 5] = x * X.W;
A[r, 6] = 0;
A[r, 7] = 0;
A[r, 8] = 0;
}
// Create the singular value decomposition
SingularValueDecomposition svd = new SingularValueDecomposition(A, false, true);
double[,] V = svd.RightSingularVectors;
// Extract the homography matrix
MatrixH H = new MatrixH((float)V[0, 8], (float)V[1, 8], (float)V[2, 8],
(float)V[3, 8], (float)V[4, 8], (float)V[5, 8],
(float)V[6, 8], (float)V[7, 8], (float)V[8, 8]);
// Denormalize
H = T2.Inverse().Multiply(H.Multiply(T1));
return(H);
}