/// <summary>
/// Detects if three points are colinear.
/// </summary>
public static bool Colinear(PointH pt1, PointH pt2, PointH pt3)
{
return(System.Math.Abs(
(pt1.Y * pt2.W - pt1.W * pt2.Y) * pt3.X +
(pt1.W * pt2.X - pt1.X * pt2.W) * pt3.Y +
(pt1.X * pt2.Y - pt1.Y * pt2.X) * pt3.W) < Special.SingleEpsilon);
}
/// <summary>
/// Transforms the given points using this transformation matrix.
/// </summary>
///
public PointH[] TransformPoints(params PointH[] points)
{
PointH[] r = new PointH[points.Length];
for (int j = 0; j < points.Length; j++)
{
r[j].X = elements[0] * points[j].X + elements[1] * points[j].Y + elements[2] * points[j].W;
r[j].Y = elements[3] * points[j].X + elements[4] * points[j].Y + elements[5] * points[j].W;
r[j].W = elements[6] * points[j].X + elements[7] * points[j].Y + points[j].W;
}
return(r);
}
/// <summary>
/// Compares two objects for equality.
/// </summary>
///
public override bool Equals(object obj)
{
if (obj is PointH)
{
PointH p = (PointH)obj;
if (px / pw == p.px / p.pw &&
py / pw == p.py / p.pw)
{
return(true);
}
}
return(false);
}
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>
/// Converts to a Integer point by rounding the point coordinates.
/// </summary>
///
public static Point Round(PointH point)
{
return(new Point(
(int)System.Math.Round(point.px / point.pw),
(int)System.Math.Round(point.py / point.pw)));
}
/// <summary>
/// Converts to a Integer point by computing the ceiling of the point coordinates.
/// </summary>
///
public static Point Ceiling(PointH point)
{
return(new Point(
(int)System.Math.Ceiling(point.px / point.pw),
(int)System.Math.Ceiling(point.py / point.pw)));
}
/// <summary>
/// Add the values of two points.
/// </summary>
///
public PointH Add(PointH value)
{
return(this + value);
}
/// <summary>
/// Subtracts the values of two points.
/// </summary>
///
public PointH Subtract(PointH value)
{
return(this - value);
}
/// <summary>
/// Transforms the given points using this transformation matrix.
/// </summary>
///
public PointH[] TransformPoints(params PointH[] points)
{
PointH[] r = new PointH[points.Length];
for (int j = 0; j < points.Length; j++)
{
r[j].X = elements[0] * points[j].X + elements[1] * points[j].Y + elements[2] * points[j].W;
r[j].Y = elements[3] * points[j].X + elements[4] * points[j].Y + elements[5] * points[j].W;
r[j].W = elements[6] * points[j].X + elements[7] * points[j].Y + points[j].W;
}
return r;
}
/// <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);
}
public void ColinearTest()
{
bool actual;
PointH p1 = new PointH(0, 1);
PointH p2 = new PointH(0, 2);
PointH p3 = new PointH(0, 3);
bool expected = true;
actual = Tools.Collinear(p1, p2, p3);
Assert.AreEqual(expected, actual);
p1 = new PointH(0, 1);
p2 = new PointH(1, 0);
p3 = new PointH(1, 1);
expected = false;
actual = Tools.Collinear(p1, p2, p3);
Assert.AreEqual(expected, actual);
}
/// <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 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");
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
float[,] A = new float[3 * N, 9];
for (int i = 0; i < N; i++)
{
PointH X = points1[i];
float x = points2[i].X;
float y = points2[i].Y;
float 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
var svd = new SingularValueDecompositionF(A,
computeLeftSingularVectors: false, computeRightSingularVectors: true,
autoTranspose: false, inPlace: true);
float[,] V = svd.RightSingularVectors;
// Extract the homography matrix
MatrixH H = new MatrixH(V[0, 8], V[1, 8], V[2, 8],
V[3, 8], V[4, 8], V[5, 8],
V[6, 8], V[7, 8], V[8, 8]);
// Denormalize
H = T2.Inverse().Multiply(H.Multiply(T1));
return H;
}
/// <summary>
/// Add the values of two points.
/// </summary>
///
public PointH Add(PointH value)
{
return this + value;
}
/// <summary>
/// Subtracts the values of two points.
/// </summary>
///
public PointH Subtract(PointH value)
{
return this - value;
}
public void ColinearTest1()
{
bool actual;
PointF pt1, pt2, pt3;
pt1 = new PointF(0, 0);
pt2 = new PointF(1, 1);
pt3 = new PointF(2, 2);
actual = Tools.Collinear(pt1, pt2, pt3);
Assert.AreEqual(true, actual);
pt1 = new PointH(0, 1);
pt2 = new PointH(1, 1);
pt3 = new PointH(2, 2);
actual = Tools.Collinear(pt1, pt2, pt3);
Assert.AreEqual(false, actual);
}
public void HomographyTest()
{
PointH[] x1 =
{
new PointH(0, 0),
new PointH(1, 0),
new PointH(0, 1),
new PointH(1, 1),
};
PointH[] x2 =
{
new PointH(0, 0),
new PointH(1, 0),
new PointH(0, 1),
new PointH(1, 1),
};
double[,] expected = Matrix.Identity(3);
double[,] actual = (double[,])Tools.Homography(x1, x2);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
actual[i, j] /= actual[2, 2];
Assert.IsTrue(Matrix.IsEqual(expected, actual, 1e-4));
x1 = new PointH[]
{
new PointH(2, 0),
new PointH(1, 0),
new PointH(5, 1),
new PointH(1, 1),
new PointH(7, 1),
new PointH(1, 2),
new PointH(1, 1),
};
x2 = new PointH[]
{
new PointH(9, 1),
new PointH(1, 5),
new PointH(9, 1),
new PointH(1, 7),
new PointH(2, 7),
new PointH(6, 5),
new PointH(1, 7),
};
expected = new double[,]
{
{ 0.2225, -3.1727, 1.8023 },
{ 0.3648, -1.7149, -0.2173 },
{ 0.0607, -0.4562, 0.1229 },
};
expected = (double[,])(new MatrixH(expected));
actual = (double[,])Tools.Homography(x1, x2);
Assert.IsTrue(Matrix.IsEqual(expected, actual, 0.01));
}
/// <summary>
/// Converts to a Integer point by truncating the point coordinates.
/// </summary>
///
public static Point Truncate(PointH point)
{
return(new Point(
(int)System.Math.Truncate(point.px / point.pw),
(int)System.Math.Truncate(point.py / point.pw)));
}
/// <summary>
/// Creates the fundamental matrix between two
/// images from a set of points from each image.
/// </summary>
///
public static float[,] Fundamental(PointH[] points1, PointH[] points2)
{
int N = points1.Length;
float[,] T1, T2; // Normalize input points
points1 = Tools.Normalize(points1, out T1);
points2 = Tools.Normalize(points2, out T2);
float[,] A = new float[N, 9];
for (int i = 0; i < N; i++)
{
float x1 = points1[i].X;
float y1 = points1[i].Y;
float x2 = points2[i].X;
float y2 = points2[i].Y;
A[i, 0] = x2 * x1;
A[i, 1] = x2 * y1;
A[i, 2] = x2;
A[i, 3] = y2 * x1;
A[i, 4] = y2 * y2;
A[i, 5] = y2;
A[i, 6] = x1;
A[i, 7] = y1;
A[i, 8] = 1;
}
float[,] F = createFundamentalMatrix(A);
// Denormalize
F = T2.Transpose().Multiply(F.Multiply(T1));
return F;
}
/// <summary>
/// Converts to a Integer point by computing the ceiling of the point coordinates.
/// </summary>
///
public static Point Ceiling(PointH point)
{
return new Point(
(int)System.Math.Ceiling(point.px / point.pw),
(int)System.Math.Ceiling(point.py / point.pw));
}
public void MultiplyTest()
{
MatrixH matrix = new MatrixH(Matrix.Identity(3));
PointH[] points = new PointH[]
{
new PointH(1, 2),
new PointH(5, 2),
new PointH(12, 2),
new PointH(1, 2),
new PointH(10, 2),
};
PointH[] expected = new PointH[]
{
new PointH(1, 2),
new PointH(5, 2),
new PointH(12, 2),
new PointH(1, 2),
new PointH(10, 2),
};
PointH[] actual = (PointH[])points.Clone();
matrix.TransformPoints(actual);
Assert.AreEqual(expected[0], actual[0]);
Assert.AreEqual(expected[1], actual[1]);
Assert.AreEqual(expected[2], actual[2]);
Assert.AreEqual(expected[3], actual[3]);
Assert.AreEqual(expected[4], actual[4]);
}
/// <summary>
/// Creates the fundamental matrix between two
/// images from a set of points from each image.
/// </summary>
///
public static float[,] Fundamental(PointH[] points1, PointH[] points2, out PointH[] epipoles)
{
var F = Fundamental(points1, points2);
SingularValueDecompositionF svd = new SingularValueDecompositionF(F,
computeLeftSingularVectors: true, computeRightSingularVectors: true,
autoTranspose: true, inPlace: false);
var U = svd.LeftSingularVectors;
var V = svd.RightSingularVectors;
PointH e1 = new PointH(V[0, 2] / V[2, 2], V[1, 2] / V[2, 2], 1);
PointH e2 = new PointH(U[0, 2] / U[2, 2], U[1, 2] / U[2, 2], 1);
epipoles = new PointH[] { e1, e2 };
return F;
}
/// <summary>
/// Converts to a Integer point by rounding the point coordinates.
/// </summary>
///
public static Point Round(PointH point)
{
return new Point(
(int)System.Math.Round(point.px / point.pw),
(int)System.Math.Round(point.py / point.pw));
}
/// <summary>
/// Detects if three points are collinear.
/// </summary>
public static bool Collinear(PointH pt1, PointH pt2, PointH pt3)
{
return Math.Abs(
(pt1.Y * pt2.W - pt1.W * pt2.Y) * pt3.X +
(pt1.W * pt2.X - pt1.X * pt2.W) * pt3.Y +
(pt1.X * pt2.Y - pt1.Y * pt2.X) * pt3.W) < Constants.SingleEpsilon;
}
/// <summary>
/// Converts to a Integer point by truncating the point coordinates.
/// </summary>
///
public static Point Truncate(PointH point)
{
return new Point(
(int)System.Math.Truncate(point.px / point.pw),
(int)System.Math.Truncate(point.py / point.pw));
}
