public GeneralMatrix[] decompose(int svCount, GeneralMatrix m) { SingularValueDecomposition s = new SingularValueDecomposition(m); try { sv = svCount; frameScale = m.RowDimension; if (m.RowDimension > m.ColumnDimension) { frameScale = m.ColumnDimension; } else if (m.RowDimension < m.ColumnDimension) { frameScale = m.RowDimension; } // Make square matrix of data if (m.RowDimension != m.ColumnDimension) { squareM = m.GetMatrix(0, frameScale - 1, 0, frameScale - 1); } else { squareM = m; } //perform the SVD here: SingularValueDecomposition svd = new SingularValueDecomposition(squareM); GeneralMatrix U = svd.GetU().GetMatrix(0, frameScale - 1, 0, sv - 1); GeneralMatrix V = svd.GetV(); double[] D = svd.SingularValues; GeneralMatrix dMat = makeDiagonalSquare(D, sv); GeneralMatrix vMat = V.Transpose().GetMatrix(0, sv - 1, 0, frameScale - 1); /*Console.WriteLine(U.RowDimension + "x" + U.ColumnDimension + " * " + dMat.RowDimension + "x" + dMat.ColumnDimension + " * " + vMat.RowDimension + "x" + vMat.ColumnDimension); recomposition = U.Multiply(dMat).Multiply(vMat); int[,] recompositionData = new int[recomposition.ColumnDimension, recomposition.RowDimension]; for (int i = 0; i < recomposition.ColumnDimension; i++) { for (int j = 0; j < recomposition.RowDimension; j++) { recompositionData[j, i] = (int)(recomposition.GetElement(j, i)); } }*/ GeneralMatrix[] result = new GeneralMatrix[2]; result[0] = U; result[1] = vMat; return result; } catch (Exception ex) { Console.WriteLine(ex.ToString()); } return null; }
private void computeaccCalButton_Click(object sender, EventArgs e) { int i,j; calStatusText.Text = "Computing Calibration..."; // Construct D matrix // D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)]; for (i = 0; i < SAMPLES; i++ ) { // x^2 term D.SetElement(i,0, loggedData[i,0]*loggedData[i,0]); // y^2 term D.SetElement(i,1,loggedData[i,1]*loggedData[i,1]); // z^2 term D.SetElement(i, 2, loggedData[i, 2] * loggedData[i, 2]); // x*y term D.SetElement(i,3,loggedData[i,0]*loggedData[i,1]); // x*z term D.SetElement(i,4,loggedData[i,0]*loggedData[i,2]); // y*z term D.SetElement(i,5,loggedData[i,1]*loggedData[i,2]); // x term D.SetElement(i,6,loggedData[i,0]); // y term D.SetElement(i,7,loggedData[i,1]); // z term D.SetElement(i,8,loggedData[i,2]); // Constant term D.SetElement(i,9,1); } // QR=triu(qr(D)) QRDecomposition QR = new QRDecomposition(D); // [U,S,V] = svd(D) SingularValueDecomposition SVD = new SingularValueDecomposition(QR.R); GeneralMatrix V = SVD.GetV(); GeneralMatrix A = new GeneralMatrix(3, 3); double[] p = new double[V.RowDimension]; for (i = 0; i < V.RowDimension; i++ ) { p[i] = V.GetElement(i,V.ColumnDimension-1); } /* A = [p(1) p(4)/2 p(5)/2; p(4)/2 p(2) p(6)/2; p(5)/2 p(6)/2 p(3)]; */ if (p[0] < 0) { for (i = 0; i < V.RowDimension; i++) { p[i] = -p[i]; } } A.SetElement(0,0,p[0]); A.SetElement(0,1,p[3]/2); A.SetElement(1,2,p[4]/2); A.SetElement(1,0,p[3]/2); A.SetElement(1,1,p[1]); A.SetElement(1,2,p[5]/2); A.SetElement(2,0,p[4]/2); A.SetElement(2,1,p[5]/2); A.SetElement(2,2,p[2]); CholeskyDecomposition Chol = new CholeskyDecomposition(A); GeneralMatrix Ut = Chol.GetL(); GeneralMatrix U = Ut.Transpose(); double[] bvect = {p[6]/2,p[7]/2,p[8]/2}; double d = p[9]; GeneralMatrix b = new GeneralMatrix(bvect,3); GeneralMatrix v = Ut.Solve(b); double vnorm_sqrd = v.GetElement(0,0)*v.GetElement(0,0) + v.GetElement(1,0)*v.GetElement(1,0) + v.GetElement(2,0)*v.GetElement(2,0); double s = 1/Math.Sqrt(vnorm_sqrd - d); GeneralMatrix c = U.Solve(v); for (i = 0; i < 3; i++) { c.SetElement(i, 0, -c.GetElement(i, 0)); } U = U.Multiply(s); for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { calMat[i, j] = U.GetElement(i, j); } } for (i = 0; i < 3; i++) { bias[i] = c.GetElement(i, 0); } accAlignment00.Text = calMat[0, 0].ToString(); accAlignment01.Text = calMat[0, 1].ToString(); accAlignment02.Text = calMat[0, 2].ToString(); accAlignment10.Text = calMat[1, 0].ToString(); accAlignment11.Text = calMat[1, 1].ToString(); accAlignment12.Text = calMat[1, 2].ToString(); accAlignment20.Text = calMat[2, 0].ToString(); accAlignment21.Text = calMat[2, 1].ToString(); accAlignment22.Text = calMat[2, 2].ToString(); biasX.Text = bias[0].ToString(); biasY.Text = bias[1].ToString(); biasZ.Text = bias[2].ToString(); calStatusText.Text = "Done"; flashCommitButton.Enabled = true; accAlignmentCommitButton.Enabled = true; }
/** * 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); }