public void MatrixSolve4()
{
Matrix ma = new Matrix(new double[][] { new double[] { 1, 2, 3 }, new double[] { 5, 7, 11 }, new double[] { 13, 17, 19 } });
Matrix mx = new Matrix(new double[][] { new double[] { 23 }, new double[] { 29 }, new double[] { 31 } });
Matrix mb = ma * mx;
Assert.That(ma.Solve(mb), NumericIs.AlmostEqualTo(mx));
}
public void SolveLinearSystem(
[Values(10, 20, 50, 100, 200, 500)] int n)
{
double[][] a = Matrix.CreateMatrixData(n, n);
double[][] x = Matrix.CreateMatrixData(n, 1);
for(int i = 0; i < n; i++)
{
for(int j = 0; j < n; j++)
{
if(j == 0)
{
x[i][0] = Rnd.NextDouble();
}
a[i][j] = Rnd.NextDouble();
}
}
Matrix ma = new Matrix(a);
Matrix mx = new Matrix(x);
Matrix mb = ma*mx;
Assert.That(ma.Solve(mb), NumericIs.AlmostEqualTo(mx, 1e-10));
}
public void MatrixSolve3()
{
Matrix ma = new Matrix(new double[][] { new double[] { 1, 2 }, new double[] { 3, 5 } });
Matrix mx = new Matrix(new double[][] { new double[] { 7 }, new double[] { 11.0 } });
Matrix mb = ma * mx;
Assert.That(ma.Solve(mb), NumericIs.AlmostEqualTo(mx));
}
public void MatrixSolve2()
{
Matrix ma = new Matrix(new double[][] { new double[] { 1, 2 }, new double[] { 3, 5 } });
Matrix mb = new Matrix(new double[][] { new double[] { 29.0 }, new double[] { 76.0 } });
Matrix mr = new Matrix(new double[][] { new double[] { 7 }, new double[] { 11.0 } });
Matrix mx = ma.Solve(mb);
Assert.That(mx, NumericIs.AlmostEqualTo(mr));
Assert.That(ma * mx, NumericIs.AlmostEqualTo(mb));
}
public void AllTests()
{
Matrix A, B, C, Z, O, I, R, S, X, SUB, M, T, SQ, DEF, SOL;
double tmp;
double[] columnwise = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0 };
double[] rowwise = { 1.0, 4.0, 7.0, 10.0, 2.0, 5.0, 8.0, 11.0, 3.0, 6.0, 9.0, 12.0 };
double[][] avals = { new double[] { 1.0, 4.0, 7.0, 10.0 }, new double[] { 2.0, 5.0, 8.0, 11.0 }, new double[] { 3.0, 6.0, 9.0, 12.0 } };
double[][] rankdef = avals;
double[][] tvals = { new double[] { 1.0, 2.0, 3.0 }, new double[] { 4.0, 5.0, 6.0 }, new double[] { 7.0, 8.0, 9.0 }, new double[] { 10.0, 11.0, 12.0 } };
double[][] subavals = { new double[] { 5.0, 8.0, 11.0 }, new double[] { 6.0, 9.0, 12.0 } };
double[][] pvals = { new double[] { 25, -5, 10 }, new double[] { -5, 17, 10 }, new double[] { 10, 10, 62 } };
double[][] ivals = { new double[] { 1.0, 0.0, 0.0, 0.0 }, new double[] { 0.0, 1.0, 0.0, 0.0 }, new double[] { 0.0, 0.0, 1.0, 0.0 } };
double[][] evals = { new double[] { 0.0, 1.0, 0.0, 0.0 }, new double[] { 1.0, 0.0, 2e-7, 0.0 }, new double[] { 0.0, -2e-7, 0.0, 1.0 }, new double[] { 0.0, 0.0, 1.0, 0.0 } };
double[][] square = { new double[] { 166.0, 188.0, 210.0 }, new double[] { 188.0, 214.0, 240.0 }, new double[] { 210.0, 240.0, 270.0 } };
double[][] sqSolution = { new double[] { 13.0 }, new double[] { 15.0 } };
double[][] condmat = { new double[] { 1.0, 3.0 }, new double[] { 7.0, 9.0 } };
int rows = 3, cols = 4;
int invalidld = 5; /* should trigger bad shape for construction with val */
int validld = 3; /* leading dimension of intended test Matrices */
int nonconformld = 4; /* leading dimension which is valid, but nonconforming */
int ib = 1, ie = 2, jb = 1, je = 3; /* index ranges for sub Matrix */
int[] rowindexset = new int[] { 1, 2 };
int[] badrowindexset = new int[] { 1, 3 };
int[] columnindexset = new int[] { 1, 2, 3 };
int[] badcolumnindexset = new int[] { 1, 2, 4 };
double columnsummax = 33.0;
double rowsummax = 30.0;
double sumofdiagonals = 15;
double sumofsquares = 650;
#region Testing constructors and constructor-like methods
// Constructors and constructor-like methods:
// double[], int
// double[,]
// int, int
// int, int, double
// int, int, double[,]
// Create(double[,])
// Random(int,int)
// Identity(int)
try
{
// check that exception is thrown in packed constructor with invalid length
A = new Matrix(columnwise, invalidld);
Assert.Fail("Catch invalid length in packed constructor: exception not thrown for invalid input");
}
catch(ArgumentException) { }
A = new Matrix(columnwise, validld);
B = new Matrix(avals);
tmp = B[0, 0];
avals[0][0] = 0.0;
C = B - A;
avals[0][0] = tmp;
B = Matrix.Create(avals);
tmp = B[0, 0];
avals[0][0] = 0.0;
Assert.AreEqual((tmp - B[0, 0]), 0.0, "Create");
avals[0][0] = columnwise[0];
I = new Matrix(ivals);
NumericAssert.AreAlmostEqual(I, Matrix.Identity(3, 4), "Identity");
#endregion
#region Testing access methods
// Access Methods:
// getColumnDimension()
// getRowDimension()
// getArray()
// getArrayCopy()
// getColumnPackedCopy()
// getRowPackedCopy()
// get(int,int)
// GetMatrix(int,int,int,int)
// GetMatrix(int,int,int[])
// GetMatrix(int[],int,int)
// GetMatrix(int[],int[])
// set(int,int,double)
// SetMatrix(int,int,int,int,Matrix)
// SetMatrix(int,int,int[],Matrix)
// SetMatrix(int[],int,int,Matrix)
// SetMatrix(int[],int[],Matrix)
// Various get methods
B = new Matrix(avals);
Assert.AreEqual(B.RowCount, rows, "getRowDimension");
Assert.AreEqual(B.ColumnCount, cols, "getColumnDimension");
B = new Matrix(avals);
double[][] barray = (Matrix)B;
Assert.AreSame(barray, avals, "getArray");
barray = B.Clone();
Assert.AreNotSame(barray, avals, "getArrayCopy");
NumericAssert.AreAlmostEqual(new Matrix(barray), B, "getArrayCopy II");
// double[] bpacked = B.ColumnPackedCopy;
// try
// {
// check(bpacked, columnwise);
// try_success("getColumnPackedCopy... ", "");
// }
// catch (System.SystemException e)
// {
// errorCount = try_failure(errorCount, "getColumnPackedCopy... ", "data not successfully (deep) copied by columns");
// System.Console.Out.WriteLine(e.Message);
// }
// bpacked = B.RowPackedCopy;
// try
// {
// check(bpacked, rowwise);
// try_success("getRowPackedCopy... ", "");
// }
// catch (System.SystemException e)
// {
// errorCount = try_failure(errorCount, "getRowPackedCopy... ", "data not successfully (deep) copied by rows");
// System.Console.Out.WriteLine(e.Message);
// }
try
{
tmp = B[B.RowCount, B.ColumnCount - 1];
Assert.Fail("get(int,int): OutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
tmp = B[B.RowCount - 1, B.ColumnCount];
Assert.Fail("get(int,int): OutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
Assert.AreEqual(B[B.RowCount - 1, B.ColumnCount - 1], avals[B.RowCount - 1][B.ColumnCount - 1], "get(int,int)");
SUB = new Matrix(subavals);
try
{
M = B.GetMatrix(ib, ie + B.RowCount + 1, jb, je);
Assert.Fail("GetMatrix(int,int,int,int): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
M = B.GetMatrix(ib, ie, jb, je + B.ColumnCount + 1);
Assert.Fail("GetMatrix(int,int,int,int): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
M = B.GetMatrix(ib, ie, jb, je);
NumericAssert.AreAlmostEqual(SUB, M, "GetMatrix(int,int,int,int)");
try
{
M = B.GetMatrix(ib, ie, badcolumnindexset);
Assert.Fail("GetMatrix(int,int,int[]): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
M = B.GetMatrix(ib, ie + B.RowCount + 1, columnindexset);
Assert.Fail("GetMatrix(int,int,int[]): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
M = B.GetMatrix(ib, ie, columnindexset);
NumericAssert.AreAlmostEqual(SUB, M, "GetMatrix(int,int,int[])");
try
{
M = B.GetMatrix(badrowindexset, jb, je);
Assert.Fail("GetMatrix(int[],int,int): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
M = B.GetMatrix(rowindexset, jb, je + B.ColumnCount + 1);
Assert.Fail("GetMatrix(int[],int,int): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
M = B.GetMatrix(rowindexset, jb, je);
NumericAssert.AreAlmostEqual(SUB, M, "GetMatrix(int[],int,int)");
try
{
M = B.GetMatrix(badrowindexset, columnindexset);
Assert.Fail("GetMatrix(int[],int[]): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
M = B.GetMatrix(rowindexset, badcolumnindexset);
Assert.Fail("GetMatrix(int[],int[]): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
M = B.GetMatrix(rowindexset, columnindexset);
NumericAssert.AreAlmostEqual(SUB, M, "GetMatrix(int[],int[])");
// Various set methods:
try
{
B[B.RowCount, B.ColumnCount - 1] = 0.0;
Assert.Fail("set(int,int,double): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
B[B.RowCount - 1, B.ColumnCount] = 0.0;
Assert.Fail("set(int,int,double): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
B[ib, jb] = 0.0;
tmp = B[ib, jb];
NumericAssert.AreAlmostEqual(tmp, 0.0, "set(int,int,double)");
M = new Matrix(2, 3, 0.0);
try
{
B.SetMatrix(ib, ie + B.RowCount + 1, jb, je, M);
Assert.Fail("SetMatrix(int,int,int,int,Matrix): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
B.SetMatrix(ib, ie, jb, je + B.ColumnCount + 1, M);
Assert.Fail("SetMatrix(int,int,int,int,Matrix): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
B.SetMatrix(ib, ie, jb, je, M);
NumericAssert.AreAlmostEqual(M - B.GetMatrix(ib, ie, jb, je), M, "SetMatrix(int,int,int,int,Matrix)");
B.SetMatrix(ib, ie, jb, je, SUB);
try
{
B.SetMatrix(ib, ie + B.RowCount + 1, columnindexset, M);
Assert.Fail("SetMatrix(int,int,int[],Matrix): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
B.SetMatrix(ib, ie, badcolumnindexset, M);
Assert.Fail("SetMatrix(int,int,int[],Matrix): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
B.SetMatrix(ib, ie, columnindexset, M);
NumericAssert.AreAlmostEqual(M - B.GetMatrix(ib, ie, columnindexset), M, "SetMatrix(int,int,int[],Matrix)");
B.SetMatrix(ib, ie, jb, je, SUB);
try
{
B.SetMatrix(rowindexset, jb, je + B.ColumnCount + 1, M);
Assert.Fail("SetMatrix(int[],int,int,Matrix): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
B.SetMatrix(badrowindexset, jb, je, M);
Assert.Fail("SetMatrix(int[],int,int,Matrix): IndexOutOfBoundsException expected but not thrown II");
}
catch(IndexOutOfRangeException) { }
B.SetMatrix(rowindexset, jb, je, M);
NumericAssert.AreAlmostEqual(M - B.GetMatrix(rowindexset, jb, je), M, "SetMatrix(int[],int,int,Matrix)");
B.SetMatrix(ib, ie, jb, je, SUB);
try
{
B.SetMatrix(rowindexset, badcolumnindexset, M);
Assert.Fail("SetMatrix(int[],int[],Matrix): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
try
{
B.SetMatrix(badrowindexset, columnindexset, M);
Assert.Fail("SetMatrix(int[],int[],Matrix): IndexOutOfBoundsException expected but not thrown");
}
catch(IndexOutOfRangeException) { }
B.SetMatrix(rowindexset, columnindexset, M);
NumericAssert.AreAlmostEqual(M - B.GetMatrix(rowindexset, columnindexset), M, "SetMatrix(int[],int[],Matrix)");
#endregion
#region Testing array-like methods
// Array-like methods:
// Subtract
// SubtractEquals
// Add
// AddEquals
// ArrayLeftDivide
// ArrayLeftDivideEquals
// ArrayRightDivide
// ArrayRightDivideEquals
// arrayTimes
// ArrayMultiplyEquals
// uminus
S = new Matrix(columnwise, nonconformld);
R = Matrix.Random(A.RowCount, A.ColumnCount);
A = R;
try
{
S = A - S;
Assert.Fail("Subtract conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
Assert.AreEqual((A - R).Norm1(), 0.0, "Subtract: difference of identical Matrices is nonzero,\nSubsequent use of Subtract should be suspect");
A = R.Clone();
A.Subtract(R);
Z = new Matrix(A.RowCount, A.ColumnCount);
try
{
A.Subtract(S);
Assert.Fail("SubtractEquals conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
Assert.AreEqual((A - Z).Norm1(), 0.0, "SubtractEquals: difference of identical Matrices is nonzero,\nSubsequent use of Subtract should be suspect");
A = R.Clone();
B = Matrix.Random(A.RowCount, A.ColumnCount);
C = A - B;
try
{
S = A + S;
Assert.Fail("Add conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
NumericAssert.AreAlmostEqual(C + B, A, "Add");
C = A - B;
C.Add(B);
try
{
A.Add(S);
Assert.Fail("AddEquals conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
NumericAssert.AreAlmostEqual(C, A, "AddEquals");
A = ((Matrix)R.Clone());
A.UnaryMinus();
NumericAssert.AreAlmostEqual(A + R, Z, "UnaryMinus");
A = (Matrix)R.Clone();
O = new Matrix(A.RowCount, A.ColumnCount, 1.0);
try
{
Matrix.ArrayDivide(A, S);
Assert.Fail("ArrayRightDivide conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
C = Matrix.ArrayDivide(A, R);
NumericAssert.AreAlmostEqual(C, O, "ArrayRightDivide");
try
{
A.ArrayDivide(S);
Assert.Fail("ArrayRightDivideEquals conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
A.ArrayDivide(R);
NumericAssert.AreAlmostEqual(A, O, "ArrayRightDivideEquals");
A = (Matrix)R.Clone();
B = Matrix.Random(A.RowCount, A.ColumnCount);
try
{
S = Matrix.ArrayMultiply(A, S);
Assert.Fail("arrayTimes conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
C = Matrix.ArrayMultiply(A, B);
C.ArrayDivide(B);
NumericAssert.AreAlmostEqual(C, A, "arrayTimes");
try
{
A.ArrayMultiply(S);
Assert.Fail("ArrayMultiplyEquals conformance check: nonconformance not raised");
}
catch(ArgumentException) { }
A.ArrayMultiply(B);
A.ArrayDivide(B);
NumericAssert.AreAlmostEqual(A, R, "ArrayMultiplyEquals");
#endregion
#region Testing linear algebra methods
// LA methods:
// Transpose
// Multiply
// Condition
// Rank
// Determinant
// trace
// Norm1
// norm2
// normF
// normInf
// Solve
// solveTranspose
// Inverse
// chol
// Eigen
// lu
// qr
// svd
A = new Matrix(columnwise, 3);
T = new Matrix(tvals);
T = Matrix.Transpose(A);
NumericAssert.AreAlmostEqual(Matrix.Transpose(A), T, "Transpose");
NumericAssert.AreAlmostEqual(A.Norm1(), columnsummax, "Norm1");
NumericAssert.AreAlmostEqual(A.NormInf(), rowsummax, "NormInf");
NumericAssert.AreAlmostEqual(A.NormF(), Math.Sqrt(sumofsquares), "NormF");
NumericAssert.AreAlmostEqual(A.Trace(), sumofdiagonals, "Trace");
NumericAssert.AreAlmostEqual(A.GetMatrix(0, A.RowCount - 1, 0, A.RowCount - 1).Determinant(), 0.0, "Determinant");
SQ = new Matrix(square);
NumericAssert.AreAlmostEqual(A * Matrix.Transpose(A), SQ, "Multiply(Matrix)");
NumericAssert.AreAlmostEqual(0.0 * A, Z, "Multiply(double)");
A = new Matrix(columnwise, 4);
QRDecomposition QR = A.QRDecomposition;
R = QR.R;
NumericAssert.AreAlmostEqual(A, QR.Q * R, "QRDecomposition");
SingularValueDecomposition SVD = A.SingularValueDecomposition;
NumericAssert.AreAlmostEqual(A, SVD.LeftSingularVectors * (SVD.S * Matrix.Transpose(SVD.RightSingularVectors)), "SingularValueDecomposition");
DEF = new Matrix(rankdef);
NumericAssert.AreAlmostEqual(DEF.Rank(), Math.Min(DEF.RowCount, DEF.ColumnCount) - 1, "Rank");
B = new Matrix(condmat);
SVD = B.SingularValueDecomposition;
double[] singularvalues = SVD.SingularValues;
NumericAssert.AreAlmostEqual(B.Condition(), singularvalues[0] / singularvalues[Math.Min(B.RowCount, B.ColumnCount) - 1], "Condition");
int n = A.ColumnCount;
A = A.GetMatrix(0, n - 1, 0, n - 1);
A[0, 0] = 0.0;
LUDecomposition LU = A.LUDecomposition;
NumericAssert.AreAlmostEqual(A.GetMatrix(LU.Pivot, 0, n - 1), LU.L * LU.U, "LUDecomposition");
X = A.Inverse();
NumericAssert.AreAlmostEqual(A * X, Matrix.Identity(3, 3), "Inverse");
O = new Matrix(SUB.RowCount, 1, 1.0);
SOL = new Matrix(sqSolution);
SQ = SUB.GetMatrix(0, SUB.RowCount - 1, 0, SUB.RowCount - 1);
NumericAssert.AreAlmostEqual(SQ.Solve(SOL), O, "Solve");
A = new Matrix(pvals);
CholeskyDecomposition Chol = A.CholeskyDecomposition;
Matrix L = Chol.TriangularFactor;
NumericAssert.AreAlmostEqual(A, L * Matrix.Transpose(L), "CholeskyDecomposition");
X = Chol.Solve(Matrix.Identity(3, 3));
NumericAssert.AreAlmostEqual(A * X, Matrix.Identity(3, 3), "CholeskyDecomposition Solve");
EigenvalueDecomposition Eig = A.EigenvalueDecomposition;
Matrix D = Eig.BlockDiagonal;
Matrix V = Eig.EigenVectors;
NumericAssert.AreAlmostEqual(A * V, V * D, "EigenvalueDecomposition (symmetric)");
A = new Matrix(evals);
Eig = A.EigenvalueDecomposition;
D = Eig.BlockDiagonal;
V = Eig.EigenVectors;
NumericAssert.AreAlmostEqual(A * V, V * D, "EigenvalueDecomposition (nonsymmetric)");
#endregion
}
public void CodeSample_PolynomialRegression()
{
double[] x = new double[] { 1000, 2000, 3000, 4000, 5000, 6000, 7000 };
double[] y = new double[] { -30, -60, -88, -123, -197, -209, -266 };
int polynomialOrder = 3;
// Build the matrix for the least-squares fitting
double[][] m = Matrix.CreateMatrixData(x.Length, polynomialOrder + 1);
for(int i = 0; i < x.Length; i++)
{
double xi = x[i];
double[] xrow = m[i];
xrow[0] = 1d;
for(int j = 1; j < xrow.Length; j++)
{
xrow[j] = xrow[j - 1] * xi;
}
}
// Find the least-squares solution
Matrix matrix = new Matrix(m);
Vector solution = matrix.Solve(y);
// Extract the values (in our case into a polynomial for fast evaluation)
Polynomial polynomial = new Polynomial(solution);
// Verify that the polynomial fits with less than 10% error for all given value pairs.
for(int i = 0; i < x.Length; i++)
{
Assert.That(polynomial.Evaluate(x[i]), NumericIs.AlmostEqualTo(y[i], 0.1), i.ToString());
}
}
/*Test a given solution by calculating b and then solving for x.
Shows only the elapsed time on console out so that we can use
matrices too large to print.*/
private Matrix TestMatrix_Solutions(Matrix ma, Matrix mx, double epsilon, bool showB)
{
Matrix mb = ma * mx;
if(showB)
{
Console.WriteLine("b");
Console.WriteLine(mb.ToString());
}
Matrix ms = null;
MyStopwatch.MethodToTime m = delegate
{
ms = ma.Solve(mb);
};
Console.Write("Solve Time (ms) for " + ma.ColumnCount + ": ");
MyStopwatch.Time(m);
Assert.IsTrue(CompareMatrices(ms, mx, epsilon), "Matrices should be equal");
//Assert.AreEqual(ms.ToString(), mx.ToString(), "Matrices should be equal");
return ms;
}
// Performs a Kalman update on track using measurement and overwrites track with new estimate
public GaussianTrack Estimate(List<GaussianMeasurement> measurements)
{
// Understand input
int N = measurements.Count;
// Check if we have sufficient samples to estimate
if (N < minBatchMeasurements)
{
return null;
}
// Sort the list by datetime
measurements.Sort((x,y) => x.dateTime.CompareTo(y.dateTime));
// Prealloc structures
Matrix A = new Matrix(3 * N, 6, 0);
Matrix g = new Matrix(3 * N, 1, 0);
Matrix R = new Matrix(3 * N, 3 * N, 0);
// Go through each measurements
for (int k = 0; k < N; k++)
{
// Compute time difference
double dt = measurements[k].dateTime.Subtract(measurements[N - 1].dateTime).TotalSeconds;
// Populate (3x6) block of A
A[3 * k, 0] = 1;
A[3 * k + 1, 1] = 1;
A[3 * k + 2, 2] = 1;
A[3 * k, 3] = dt;
A[3 * k + 1, 4] = dt;
A[3 * k + 2, 5] = dt;
// Convert measurement to Unity (track) space
Vector convMean;
Matrix convJacobian;
Coordinate.Convert(measurements[k].creatorUnityReference, measurements[k].coordinateType,
measurements[k].gaussianVector.mean, new Vector(3), Coordinate.Type.UNITY3,
out convMean, out convJacobian);
Matrix convJacobianT = convJacobian.Clone();
convJacobianT.Transpose();
Matrix convCovariance = convJacobian * measurements[k].gaussianVector.covariance * convJacobianT;
// Populate (3x1) block of g with converted measurement mean
g[3 * k, 0] = convMean[0];
g[3 * k + 1, 0] = convMean[1];
g[3 * k + 2, 0] = convMean[2];
// Copy coverted measurement covariance to (3x3) block in R matrix
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R[3*k + i, 3*k + j] = convCovariance[i, j];
}
}
}
// Finished building A, g, R matrices
// Compute weighted least squares fit
// Ps = inv(A'*inv(R)*A)
// Xs = Ps*A'*inv(R)*g
Matrix AT = A.Clone();
AT.Transpose();
Matrix Ps = (AT * R.Solve(A)).Inverse();
Vector Xs = (Ps * AT * R.Solve(g)).GetColumnVector(0);
// Create a new track and return
return new GaussianTrack(fusionEngine.RequestNewTrackID(),
new GaussianVector(Xs, Ps), Coordinate.Type.UNITY6,
measurements[N - 1].dateTime);
}
/*
* Gute Parameterwerte sind (mit angepasster GUI):
*
* a = 0.05
* Search Range = 50
* k = 5
* Ransac Steps = 10000
* Ransac Threshold = 0.1
*
*/
/// <summary>
/// Berechnet eine Homographie aus korrespondierenden Punktepaaren. Benutzt die Methode aus
/// "Computer Vision: Algorithms and Applications" von Richard Szeliski, http://szeliski.org/Book/. Kapitel 6.1,
/// insbesondere 6.1.3
/// </summary>
/// <param name="sourcePoints">Punkte im linken Bild</param>
/// <param name="targetPoints">Punkte im rechten Bild</param>
/// <returns>Homographiematrix</returns>
private double[,] getHomographyFromPoints(double[,] affineMatrix, PointF[] sourcePoints, PointF[] targetPoints)
{
//Bezeichnungen orientieren sich an den Bezeichnungein in 6.1.3
Matrix deltaP, A, b, ri;
ri = new Matrix(2, 1);
double lambda = 0;
//Jacobi Matrix und Transponierte
Matrix J, Jt;
//Anzahl der Matches
int nPoints = sourcePoints.Length;
//von der Homographie vorausgesagte Position von p (siehe (6.3))
PointF predLoc;
//Berechne Initial guess von deltaP: In unserem Fall sind die Bilder fast affin verschoben.
//Also ist die affine Schätzung ein guter Anfang
deltaP = new Matrix(8, 1);
//double[,] affineMatrix = getHomographyFromPointsOld(sourcePoints, targetPoints);
deltaP[0, 0] = affineMatrix[0, 0] - 1; //wegen Konvention in 6.1.3, zweiter Absatz
deltaP[1, 0] = affineMatrix[0, 1];
deltaP[2, 0] = affineMatrix[0, 2];
deltaP[3, 0] = affineMatrix[1, 0];
deltaP[4, 0] = affineMatrix[1, 1] - 1;
deltaP[5, 0] = affineMatrix[1, 2];
deltaP[6, 0] = affineMatrix[2, 0];
deltaP[7, 0] = affineMatrix[2, 1];
//Algorithmus iterieren
int iterations = 100;
A = new Matrix(8, 8);
b = new Matrix(8, 1);
for (int step = 0; step < iterations; step++)
{
//compute A und B:
for (int i = 0; i < nPoints; i++)
{
//Jacobimatrix berechnen
J = getJacobi(deltaP, sourcePoints[i]);
Jt = J.Clone();
//Transponierte Jacobimatrix berechnen
Jt.Transpose();
//sourcePoints[i] mit aktueller Homographie abbilden
double[,] thisH = new double[3, 3];
thisH[0, 0] = deltaP[0, 0] + 1;
thisH[0, 1] = deltaP[1, 0];
thisH[0, 2] = deltaP[2, 0];
thisH[1, 0] = deltaP[3, 0];
thisH[1, 1] = deltaP[4, 0] + 1;
thisH[1, 2] = deltaP[5, 0];
thisH[2, 0] = deltaP[6, 0];
thisH[2, 1] = deltaP[7, 0];
thisH[2, 2] = 1;
predLoc = projectPoint(thisH, sourcePoints[i]);
//ri berechnen (6.10)
ri[0, 0] = targetPoints[i].X - predLoc.X;
ri[1, 0] = targetPoints[i].Y - predLoc.Y;
//A und b updaten (6.9) und (6.10)
A.Add(Jt.Multiply(J));
b.Add(Jt.Multiply(ri));
}
//numerischer Trick (6.18)
for (int j = 0; j < 8; j++)
A[j, j] += A[j, j] * lambda;
//lasse Schritte, in denen A singulär ist, aus, passiert aber sowieso fast nie
try
{
//nach deltaP lösen und deltaP auf das aktuelle p (=deltaP) addieren (6.18)
deltaP.Add(A.Solve(b));
}
catch
{ }
}
//lese deltaP aus
double[,] homography = new double[3, 3];
homography[0, 0] = deltaP[0, 0] + 1;
homography[0, 1] = deltaP[1, 0];
homography[0, 2] = deltaP[2, 0];
homography[1, 0] = deltaP[3, 0];
homography[1, 1] = deltaP[4, 0] + 1;
homography[1, 2] = deltaP[5, 0];
homography[2, 0] = deltaP[6, 0];
homography[2, 1] = deltaP[7, 0];
homography[2, 2] = 1;
return homography;
}
/// <summary>
/// Findet in den korrespondierenden Punktepaaren correspondences eine affine Homographie mittels RANSAC
/// </summary>
/// <param name="imageL">Das linke Bild</param>
/// <param name="imageR">Das rechte Bild</param>
/// <param name="correspondences">Menge von korrespondierenden Punktepaaren, wobei erwartet wird, dass kein Punktepaar doppelt vorkommt</param>
/// <returns>affine Homographiematrix, die die Transformation zwischen linkem und rechtem Bild beschreibt</returns>
public override double[,] ransacH(
Image<Bgr, byte> imageL,
Image<Bgr, byte> imageR,
Match[] correspondences,
int ransacSteps,
double ransacThresh)
{
Random rng = new Random();
Matrix L, R, H;
// Anzahl der Korrespondenzen
int n = correspondences.Length;
//Sicherstellen, dass mindestens 3 Korrespondenzen vorhand sind, damit das GLS (sieh unten) eindeutig gelöst werden kann
if (n < 3) throw new Exception("Nicht genug Korrespondenzen gefunden!");
// Speicherplätze für ConsensusSets:
// das aktuell verarbeitete; das mit bisher am meisten Elementen
List<Match> currConsensusSet, biggestConsensusSet = new List<Match>();
for (int i = 0; i < ransacSteps; i++)
{
// Die Matrizen für das zu lösende GLS mit drei zufälligen, verschiedenen Punkten erstellen:
List<Match> matches = new List<Match>(); //Speicherplatz für drei zufällig ausgewählte, verschiedene Korrespondenzen
L = new Matrix(3, 3); // enthält drei Punkte aus dem linken Bild (vgl. linke Matrix des GLS vom Aufgabenblatt)
R = new Matrix(3, 3); // enthält drei punkte aus dem rechten Bild (vgl. rechte Matrix des GLS vom Aufgabenblatt)
for (int j = 0; j < 3; j++)
{
int index;
do
index = rng.Next(n); //zufällige, neue Korrespondenz holen
while (matches.Contains(correspondences[index]));
matches.Add(correspondences[index]);
//Die beiden Punkte der aktuellen Korrespondenz den Matrizen hinzufügen
L[j, 0] = correspondences[index].p1.X;
L[j, 1] = correspondences[index].p1.Y;
L[j, 2] = 1;
R[j, 0] = correspondences[index].p2.X;
R[j, 1] = correspondences[index].p2.Y;
R[j, 2] = 1;
}
//lasse Schritte, in denen A singulär ist, aus, passiert aber sowieso fast nie
try
{
H = R.Solve(L); //Das GLS (vgl. Aufgabenblatt) lösen lassen, um die transponierte Homographie-Matrix zu erhalten
H.Transpose(); // Die transponierte Homographie-Matrix transponieren, um die Homographie-Matrix zu erhalten
}
catch { continue; }
// Bestimmen des ConsensusSets:
currConsensusSet = new List<Match>();
for (int j = 0; j < n; j++)
{
// Rechter Punkt der aktuellen Korrespondenz mit der Homographie-Matrix transformieren
// und den (euklidschen) Abstand zum linken Punkt berechnen
PointF rightPointTransformed = new PointF();
rightPointTransformed.X = (float)(H[0, 0] * correspondences[j].p2.X + H[0, 1] * correspondences[j].p2.Y + H[0, 2]);
rightPointTransformed.Y = (float)(H[1, 0] * correspondences[j].p2.X + H[1, 1] * correspondences[j].p2.Y + H[1, 2]);
float z = (float)(H[2, 0] + H[2, 1] + H[2, 2]);
rightPointTransformed.X /= z;
rightPointTransformed.Y /= z;
//Abstand berechnen
double distance = Math.Sqrt((correspondences[j].p1.X - rightPointTransformed.X) * (correspondences[j].p1.X - rightPointTransformed.X) + (correspondences[j].p1.Y - rightPointTransformed.Y) * (correspondences[j].p1.Y - rightPointTransformed.Y));
// Wenn der Abstand des linken Punktes zum transformierten rechten Punkt noch im Rahmen liegt, hinzufügen:
if (distance < ransacThresh)
currConsensusSet.Add(correspondences[j]);
}
// Es wird das ConsensusSet gesucht, das die meisten Elemente enthält:
if (currConsensusSet.Count > biggestConsensusSet.Count)
biggestConsensusSet = currConsensusSet;
}
//Nachdem des größte ConsensusSet gefunden wurde, muss dessen Homographie-Matrix mit Hilfe aller unterstützenden Punkte berechnet werden (analog zu oben)
//Bei einer Überbestimmung des GLS wendet Solve als Ausgleichsverfahren leastSquares an
L = new Matrix(biggestConsensusSet.Count, 3);
R = new Matrix(biggestConsensusSet.Count, 3);
for (int i = 0; i < biggestConsensusSet.Count; i++)
{
L[i, 0] = biggestConsensusSet[i].p1.X;
L[i, 1] = biggestConsensusSet[i].p1.Y;
L[i, 2] = 1;
R[i, 0] = biggestConsensusSet[i].p2.X;
R[i, 1] = biggestConsensusSet[i].p2.Y;
R[i, 2] = 1;
}
H = R.Solve(L);
H.Transpose();
//correspondences in das passende Format für getHomographyFromPoints überführen
var l = new PointF[correspondences.Length];
var r = new PointF[correspondences.Length];
for(int i = 0; i < correspondences.Length; i++)
{
l[i] = correspondences[i].p2;
r[i] = correspondences[i].p1;
}
//Die gefundene affine Homographie-Matrix nutzen, um eine nicht-affine Matrix zu generieren
//Ergebnis zurückgeben
return getHomographyFromPoints(H.CopyToArray(), l, r);
}