/// <summary>
/// Returns the dot product of two vectors whose
/// dimensions should be [3] or [3,1].
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="V1">First Matrix object (dimension [3,1]) in the dot product</param>
/// <param name="V2">Second Matrix object (dimension [3,1]) in the dot product</param>
/// <returns>Dot product of V1 and V2</returns>
public static double DotProduct(NMatrix V1, NMatrix V2)
{
return (DotProduct(V1.in_Mat, V2.in_Mat));
}
/// <summary>
/// Returns the Eigenvalues and Eigenvectors of a real symmetric
/// matrix, which is of dimensions [n,n]. In case of an error the
/// error is raised as an exception.
/// Note: This method is based on the 'Eigenvalues and Eigenvectors of a TridiagonalMatrix'
/// section of Numerical Recipes in C by William H. Press,
/// Saul A. Teukolsky, William T. Vetterling and Brian P. Flannery,
/// University of Cambridge Press 1992.
/// </summary>
/// <param name="Mat">
/// The Matrix object whose Eigenvalues and Eigenvectors are to be found
/// </param>
/// <param name="d">A Matrix object where the eigenvalues are returned</param>
/// <param name="v">A Matrix object where the eigenvectors are returned</param>
public static void Eigen(NMatrix Mat, out NMatrix d, out NMatrix v)
{
double[,] D, V;
Eigen(Mat.in_Mat, out D, out V);
d = new NMatrix(D);
v = new NMatrix(V);
}
/// <summary>
/// Returns a matrix as a string, so it can be viewed
/// in a multi-text textbox or in a richtextBox (preferred).
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">The Matrix object to be viewed</param>
/// <returns>The string view of the Matrix object</returns>
public static string PrintMat(NMatrix Mat)
{
return (PrintMat(Mat.in_Mat));
}
/// <summary>
/// Returns the determinant of a matrix with [n,n] dimension.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">
/// Matrix object with [n,n] dimension whose determinant is to be found
/// </param>
/// <returns>Determinant of the Matrix object</returns>
public static double Det(NMatrix Mat)
{
return Det(Mat.in_Mat);
}
public static bool TryGaussJordanElimination(NMatrix a, NMatrix b, out NMatrix result)
{
if (a.NoRows != b.NoRows)
{
throw new ArgumentException();
}
result = null;
NMatrix lhs = a.Copy();
NMatrix rhs = b.Copy();
int rowCount = lhs.NoRows;
int colCount = rhs.NoCols + lhs.NoCols;
// augment lhs array with rhs array and store in arr2
var matrixArray = new double[rowCount, colCount];
for (int row = 0; row < rowCount; ++row)
{
for (int col = 0; col < lhs.NoCols; ++col)
{
matrixArray[row, col] = lhs[row, col];
}
for (int col = 0; col < rhs.NoCols; ++col)
{
matrixArray[row, lhs.NoCols + col] = rhs[row, col];
}
}
// perform forward elimination to get arr2 in row-echelon form
for (int row = 0; row < rowCount; ++row)
{
// run along diagonal, swapping rows to move zeros in working position
// (along the diagonal) downwards
if (IsZero(matrixArray[row, row]))
{
if (row == (rowCount - 1))
{
return(false); // no solution
}
}
for (int insideRow = row + 1; insideRow < rowCount; insideRow++)
{
if (!IsZero(matrixArray[insideRow, row]))
{
SwapRows(matrixArray, row, insideRow);
break;
}
}
// divide working row by value of working position to get a 1 on the
// diagonal
if (IsZero(matrixArray[row, row]))
{
return(false);
}
double diagonal = matrixArray[row, row];
for (int col = 0; col < colCount; ++col)
{
matrixArray[row, col] /= diagonal;
}
// eliminate value below working position by subtracting a multiple of
// the current row
for (int insideRow = row + 1; insideRow < rowCount; ++insideRow)
{
double coefficient = matrixArray[insideRow, insideRow];
for (int col = 0; col < colCount; ++col)
{
matrixArray[insideRow, col] -= coefficient * matrixArray[insideRow, col];
}
}
}
// backward substitution steps
for (int dindex = rowCount - 1; dindex >= 0; --dindex)
{
// eliminate value above working position by subtracting a multiple of
// the current row
for (int row = dindex - 1; row >= 0; --row)
{
double coefficient = matrixArray[row, dindex];
for (int col = 0; col < colCount; ++col)
{
matrixArray[row, col] -= coefficient * matrixArray[dindex, col];
}
}
}
result = new NMatrix(matrixArray);
return(true);
}
public Line GetLine(PointUV pointUV)
{
var unitMatrix = new NMatrix(new double[,] { { pointUV.U }, { pointUV.V }, { 1 } });
NMatrix solution;
if (!NMatrix.TryGaussJordanElimination(Homography, unitMatrix, out solution))
throw new ArgumentException();
//a = Vector.Create(solution[0, 3], solution[1, 3], solution[2, 3]);
//b = Vector.Create(solution[0, 4], solution[1, 4], solution[2, 4]);
////var vector = Vector.Create(pointUV.U * a.X, pointUV.V * a.Y, a.Z);
//return Line.Create(Point.Origin + a, b.Direction);
return Line.Create(Point.Origin, Direction.Create(
solution[0, 4] / (solution[0, 3] + 1),
solution[1, 4] / (solution[1, 3] + 1),
solution[2, 4] / (solution[2, 3] + 1)
));
}
/// <summary>
/// Evaluates the Singular Value Decomposition of a matrix,
/// returns the matrices S, U and V. Such that a given
/// Matrix = U x S x V'.
/// In case of an error the error is raised as an exception.
/// Note: This method is based on the 'Singular Value Decomposition'
/// section of Numerical Recipes in C by William H. Press,
/// Saul A. Teukolsky, William T. Vetterling and Brian P. Flannery,
/// University of Cambridge Press 1992.
/// </summary>
/// <param name="Mat">Matrix object whose SVD is to be computed</param>
/// <param name="S">A Matrix object where the S matrix is returned</param>
/// <param name="U">A Matrix object where the U matrix is returned</param>
/// <param name="V">A Matrix object where the V matrix is returned</param>
public static void SVD(NMatrix Mat, out NMatrix S, out NMatrix U, out NMatrix V)
{
double[,] s, u, v;
SVD(Mat.in_Mat, out s, out u, out v);
S = new NMatrix(s);
U = new NMatrix(u);
V = new NMatrix(v);
}
/// <summary>
/// Returns the LU Decomposition of a matrix.
/// the output is: lower triangular matrix L, upper
/// triangular matrix U, and permutation matrix P so that
/// P*X = L*U.
/// In case of an error the error is raised as an exception.
/// Note: This method is based on the 'LU Decomposition and Its Applications'
/// section of Numerical Recipes in C by William H. Press,
/// Saul A. Teukolsky, William T. Vetterling and Brian P. Flannery,
/// University of Cambridge Press 1992.
/// </summary>
/// <param name="Mat">Matrix object which will be LU Decomposed</param>
/// <param name="L">A Matrix object where the lower traingular matrix is returned</param>
/// <param name="U">A Matrix object where the upper traingular matrix is returned</param>
/// <param name="P">A Matrix object where the permutation matrix is returned</param>
public static void LU(NMatrix Mat, out NMatrix L, out NMatrix U, out NMatrix P)
{
double[,] l, u, p;
LU(Mat.in_Mat, out l, out u, out p);
L = new NMatrix(l);
U = new NMatrix(u);
P = new NMatrix(p);
}
/// <summary>
/// Solves a set of n linear equations A.X = B, and returns
/// X, where A is [n,n] and B is [n,1].
/// In the same manner if you need to compute: inverse(A).B, it is
/// better to use this method instead, as it is much faster.
/// In case of an error the error is raised as an exception.
/// Note: This method is based on the 'LU Decomposition and Its Applications'
/// section of Numerical Recipes in C by William H. Press,
/// Saul A. Teukolsky, William T. Vetterling and Brian P. Flannery,
/// University of Cambridge Press 1992.
/// </summary>
/// <param name="MatA">Matrix object 'A' on the left side of the equations A.X = B</param>
/// <param name="MatB">Matrix object 'B' on the right side of the equations A.X = B</param>
/// <returns>Matrix object 'X' in the system of equations A.X = B</returns>
public static NMatrix SolveLinear(NMatrix MatA, NMatrix MatB)
{
return new NMatrix(NMatrix.SolveLinear(MatA.in_Mat, MatB.in_Mat));
}
/// <summary>
/// Returns the difference of two matrices with compatible
/// dimensions.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat1">First matrix in the subtraction</param>
/// <param name="Mat2">Second matrix in the subtraction</param>
/// <returns>Difference of Mat1 and Mat2 as a Matrix object</returns>
public static NMatrix Subtract(NMatrix Mat1, NMatrix Mat2)
{
return new NMatrix(Subtract(Mat1.in_Mat, Mat2.in_Mat));
}
/// <summary>
/// Returns the multiplication of a matrix or a vector (i.e
/// dimension [3,1]) with a scalar quantity.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Value">The scalar value to multiply the array</param>
/// <param name="Mat">Matrix which is to be multiplied by a scalar</param>
/// <returns>The multiplication of the scalar and the array as an array</returns>
public static NMatrix ScalarMultiply(double Value, NMatrix Mat)
{
return new NMatrix(ScalarMultiply(Value, Mat.in_Mat));
}
/// <summary>
/// Returns the division of a matrix or a vector (i.e
/// dimension [3,1]) by a scalar quantity.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Value">The scalar value to divide the array with</param>
/// <param name="Mat">Matrix which is to be divided by a scalar</param>
/// <returns>The division of the array and the scalar as an array</returns>
public static NMatrix ScalarDivide(double Value, NMatrix Mat)
{
return new NMatrix(ScalarDivide(Value, Mat.in_Mat));
}
/// <summary>
/// Returns the rank of a matrix.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">a Matrix object whose rank is to be found</param>
/// <returns>The rank of the Matrix object</returns>
public static int Rank(NMatrix Mat)
{
return Rank(Mat.in_Mat);
}
/// <summary>
/// Returns the inverse of a matrix with [n,n] dimension
/// and whose determinant is not zero.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">
/// Matrix object with [n,n] dimension whose inverse is to be found
/// </param>
/// <returns>Inverse of the matrix as a Matrix object</returns>
public static NMatrix Inverse(NMatrix Mat)
{
return new NMatrix(Inverse(Mat.in_Mat));
}
/// <summary>
/// Returns the transpose of a matrix.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">Matrix object whose transpose is to be found</param>
/// <returns>Transpose of the Matrix object as a Matrix object</returns>
public static NMatrix Transpose(NMatrix Mat)
{
return new NMatrix(Transpose(Mat.in_Mat));
}
/// <summary>
/// Checks if two matrices of equal dimensions are equal or not.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat1">First Matrix in equality check</param>
/// <param name="Mat2">Second Matrix in equality check</param>
/// <returns>If two matrices are equal or not</returns>
public static bool IsEqual(NMatrix Mat1, NMatrix Mat2)
{
return IsEqual(Mat1.in_Mat, Mat2.in_Mat);
}
/// <summary>
/// Returns the magnitude of a vector whose dimension is [3] or [3,1].
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="V">Matrix object (dimension [3,1]) whose magnitude is to be found</param>
/// <returns>The magnitude of the Matrix object</returns>
public static double VectorMagnitude(NMatrix V)
{
return (VectorMagnitude(V.in_Mat));
}
/// <summary>
/// Returns the multiplication of two matrices with compatible
/// dimensions OR the cross-product of two vectors.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat1">
/// First matrix or vector (i.e: dimension [3,1]) object in
/// multiplication
/// </param>
/// <param name="Mat2">
/// Second matrix or vector (i.e: dimension [3,1]) object in
/// multiplication
/// </param>
/// <returns>Mat1 multiplied by Mat2 as a Matrix object</returns>
public static NMatrix Multiply(NMatrix Mat1, NMatrix Mat2)
{
if ((Mat1.NoRows == 3) && (Mat2.NoRows == 3) &&
(Mat1.NoCols == 1) && (Mat1.NoCols == 1)) { return new NMatrix(CrossProduct(Mat1.in_Mat, Mat2.in_Mat)); }
else { return new NMatrix(Multiply(Mat1.in_Mat, Mat2.in_Mat)); }
}
/// <summary>
/// Returns the summation of two matrices with compatible
/// dimensions.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat1">First matrix in the summation</param>
/// <param name="Mat2">Second matrix in the summation</param>
/// <returns>Sum of Mat1 and Mat2 as a Matrix object</returns>
public static NMatrix Add(NMatrix Mat1, NMatrix Mat2)
{
return new NMatrix(Add(Mat1.in_Mat, Mat2.in_Mat));
}
public void Calibrate()
{
//CalibrationPoints.Add(PointUV.Create(-100, -100));
//CalibrationPoints.Add(PointUV.Create(-100, 100));
//CalibrationPoints.Add(PointUV.Create(100, -100));
//CalibrationPoints.Add(PointUV.Create(100, 100));
//CalibrationPoints.Add(PointUV.Create(-80, -80));
//CalibrationPoints.Add(PointUV.Create(-80, 80));
//CalibrationPoints.Add(PointUV.Create(80, -80));
//CalibrationPoints.Add(PointUV.Create(80, 80));
double h = (double)imageSize.Height;
CalibrationPoints.Add(PointUV.Create(0, 0));
CalibrationPoints.Add(PointUV.Create(0, h));
CalibrationPoints.Add(PointUV.Create(h, 0));
CalibrationPoints.Add(PointUV.Create(h, h));
h *= 2;
CalibrationPoints.Add(PointUV.Create(0, 0));
CalibrationPoints.Add(PointUV.Create(0, h));
CalibrationPoints.Add(PointUV.Create(h, 0));
CalibrationPoints.Add(PointUV.Create(h, h));
NMatrix calibrationMatrixA = new NMatrix(2 * CalibrationPoints.Count, 11);
NMatrix calibrationMatrixB = new NMatrix(2 * CalibrationPoints.Count, 1);
for (int i = 0; i < CalibrationPoints.Count; i++) {
double x = controlForm.CalibrationPoints[i].X;
double y = controlForm.CalibrationPoints[i].Y;
double z = controlForm.CalibrationPoints[i].Z;
double u = CalibrationPoints[i].U;
double v = CalibrationPoints[i].V;
calibrationMatrixA[2 * i, 0] = x;
calibrationMatrixA[2 * i, 1] = y;
calibrationMatrixA[2 * i, 2] = z;
calibrationMatrixA[2 * i, 3] = 1;
calibrationMatrixA[2 * i, 8] = -u * x;
calibrationMatrixA[2 * i, 9] = -u * y;
calibrationMatrixA[2 * i, 10] = -u * z;
calibrationMatrixA[2 * i + 1, 4] = x;
calibrationMatrixA[2 * i + 1, 5] = y;
calibrationMatrixA[2 * i + 1, 6] = z;
calibrationMatrixA[2 * i + 1, 7] = 1;
calibrationMatrixA[2 * i + 1, 8] = -v * x;
calibrationMatrixA[2 * i + 1, 9] = -v * y;
calibrationMatrixA[2 * i + 1, 10] = -v * z;
calibrationMatrixB[2 * i, 0] = u;
calibrationMatrixB[2 * i + 1, 0] = v;
}
NMatrix calibrationMatrixAT = NMatrix.Transpose(calibrationMatrixA);
NMatrix homographyData = NMatrix.Inverse(calibrationMatrixAT * calibrationMatrixA) * calibrationMatrixAT * calibrationMatrixB;
Homography = new NMatrix(new double[,]{
{homographyData[0,0], homographyData[1,0], homographyData[2,0], homographyData[3,0]} ,
{homographyData[4,0], homographyData[5,0], homographyData[6,0], homographyData[7,0]} ,
{homographyData[8,0], homographyData[9,0], homographyData[10,0], 1}
});
}
/// <summary>
/// Returns the cross product of two vectors whose
/// dimensions should be [3] or [3x1].
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="V1">First Matrix (dimensions [3,1]) in the cross product</param>
/// <param name="V2">Second Matrix (dimensions [3,1]) in the cross product</param>
/// <returns>Cross product of V1 and V2 as a matrix (dimension [3,1])</returns>
public static NMatrix CrossProduct(NMatrix V1, NMatrix V2)
{
return (new NMatrix((CrossProduct(V1.in_Mat, V2.in_Mat))));
}
public static bool TryGaussJordanElimination(NMatrix a, NMatrix b, out NMatrix result)
{
if (a.NoRows != b.NoRows)
throw new ArgumentException();
result = null;
NMatrix lhs = a.Copy();
NMatrix rhs = b.Copy();
int rowCount = lhs.NoRows;
int colCount = rhs.NoCols + lhs.NoCols;
// augment lhs array with rhs array and store in arr2
var matrixArray = new double[rowCount, colCount];
for (int row = 0; row < rowCount; ++row) {
for (int col = 0; col < lhs.NoCols; ++col) {
matrixArray[row, col] = lhs[row, col];
}
for (int col = 0; col < rhs.NoCols; ++col) {
matrixArray[row, lhs.NoCols + col] = rhs[row, col];
}
}
// perform forward elimination to get arr2 in row-echelon form
for (int row = 0; row < rowCount; ++row) {
// run along diagonal, swapping rows to move zeros in working position
// (along the diagonal) downwards
if (IsZero(matrixArray[row, row]))
if (row == (rowCount - 1))
return false; // no solution
for (int insideRow = row + 1; insideRow < rowCount; insideRow++) {
if (!IsZero(matrixArray[insideRow, row])) {
SwapRows(matrixArray, row, insideRow);
break;
}
}
// divide working row by value of working position to get a 1 on the
// diagonal
if (IsZero(matrixArray[row, row]))
return false;
double diagonal = matrixArray[row, row];
for (int col = 0; col < colCount; ++col)
matrixArray[row, col] /= diagonal;
// eliminate value below working position by subtracting a multiple of
// the current row
for (int insideRow = row + 1; insideRow < rowCount; ++insideRow) {
double coefficient = matrixArray[insideRow, insideRow];
for (int col = 0; col < colCount; ++col)
matrixArray[insideRow, col] -= coefficient * matrixArray[insideRow, col];
}
}
// backward substitution steps
for (int dindex = rowCount - 1; dindex >= 0; --dindex) {
// eliminate value above working position by subtracting a multiple of
// the current row
for (int row = dindex - 1; row >= 0; --row) {
double coefficient = matrixArray[row, dindex];
for (int col = 0; col < colCount; ++col)
matrixArray[row, col] -= coefficient * matrixArray[dindex, col];
}
}
result = new NMatrix(matrixArray);
return true;
}
/// <summary>
/// Returns the pseudoinverse of a matrix, such that
/// X = PINV(A) produces a matrix 'X' of the same dimensions
/// as A' so that A*X*A = A, X*A*X = X.
/// In case of an error the error is raised as an exception.
/// </summary>
/// <param name="Mat">a Matrix object whose pseudoinverse is to be found</param>
/// <returns>The pseudoinverse of the Matrix object as a Matrix Object</returns>
public static NMatrix PINV(NMatrix Mat)
{
return new NMatrix(PINV(Mat.in_Mat));
}
