/** * Computes the Moore–Penrose pseudoinverse using the SVD method. * * Modified version of the original implementation by Kim van der Linde. */ public static GeneralMatrix pinv(GeneralMatrix x) { if (x.Rank() < 1) return null; if (x.ColumnDimension > x.RowDimension) return pinv(x.Transpose()).Transpose(); SingularValueDecomposition svdX = new SingularValueDecomposition(x); double[] singularValues = svdX.SingularValues; double tol = Math.Max(x.ColumnDimension, x.RowDimension) * singularValues[0] * 2E-16; double[] singularValueReciprocals = new double[singularValues.Count()]; for (int i = 0; i < singularValues.Count(); i++) singularValueReciprocals[i] = Math.Abs(singularValues[i]) < tol ? 0 : (1.0 / singularValues[i]); double[][] u = svdX.GetU().Array; double[][] v = svdX.GetV().Array; int min = Math.Min(x.ColumnDimension, u[0].Count()); double[][] inverse = new double[x.ColumnDimension][]; for (int i = 0; i < x.ColumnDimension; i++) { inverse[i] = new double[x.RowDimension]; for (int j = 0; j < u.Count(); j++) for (int k = 0; k < min; k++) inverse[i][j] += v[i][k] * singularValueReciprocals[k] * u[j][k]; } return new GeneralMatrix(inverse); }
protected override GeneralMatrix CalculateNextHessianApproximation(GeneralMatrix previousH, double[]prevX, double[]curX, double[]prevGrad, double[]curGrad) { GeneralMatrix currentH = new GeneralMatrix(_nDim,_nDim); GeneralMatrix cX = new GeneralMatrix(curX,_nDim); GeneralMatrix pX = new GeneralMatrix(prevX,_nDim); GeneralMatrix cG = new GeneralMatrix(curGrad,_nDim); GeneralMatrix pG = new GeneralMatrix(prevGrad,_nDim); GeneralMatrix dX = cX.Subtract(pX); GeneralMatrix dG = cG.Subtract(pG); double aK1 = 1/(dX.Transpose().Multiply(dG).GetElement(0,0)); GeneralMatrix aK2 = dX.Multiply(dX.Transpose()); GeneralMatrix aK = aK2.Multiply(aK1); double bK1 = -1/(dG.Transpose().Multiply(previousH).Multiply(dG).GetElement(0,0)); GeneralMatrix bK2 = previousH.Multiply(dG).Multiply(dG.Transpose()).Multiply(previousH.Transpose()); GeneralMatrix bK =bK2.Multiply(bK1); currentH = previousH.Add(aK).Add(bK); return currentH; }
/// <summary>Solve X*A = B, which is also A'*X' = B'</summary> /// <param name="B"> right hand side /// </param> /// <returns> solution if A is square, least squares solution otherwise. /// </returns> public virtual GeneralMatrix SolveTranspose(GeneralMatrix B) { return Transpose().Solve(B.Transpose()); }
public static void Main(System.String[] argv) { GeneralMatrix A, B, C, Z, O, I, R, S, X, SUB, M, T, SQ, DEF, SOL; int errorCount = 0; int warningCount = 0; double tmp; double[] columnwise = new double[]{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 = new double[]{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[][] rvals = {new double[]{1.0, 4.0, 7.0}, new double[]{2.0, 5.0, 8.0, 11.0}, new double[]{3.0, 6.0, 9.0, 12.0}}; double[][] pvals = {new double[]{1.0, 1.0, 1.0}, new double[]{1.0, 2.0, 3.0}, new double[]{1.0, 3.0, 6.0}}; 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 raggedr = 0; /* (raggedr,raggedc) should be out of bounds in ragged array */ int raggedc = 4; 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 GeneralMatrix */ 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; /// <summary>Constructors and constructor-like methods: /// double[], int /// double[][] /// int, int /// int, int, double /// int, int, double[][] /// Create(double[][]) /// Random(int,int) /// Identity(int) /// /// </summary> print("\nTesting constructors and constructor-like methods...\n"); try { /// <summary>check that exception is thrown in packed constructor with invalid length *</summary> A = new GeneralMatrix(columnwise, invalidld); errorCount = try_failure(errorCount, "Catch invalid length in packed constructor... ", "exception not thrown for invalid input"); } catch (System.ArgumentException e) { try_success("Catch invalid length in packed constructor... ", e.Message); } try { /// <summary>check that exception is thrown in default constructor /// if input array is 'ragged' * /// </summary> A = new GeneralMatrix(rvals); tmp = A.GetElement(raggedr, raggedc); } catch (System.ArgumentException e) { try_success("Catch ragged input to default constructor... ", e.Message); } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "Catch ragged input to constructor... ", "exception not thrown in construction...ArrayIndexOutOfBoundsException thrown later"); System.Console.Out.WriteLine(e.Message); } try { /// <summary>check that exception is thrown in Create /// if input array is 'ragged' * /// </summary> A = GeneralMatrix.Create(rvals); tmp = A.GetElement(raggedr, raggedc); } catch (System.ArgumentException e) { try_success("Catch ragged input to Create... ", e.Message); System.Console.Out.WriteLine(e.Message); } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "Catch ragged input to Create... ", "exception not thrown in construction...ArrayIndexOutOfBoundsException thrown later"); System.Console.Out.WriteLine(e.Message); } A = new GeneralMatrix(columnwise, validld); B = new GeneralMatrix(avals); tmp = B.GetElement(0, 0); avals[0][0] = 0.0; C = B.Subtract(A); avals[0][0] = tmp; B = GeneralMatrix.Create(avals); tmp = B.GetElement(0, 0); avals[0][0] = 0.0; if ((tmp - B.GetElement(0, 0)) != 0.0) { /// <summary>check that Create behaves properly *</summary> errorCount = try_failure(errorCount, "Create... ", "Copy not effected... data visible outside"); } else { try_success("Create... ", ""); } avals[0][0] = columnwise[0]; I = new GeneralMatrix(ivals); try { check(I, GeneralMatrix.Identity(3, 4)); try_success("Identity... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Identity... ", "Identity GeneralMatrix not successfully created"); System.Console.Out.WriteLine(e.Message); } /// <summary>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,GeneralMatrix) /// SetMatrix(int,int,int[],GeneralMatrix) /// SetMatrix(int[],int,int,GeneralMatrix) /// SetMatrix(int[],int[],GeneralMatrix) /// /// </summary> print("\nTesting access methods...\n"); /// <summary>Various get methods: /// /// </summary> B = new GeneralMatrix(avals); if (B.RowDimension != rows) { errorCount = try_failure(errorCount, "getRowDimension... ", ""); } else { try_success("getRowDimension... ", ""); } if (B.ColumnDimension != cols) { errorCount = try_failure(errorCount, "getColumnDimension... ", ""); } else { try_success("getColumnDimension... ", ""); } B = new GeneralMatrix(avals); double[][] barray = B.Array; if (barray != avals) { errorCount = try_failure(errorCount, "getArray... ", ""); } else { try_success("getArray... ", ""); } barray = B.ArrayCopy; if (barray == avals) { errorCount = try_failure(errorCount, "getArrayCopy... ", "data not (deep) copied"); } try { check(barray, avals); try_success("getArrayCopy... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "getArrayCopy... ", "data not successfully (deep) copied"); System.Console.Out.WriteLine(e.Message); } 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.GetElement(B.RowDimension, B.ColumnDimension - 1); errorCount = try_failure(errorCount, "get(int,int)... ", "OutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { tmp = B.GetElement(B.RowDimension - 1, B.ColumnDimension); errorCount = try_failure(errorCount, "get(int,int)... ", "OutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("get(int,int)... OutofBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "get(int,int)... ", "OutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { if (B.GetElement(B.RowDimension - 1, B.ColumnDimension - 1) != avals[B.RowDimension - 1][B.ColumnDimension - 1]) { errorCount = try_failure(errorCount, "get(int,int)... ", "GeneralMatrix entry (i,j) not successfully retreived"); } else { try_success("get(int,int)... ", ""); } } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "get(int,int)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e.Message); } SUB = new GeneralMatrix(subavals); try { M = B.GetMatrix(ib, ie + B.RowDimension + 1, jb, je); errorCount = try_failure(errorCount, "GetMatrix(int,int,int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { M = B.GetMatrix(ib, ie, jb, je + B.ColumnDimension + 1); errorCount = try_failure(errorCount, "GetMatrix(int,int,int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("GetMatrix(int,int,int,int)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { M = B.GetMatrix(ib, ie, jb, je); try { check(SUB, M); try_success("GetMatrix(int,int,int,int)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int,int)... ", "submatrix not successfully retreived"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int,int)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e.Message); } try { M = B.GetMatrix(ib, ie, badcolumnindexset); errorCount = try_failure(errorCount, "GetMatrix(int,int,int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { M = B.GetMatrix(ib, ie + B.RowDimension + 1, columnindexset); errorCount = try_failure(errorCount, "GetMatrix(int,int,int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("GetMatrix(int,int,int[])... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { M = B.GetMatrix(ib, ie, columnindexset); try { check(SUB, M); try_success("GetMatrix(int,int,int[])... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int[])... ", "submatrix not successfully retreived"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "GetMatrix(int,int,int[])... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e.Message); } try { M = B.GetMatrix(badrowindexset, jb, je); errorCount = try_failure(errorCount, "GetMatrix(int[],int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { M = B.GetMatrix(rowindexset, jb, je + B.ColumnDimension + 1); errorCount = try_failure(errorCount, "GetMatrix(int[],int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("GetMatrix(int[],int,int)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "GetMatrix(int[],int,int)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { M = B.GetMatrix(rowindexset, jb, je); try { check(SUB, M); try_success("GetMatrix(int[],int,int)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "GetMatrix(int[],int,int)... ", "submatrix not successfully retreived"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "GetMatrix(int[],int,int)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e.Message); } try { M = B.GetMatrix(badrowindexset, columnindexset); errorCount = try_failure(errorCount, "GetMatrix(int[],int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { M = B.GetMatrix(rowindexset, badcolumnindexset); errorCount = try_failure(errorCount, "GetMatrix(int[],int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("GetMatrix(int[],int[])... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "GetMatrix(int[],int[])... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { M = B.GetMatrix(rowindexset, columnindexset); try { check(SUB, M); try_success("GetMatrix(int[],int[])... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "GetMatrix(int[],int[])... ", "submatrix not successfully retreived"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e) { errorCount = try_failure(errorCount, "GetMatrix(int[],int[])... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e.Message); } /// <summary>Various set methods: /// /// </summary> try { B.SetElement(B.RowDimension, B.ColumnDimension - 1, 0.0); errorCount = try_failure(errorCount, "set(int,int,double)... ", "OutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { B.SetElement(B.RowDimension - 1, B.ColumnDimension, 0.0); errorCount = try_failure(errorCount, "set(int,int,double)... ", "OutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("set(int,int,double)... OutofBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "set(int,int,double)... ", "OutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { B.SetElement(ib, jb, 0.0); tmp = B.GetElement(ib, jb); try { check(tmp, 0.0); try_success("set(int,int,double)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "set(int,int,double)... ", "GeneralMatrix element not successfully set"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e1) { errorCount = try_failure(errorCount, "set(int,int,double)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e1.Message); } M = new GeneralMatrix(2, 3, 0.0); try { B.SetMatrix(ib, ie + B.RowDimension + 1, jb, je, M); errorCount = try_failure(errorCount, "SetMatrix(int,int,int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { B.SetMatrix(ib, ie, jb, je + B.ColumnDimension + 1, M); errorCount = try_failure(errorCount, "SetMatrix(int,int,int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("SetMatrix(int,int,int,int,GeneralMatrix)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(ib, ie, jb, je, M); try { check(M.Subtract(B.GetMatrix(ib, ie, jb, je)), M); try_success("SetMatrix(int,int,int,int,GeneralMatrix)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int,int,GeneralMatrix)... ", "submatrix not successfully set"); System.Console.Out.WriteLine(e.Message); } B.SetMatrix(ib, ie, jb, je, SUB); } catch (System.IndexOutOfRangeException e1) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int,int,GeneralMatrix)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(ib, ie + B.RowDimension + 1, columnindexset, M); errorCount = try_failure(errorCount, "SetMatrix(int,int,int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { B.SetMatrix(ib, ie, badcolumnindexset, M); errorCount = try_failure(errorCount, "SetMatrix(int,int,int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("SetMatrix(int,int,int[],GeneralMatrix)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(ib, ie, columnindexset, M); try { check(M.Subtract(B.GetMatrix(ib, ie, columnindexset)), M); try_success("SetMatrix(int,int,int[],GeneralMatrix)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int[],GeneralMatrix)... ", "submatrix not successfully set"); System.Console.Out.WriteLine(e.Message); } B.SetMatrix(ib, ie, jb, je, SUB); } catch (System.IndexOutOfRangeException e1) { errorCount = try_failure(errorCount, "SetMatrix(int,int,int[],GeneralMatrix)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(rowindexset, jb, je + B.ColumnDimension + 1, M); errorCount = try_failure(errorCount, "SetMatrix(int[],int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { B.SetMatrix(badrowindexset, jb, je, M); errorCount = try_failure(errorCount, "SetMatrix(int[],int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("SetMatrix(int[],int,int,GeneralMatrix)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "SetMatrix(int[],int,int,GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(rowindexset, jb, je, M); try { check(M.Subtract(B.GetMatrix(rowindexset, jb, je)), M); try_success("SetMatrix(int[],int,int,GeneralMatrix)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "SetMatrix(int[],int,int,GeneralMatrix)... ", "submatrix not successfully set"); System.Console.Out.WriteLine(e.Message); } B.SetMatrix(ib, ie, jb, je, SUB); } catch (System.IndexOutOfRangeException e1) { errorCount = try_failure(errorCount, "SetMatrix(int[],int,int,GeneralMatrix)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(rowindexset, badcolumnindexset, M); errorCount = try_failure(errorCount, "SetMatrix(int[],int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e) { System.Console.Out.WriteLine(e.Message); try { B.SetMatrix(badrowindexset, columnindexset, M); errorCount = try_failure(errorCount, "SetMatrix(int[],int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); } catch (System.IndexOutOfRangeException e1) { try_success("SetMatrix(int[],int[],GeneralMatrix)... ArrayIndexOutOfBoundsException... ", ""); System.Console.Out.WriteLine(e1.Message); } } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "SetMatrix(int[],int[],GeneralMatrix)... ", "ArrayIndexOutOfBoundsException expected but not thrown"); System.Console.Out.WriteLine(e1.Message); } try { B.SetMatrix(rowindexset, columnindexset, M); try { check(M.Subtract(B.GetMatrix(rowindexset, columnindexset)), M); try_success("SetMatrix(int[],int[],GeneralMatrix)... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "SetMatrix(int[],int[],GeneralMatrix)... ", "submatrix not successfully set"); System.Console.Out.WriteLine(e.Message); } } catch (System.IndexOutOfRangeException e1) { errorCount = try_failure(errorCount, "SetMatrix(int[],int[],GeneralMatrix)... ", "Unexpected ArrayIndexOutOfBoundsException"); System.Console.Out.WriteLine(e1.Message); } /// <summary>Array-like methods: /// Subtract /// SubtractEquals /// Add /// AddEquals /// ArrayLeftDivide /// ArrayLeftDivideEquals /// ArrayRightDivide /// ArrayRightDivideEquals /// arrayTimes /// ArrayMultiplyEquals /// uminus /// /// </summary> print("\nTesting array-like methods...\n"); S = new GeneralMatrix(columnwise, nonconformld); R = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); A = R; try { S = A.Subtract(S); errorCount = try_failure(errorCount, "Subtract conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("Subtract conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } if (A.Subtract(R).Norm1() != 0.0) { errorCount = try_failure(errorCount, "Subtract... ", "(difference of identical Matrices is nonzero,\nSubsequent use of Subtract should be suspect)"); } else { try_success("Subtract... ", ""); } A = R.Copy(); A.SubtractEquals(R); Z = new GeneralMatrix(A.RowDimension, A.ColumnDimension); try { A.SubtractEquals(S); errorCount = try_failure(errorCount, "SubtractEquals conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("SubtractEquals conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } if (A.Subtract(Z).Norm1() != 0.0) { errorCount = try_failure(errorCount, "SubtractEquals... ", "(difference of identical Matrices is nonzero,\nSubsequent use of Subtract should be suspect)"); } else { try_success("SubtractEquals... ", ""); } A = R.Copy(); B = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); C = A.Subtract(B); try { S = A.Add(S); errorCount = try_failure(errorCount, "Add conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("Add conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } try { check(C.Add(B), A); try_success("Add... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Add... ", "(C = A - B, but C + B != A)"); System.Console.Out.WriteLine(e.Message); } C = A.Subtract(B); C.AddEquals(B); try { A.AddEquals(S); errorCount = try_failure(errorCount, "AddEquals conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("AddEquals conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } try { check(C, A); try_success("AddEquals... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "AddEquals... ", "(C = A - B, but C = C + B != A)"); System.Console.Out.WriteLine(e.Message); } A = R.UnaryMinus(); try { check(A.Add(R), Z); try_success("UnaryMinus... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "uminus... ", "(-A + A != zeros)"); System.Console.Out.WriteLine(e.Message); } A = R.Copy(); O = new GeneralMatrix(A.RowDimension, A.ColumnDimension, 1.0); C = A.ArrayLeftDivide(R); try { S = A.ArrayLeftDivide(S); errorCount = try_failure(errorCount, "ArrayLeftDivide conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("ArrayLeftDivide conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } try { check(C, O); try_success("ArrayLeftDivide... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "ArrayLeftDivide... ", "(M.\\M != ones)"); System.Console.Out.WriteLine(e.Message); } try { A.ArrayLeftDivideEquals(S); errorCount = try_failure(errorCount, "ArrayLeftDivideEquals conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("ArrayLeftDivideEquals conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } A.ArrayLeftDivideEquals(R); try { check(A, O); try_success("ArrayLeftDivideEquals... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "ArrayLeftDivideEquals... ", "(M.\\M != ones)"); System.Console.Out.WriteLine(e.Message); } A = R.Copy(); try { A.ArrayRightDivide(S); errorCount = try_failure(errorCount, "ArrayRightDivide conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("ArrayRightDivide conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } C = A.ArrayRightDivide(R); try { check(C, O); try_success("ArrayRightDivide... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "ArrayRightDivide... ", "(M./M != ones)"); System.Console.Out.WriteLine(e.Message); } try { A.ArrayRightDivideEquals(S); errorCount = try_failure(errorCount, "ArrayRightDivideEquals conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("ArrayRightDivideEquals conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } A.ArrayRightDivideEquals(R); try { check(A, O); try_success("ArrayRightDivideEquals... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "ArrayRightDivideEquals... ", "(M./M != ones)"); System.Console.Out.WriteLine(e.Message); } A = R.Copy(); B = GeneralMatrix.Random(A.RowDimension, A.ColumnDimension); try { S = A.ArrayMultiply(S); errorCount = try_failure(errorCount, "arrayTimes conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("arrayTimes conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } C = A.ArrayMultiply(B); try { check(C.ArrayRightDivideEquals(B), A); try_success("arrayTimes... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "arrayTimes... ", "(A = R, C = A.*B, but C./B != A)"); System.Console.Out.WriteLine(e.Message); } try { A.ArrayMultiplyEquals(S); errorCount = try_failure(errorCount, "ArrayMultiplyEquals conformance check... ", "nonconformance not raised"); } catch (System.ArgumentException e) { try_success("ArrayMultiplyEquals conformance check... ", ""); System.Console.Out.WriteLine(e.Message); } A.ArrayMultiplyEquals(B); try { check(A.ArrayRightDivideEquals(B), R); try_success("ArrayMultiplyEquals... ", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "ArrayMultiplyEquals... ", "(A = R, A = A.*B, but A./B != R)"); System.Console.Out.WriteLine(e.Message); } /// <summary>LA methods: /// Transpose /// Multiply /// Condition /// Rank /// Determinant /// trace /// Norm1 /// norm2 /// normF /// normInf /// Solve /// solveTranspose /// Inverse /// chol /// Eigen /// lu /// qr /// svd /// /// </summary> print("\nTesting linear algebra methods...\n"); A = new GeneralMatrix(columnwise, 3); T = new GeneralMatrix(tvals); T = A.Transpose(); try { check(A.Transpose(), T); try_success("Transpose...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Transpose()...", "Transpose unsuccessful"); System.Console.Out.WriteLine(e.Message); } A.Transpose(); try { check(A.Norm1(), columnsummax); try_success("Norm1...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Norm1()...", "incorrect norm calculation"); System.Console.Out.WriteLine(e.Message); } try { check(A.NormInf(), rowsummax); try_success("normInf()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "normInf()...", "incorrect norm calculation"); System.Console.Out.WriteLine(e.Message); } try { check(A.NormF(), System.Math.Sqrt(sumofsquares)); try_success("normF...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "normF()...", "incorrect norm calculation"); System.Console.Out.WriteLine(e.Message); } try { check(A.Trace(), sumofdiagonals); try_success("trace()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "trace()...", "incorrect trace calculation"); System.Console.Out.WriteLine(e.Message); } try { check(A.GetMatrix(0, A.RowDimension - 1, 0, A.RowDimension - 1).Determinant(), 0.0); try_success("Determinant()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Determinant()...", "incorrect determinant calculation"); System.Console.Out.WriteLine(e.Message); } SQ = new GeneralMatrix(square); try { check(A.Multiply(A.Transpose()), SQ); try_success("Multiply(GeneralMatrix)...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Multiply(GeneralMatrix)...", "incorrect GeneralMatrix-GeneralMatrix product calculation"); System.Console.Out.WriteLine(e.Message); } try { check(A.Multiply(0.0), Z); try_success("Multiply(double)...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Multiply(double)...", "incorrect GeneralMatrix-scalar product calculation"); System.Console.Out.WriteLine(e.Message); } A = new GeneralMatrix(columnwise, 4); QRDecomposition QR = A.QRD(); R = QR.R; try { check(A, QR.Q.Multiply(R)); try_success("QRDecomposition...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "QRDecomposition...", "incorrect QR decomposition calculation"); System.Console.Out.WriteLine(e.Message); } SingularValueDecomposition SVD = A.SVD(); try { check(A, SVD.GetU().Multiply(SVD.S.Multiply(SVD.GetV().Transpose()))); try_success("SingularValueDecomposition...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "SingularValueDecomposition...", "incorrect singular value decomposition calculation"); System.Console.Out.WriteLine(e.Message); } DEF = new GeneralMatrix(rankdef); try { check(DEF.Rank(), System.Math.Min(DEF.RowDimension, DEF.ColumnDimension) - 1); try_success("Rank()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Rank()...", "incorrect Rank calculation"); System.Console.Out.WriteLine(e.Message); } B = new GeneralMatrix(condmat); SVD = B.SVD(); double[] singularvalues = SVD.SingularValues; try { check(B.Condition(), singularvalues[0] / singularvalues[System.Math.Min(B.RowDimension, B.ColumnDimension) - 1]); try_success("Condition()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Condition()...", "incorrect condition number calculation"); System.Console.Out.WriteLine(e.Message); } int n = A.ColumnDimension; A = A.GetMatrix(0, n - 1, 0, n - 1); A.SetElement(0, 0, 0.0); LUDecomposition LU = A.LUD(); try { check(A.GetMatrix(LU.Pivot, 0, n - 1), LU.L.Multiply(LU.U)); try_success("LUDecomposition...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "LUDecomposition...", "incorrect LU decomposition calculation"); System.Console.Out.WriteLine(e.Message); } X = A.Inverse(); try { check(A.Multiply(X), GeneralMatrix.Identity(3, 3)); try_success("Inverse()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Inverse()...", "incorrect Inverse calculation"); System.Console.Out.WriteLine(e.Message); } O = new GeneralMatrix(SUB.RowDimension, 1, 1.0); SOL = new GeneralMatrix(sqSolution); SQ = SUB.GetMatrix(0, SUB.RowDimension - 1, 0, SUB.RowDimension - 1); try { check(SQ.Solve(SOL), O); try_success("Solve()...", ""); } catch (System.ArgumentException e1) { errorCount = try_failure(errorCount, "Solve()...", e1.Message); System.Console.Out.WriteLine(e1.Message); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "Solve()...", e.Message); System.Console.Out.WriteLine(e.Message); } A = new GeneralMatrix(pvals); CholeskyDecomposition Chol = A.chol(); GeneralMatrix L = Chol.GetL(); try { check(A, L.Multiply(L.Transpose())); try_success("CholeskyDecomposition...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "CholeskyDecomposition...", "incorrect Cholesky decomposition calculation"); System.Console.Out.WriteLine(e.Message); } X = Chol.Solve(GeneralMatrix.Identity(3, 3)); try { check(A.Multiply(X), GeneralMatrix.Identity(3, 3)); try_success("CholeskyDecomposition Solve()...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "CholeskyDecomposition Solve()...", "incorrect Choleskydecomposition Solve calculation"); System.Console.Out.WriteLine(e.Message); } EigenvalueDecomposition Eig = A.Eigen(); GeneralMatrix D = Eig.D; GeneralMatrix V = Eig.GetV(); try { check(A.Multiply(V), V.Multiply(D)); try_success("EigenvalueDecomposition (symmetric)...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "EigenvalueDecomposition (symmetric)...", "incorrect symmetric Eigenvalue decomposition calculation"); System.Console.Out.WriteLine(e.Message); } A = new GeneralMatrix(evals); Eig = A.Eigen(); D = Eig.D; V = Eig.GetV(); try { check(A.Multiply(V), V.Multiply(D)); try_success("EigenvalueDecomposition (nonsymmetric)...", ""); } catch (System.SystemException e) { errorCount = try_failure(errorCount, "EigenvalueDecomposition (nonsymmetric)...", "incorrect nonsymmetric Eigenvalue decomposition calculation"); System.Console.Out.WriteLine(e.Message); } print("\nTestMatrix completed.\n"); print("Total errors reported: " + System.Convert.ToString(errorCount) + "\n"); print("Total warnings reported: " + System.Convert.ToString(warningCount) + "\n"); }
public void TestTranspose() { GeneralMatrix _gm = new GeneralMatrix(2,2); _gm.SetElement(0,0,1); _gm.SetElement(0,1,2); _gm.SetElement(1,0,3); _gm.SetElement(1,1,4); GeneralMatrix _ngm = _gm.Transpose(); Assert.AreEqual(1,0,2); }
//encrypt line of input static void Encrypt(string input, GeneralMatrix key, Dictionary<char, int> hillDict, StringBuilder encText) { string ct = ""; string[] pt = input.Trim().Split(' '); for (int i = 0; i < pt.Length; i += 3) { //encrypt 3 letters at a time double[] temp = new double[3]; temp[0] = Convert.ToInt32(pt[i]); temp[1] = Convert.ToInt32(pt[i + 1]); temp[2] = Convert.ToInt32(pt[i + 2]); //create plain text matrix, transpose and encrypt it GeneralMatrix ptMat = new GeneralMatrix(new double[] { temp[0], temp[1], temp[2] }, 3); GeneralMatrix trnasPTMat = ptMat.Transpose(); GeneralMatrix ctMat = key.Multiply(ptMat); for (int x = 0; x < ctMat.RowDimension; x++) { for (int y = 0; y < ctMat.ColumnDimension; y++) { var tempElement = Convert.ToInt32(ctMat.GetElement(x, y)) % 31; ctMat.SetElement(x, y, tempElement); } } for (int x = 0; x < ctMat.RowDimension; x++) { for (int y = 0; y < ctMat.ColumnDimension; y++) { ct += hillDict.FirstOrDefault(z => z.Value == ctMat.GetElement(x, y)).Key; } } } //append to string builder encText.AppendLine(ct); }
/// <summary> /// ( /// </summary> /// <param name="matrix"></param> /// <returns></returns> public static GeneralMatrix ExpandUtility(GeneralMatrix matrix) { double val=0.0; int n = matrix.RowDimension; int m = matrix.ColumnDimension; if (n!=m) throw new ArgumentException("Criteria matrix must be symmetrical"); GeneralMatrix newMatrix = matrix.Transpose(); //for all transposed elements calculate their inverse values //set diagonal elements to 0 for (int i=0; i<n; i++) for (int j=0; j<=i; j++) { val = newMatrix.GetElement(i,j); if (val==0.0) throw new ArgumentException("Criteria comparison values van't be 0"); newMatrix.SetElement(i,j,1/val); if (i==j) newMatrix.SetElement(i,j,0); } //add transposed, inverse matrix to the original one //create fully expanded matrix return newMatrix.Add(matrix); }
protected RpropResult RPropLoop(double[] seed, bool precise) { //Console.WriteLine("RpropLoop"); InitialStepSize(); double[] curGradient; RpropResult ret = new RpropResult(); if (seed != null) { curGradient = InitialPointFromSeed(ret, seed); } else { curGradient = InitialPoint(ret); } double curUtil = ret.initialUtil; double oldUtil = curUtil; double[] formerGradient = new double[dim]; double[] curValue = new double[dim]; double[] testValue = new double[dim]; double lambda = 0.1; Tuple <double[], double> tup; Buffer.BlockCopy(ret.initialValue, 0, curValue, 0, sizeof(double) * dim); formerGradient = curGradient; //Buffer.BlockCopy(curGradient,0,formerGradient,0,sizeof(double)*dim); int itcounter = 0; int badcounter = 0; /*Console.WriteLine("Initial Sol:"); * for(int i=0; i<dim;i++) { * Console.Write("{0} ",curValue[i]); * } * Console.WriteLine(); * Console.WriteLine("Initial Util: {0}",curUtil); */ #if (GSOLVER_LOG) Log(curUtil, curValue); #endif int maxIter = 60; int maxBad = 30; double minStep = 1E-11; if (precise) { maxIter = 120; //110 maxBad = 60; //60 minStep = 1E-15; //15 } int convergendDims = 0; while (itcounter++ < maxIter && badcounter < maxBad) { convergendDims = 0; //First Order resp. approximated Second Order Gradient for (int i = 0; i < dim; i++) { if (curGradient[i] * formerGradient[i] > 0) { rpropStepWidth[i] *= 1.3; } else if (curGradient[i] * formerGradient[i] < 0) { rpropStepWidth[i] *= 0.5; } rpropStepWidth[i] = Math.Max(minStep, rpropStepWidth[i]); //rpropStepWidth[i] = Math.Max(0.000001,rpropStepWidth[i]); if (curUtil > 0.0) { //if (curGradient[i] > 0) curValue[i] += rpropStepWidth[i]; //else if (curGradient[i] < 0) curValue[i] -= rpropStepWidth[i]; } else { //linear assumption //curValue[i] += -curUtil/curGradient[i]; //quadratic assumption /*double ypSquare = 0; * for(int j=0; j<dim; j++) { * ypSquare += curGradient[j]*curGradient[j]; * } * double m=ypSquare/curUtil; * curValue[i] = -curGradient[i]/(2*m) + curValue[i];*/ } if (curValue[i] > limits[i, 1]) { curValue[i] = limits[i, 1]; } else if (curValue[i] < limits[i, 0]) { curValue[i] = limits[i, 0]; } if (rpropStepWidth[i] < rpropStepConvergenceThreshold[i]) { ++convergendDims; } } //Abort if all dimensions are converged if (!precise && convergendDims >= dim) { if (curUtil > ret.finalUtil) { ret.finalUtil = curUtil; Buffer.BlockCopy(curValue, 0, ret.finalValue, 0, sizeof(double) * dim); } return(ret); } // Conjugate Gradient if (curUtil < 0.5) { DotNetMatrix.GeneralMatrix X = new DotNetMatrix.GeneralMatrix(dim * 2 + 1, dim); DotNetMatrix.GeneralMatrix Y = new DotNetMatrix.GeneralMatrix(dim * 2 + 1, 1); for (int n = 0; n < dim; n++) { X.SetElement(0, n, curValue[n]); } Y.SetElement(0, 0, 0); for (int j = 0; j < dim * 2; j++) { for (int n = 0; n < dim; n++) { double rVal = rand.NextDouble() * rpropStepConvergenceThreshold[n] * 1000.0; testValue[n] = curValue[n] + rVal; } tup = term.Differentiate(testValue); for (int n = 0; n < dim; n++) { X.SetElement(j + 1, n, tup.Item1[n]); } Y.SetElement(j + 1, 0, tup.Item2 - curUtil); } DotNetMatrix.GeneralMatrix JJ = X.Transpose().Multiply(X); /*if(curUtil>oldUtil) lambda *= 10; * else lambda *= 0.1;*/ //DotNetMatrix.GeneralMatrix B = JJ.Add(GeneralMatrix.Identity(dim, dim).Multiply(lambda)).Inverse().Multiply(X.Transpose()).Multiply(Y); DotNetMatrix.GeneralMatrix B = JJ.Add(JJ.SVD().S.Multiply(lambda)).Inverse().Multiply(X.Transpose()).Multiply(Y); //DotNetMatrix.GeneralMatrix B = JJ.Inverse().Multiply(X.Transpose()).Multiply(Y); for (int j = 0; j < dim; j++) { curValue[j] += 0.01 * B.GetElement(j, 0); } Console.WriteLine(curUtil); Console.Write(B.Transpose()); Console.WriteLine(); } ///////////////////////// this.FEvals++; tup = term.Differentiate(curValue); bool allZero = true; for (int i = 0; i < dim; i++) { if (Double.IsNaN(tup.Item1[i])) { ret.aborted = true; #if (GSOLVER_LOG) LogStep(); #endif return(ret); } allZero &= (tup.Item1[i] == 0); } oldUtil = curUtil; curUtil = tup.Item2; formerGradient = curGradient; curGradient = tup.Item1; #if (GSOLVER_LOG) Log(curUtil, curValue); #endif //Console.WriteLine("CurUtil: {0} Final {1}",curUtil,ret.finalUtil); if (curUtil > ret.finalUtil) { badcounter = 0; //Math.Max(0,badcounter-1); //if (curUtil-ret.finalUtil < 0.00000000000001) { //Console.WriteLine("not better"); // badcounter++; //} else { //badcounter = 0; //} ret.finalUtil = curUtil; Buffer.BlockCopy(curValue, 0, ret.finalValue, 0, sizeof(double) * dim); //ret.finalValue = curValue; #if (ALWAYS_CHECK_THRESHOLD) if (curUtil > utilityThreshold) { return(ret); } #endif } else { //if (curUtil < ret.finalUtil || curUtil > 0) badcounter++; badcounter++; } if (allZero) { //Console.WriteLine("All Zero!"); /*Console.WriteLine("Util {0}",curUtil); * Console.Write("Vals: "); * for(int i=0; i < dim; i++) { * Console.Write("{0}\t",curValue[i]); * } * Console.WriteLine();*/ ret.aborted = false; #if (GSOLVER_LOG) LogStep(); #endif return(ret); } } #if (GSOLVER_LOG) LogStep(); #endif ret.aborted = false; return(ret); }
/// <summary>Solve X*A = B, which is also A'*X' = B'</summary> /// <param name="B"> right hand side /// </param> /// <returns> solution if A is square, least squares solution otherwise. /// </returns> public virtual GeneralMatrix SolveTranspose(GeneralMatrix B) { return(Transpose().Solve(B.Transpose())); }
// this is the straight forward implementation of the kabsch algorithm. // see http://en.wikipedia.org/wiki/Kabsch_algorithm for a detailed explanation. public void Evaluate(int SpreadMax) { FOutput.SliceCount = 1; Matrix4x4 mOut; if (FInputP.SliceCount > 0 && FInputQ.SliceCount > 0 && FInputEnabled[0]){ // ======================== STEP 1 ======================== // translate both sets so that their centroids coincides with the origin // of the coordinate system. double[] meanP = new double[3]{0.0,0.0,0.0}; // mean of first point set for(int i=0; i < FInputP.SliceCount;i++){ meanP[0] += FInputP[i].x; meanP[1] += FInputP[i].y; meanP[2] += FInputP[i].z; } meanP[0] /= FInputP.SliceCount; meanP[1] /= FInputP.SliceCount; meanP[2] /= FInputP.SliceCount; double[][]centroidP = new double[3][]{new double[]{meanP[0]},new double[]{meanP[1]},new double[]{meanP[2]}}; GeneralMatrix mCentroidP = new GeneralMatrix(centroidP); double[][] arrayP = new double[FInputP.SliceCount][]; for(int i=0; i < FInputP.SliceCount;i++){ arrayP[i] = new double[3]; arrayP[i][0] = FInputP[i].x - meanP[0]; // subtract the mean values from the incoming pointset arrayP[i][1] = FInputP[i].y - meanP[1]; arrayP[i][2] = FInputP[i].z - meanP[2]; } // this is the matrix of the first pointset translated to the origin of the coordinate system GeneralMatrix P = new GeneralMatrix(arrayP); double[] meanQ = new double[3]{0.0,0.0,0.0}; // mean of second point set for(int i=0; i < FInputQ.SliceCount;i++){ meanQ[0] += FInputQ[i].x; meanQ[1] += FInputQ[i].y; meanQ[2] += FInputQ[i].z; } meanQ[0] /= FInputQ.SliceCount; meanQ[1] /= FInputQ.SliceCount; meanQ[2] /= FInputQ.SliceCount; double[][]centroidQ = new double[3][]{new double[]{meanQ[0]},new double[]{meanQ[1]},new double[]{meanQ[2]}}; GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ); double[][] arrayQ = new double[FInputQ.SliceCount][]; for(int i=0; i < FInputQ.SliceCount;i++){ arrayQ[i] = new double[3]; arrayQ[i][0] = FInputQ[i].x - meanQ[0]; // subtract the mean values from the incoming pointset arrayQ[i][1] = FInputQ[i].y - meanQ[1]; arrayQ[i][2] = FInputQ[i].z - meanQ[2]; } // this is the matrix of the second pointset translated to the origin of the coordinate system GeneralMatrix Q = new GeneralMatrix(arrayQ); // ======================== STEP2 ======================== // calculate a covariance matrix A and compute the optimal rotation matrix GeneralMatrix A = P.Transpose() * Q; SingularValueDecomposition svd = A.SVD(); GeneralMatrix U = svd.GetU(); GeneralMatrix V = svd.GetV(); // calculate determinant for a special reflexion case. double det = (V * U.Transpose()).Determinant(); double[][] arrayD = new double[3][]{ new double[]{1,0,0}, new double[] {0,1,0}, new double[] {0,0,1} }; arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0 GeneralMatrix D = new GeneralMatrix(arrayD); // now we can compute the rotation matrix: GeneralMatrix R = V * D * U.Transpose(); // ======================== STEP3 ======================== // calculate the translation: GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ; // ================== OUTPUT TRANSFORM =================== mOut.m11 = (R.Array)[0][0]; mOut.m12 = (R.Array)[0][1]; mOut.m13 = (R.Array)[0][2]; mOut.m14 = 0; mOut.m21 = (R.Array)[1][0]; mOut.m22 = (R.Array)[1][1]; mOut.m23 = (R.Array)[1][2]; mOut.m24 = 0; mOut.m31 = (R.Array)[2][0]; mOut.m32 = (R.Array)[2][1]; mOut.m33 = (R.Array)[2][2]; mOut.m34 = 0; mOut.m41 = (T.Array)[0][0]; mOut.m42 = (T.Array)[1][0]; mOut.m43 = (T.Array)[2][0]; mOut.m44 = 1; FOutput[0] = mOut; } //FLogger.Log(LogType.Debug, T.Array[2][0].ToString()); }
/// <summary> /// Solves between two point sets /// </summary> /// <param name="points">Point set</param> /// <returns>Affine matrix for each point set</returns> public Matrix Solve(IReadOnlyList<CameraToCameraPoint> points) { if (points == null) throw new ArgumentNullException("points"); if (points.Count < 1) throw new ArgumentException("points", "No points provided"); double[] meanP = new double[3] { 0.0, 0.0, 0.0 }; // mean of first point set for (int i = 0; i < points.Count; i++) { Vector3 orig = points[i].Origin; meanP[0] += orig.X; meanP[1] += orig.Y; meanP[2] += orig.Z; } double invCount = 1.0 / (double)points.Count; meanP[0] *= invCount; meanP[1] *= invCount; meanP[2] *= invCount; double[][] centroidP = new double[3][] { new double[] { meanP[0] }, new double[] { meanP[1] }, new double[] { meanP[2] } }; GeneralMatrix mCentroidP = new GeneralMatrix(centroidP); double[][] arrayP = new double[points.Count][]; for (int i = 0; i < points.Count; i++) { Vector3 orig = points[i].Origin; arrayP[i] = new double[3]; arrayP[i][0] = orig.X - meanP[0]; // subtract the mean values from the incoming pointset arrayP[i][1] = orig.Y - meanP[1]; arrayP[i][2] = orig.Z - meanP[2]; } // this is the matrix of the first pointset translated to the origin of the coordinate system GeneralMatrix P = new GeneralMatrix(arrayP); double[] meanQ = new double[3] { 0.0, 0.0, 0.0 }; // mean of second point set for (int i = 0; i < points.Count; i++) { Vector3 dest = points[i].Destination; meanQ[0] += dest.X; meanQ[1] += dest.Y; meanQ[2] += dest.Z; } meanQ[0] *= invCount; meanQ[1] *= invCount; meanQ[2] *= invCount; double[][] centroidQ = new double[3][] { new double[] { meanQ[0] }, new double[] { meanQ[1] }, new double[] { meanQ[2] } }; GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ); double[][] arrayQ = new double[points.Count][]; for (int i = 0; i < points.Count; i++) { Vector3 dest = points[i].Destination; arrayQ[i] = new double[3]; arrayQ[i][0] = dest.X - meanQ[0]; // subtract the mean values from the incoming pointset arrayQ[i][1] = dest.Y - meanQ[1]; arrayQ[i][2] = dest.Z - meanQ[2]; } // this is the matrix of the second pointset translated to the origin of the coordinate system GeneralMatrix Q = new GeneralMatrix(arrayQ); // ======================== STEP2 ======================== // calculate a covariance matrix A and compute the optimal rotation matrix GeneralMatrix A = P.Transpose() * Q; SingularValueDecomposition svd = A.SVD(); GeneralMatrix U = svd.GetU(); GeneralMatrix V = svd.GetV(); // calculate determinant for a special reflexion case. double det = (V * U.Transpose()).Determinant(); double[][] arrayD = new double[3][]{ new double[]{1,0,0}, new double[] {0,1,0}, new double[] {0,0,1} }; arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0 GeneralMatrix D = new GeneralMatrix(arrayD); // now we can compute the rotation matrix: GeneralMatrix R = V * D * U.Transpose(); // ======================== STEP3 ======================== // calculate the translation: GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ; return new Matrix((float)(R.Array)[0][0], (float)(R.Array)[0][1], (float)(R.Array)[0][2], 0.0f, (float)(R.Array)[1][0], (float)(R.Array)[1][1], (float)(R.Array)[1][2], 0.0f, (float)(R.Array)[2][0], (float)(R.Array)[2][1], (float)(R.Array)[2][2], 0.0f, (float)(T.Array)[0][0], (float)(T.Array)[1][0], (float)(T.Array)[2][0], 1.0f); }