/// <summary> /// Compute the eigenvalues for a symmetric, generalized eigenvalue problem A*x = lambda*B*x. /// </summary> /// <param name="A">Symmetric matrix.</param> /// <param name="B">Symmetric, positive definite matrix.</param> /// <param name="E">Matrix containing the eigenvectors on return.</param> /// <returns>A vector of eigenvalues.</returns> /// <remarks> /// See http://www.cmth.ph.ic.ac.uk/people/a.mackinnon/Lectures/compphys/node72.html /// and http://www.netlib.org/lapack/lug/node54.html /// </remarks> public static Vector <Complex> GeneralizedEigenvalues(this DenseMatrix A, DenseMatrix B, DenseMatrix E) { // Cholesky factor of B. var L = B.Cholesky().Factor; // Compute L^-1. InvertLowerTriangle((DenseMatrix)L); // Compute L^-t = (L^-1)^t. var Lt = L.Transpose(); // Save L^-t for recovery of eigenvectors. var copy = (DenseMatrix)Lt.Clone(); // Build L^-1 * A * L^-t A.Multiply(Lt, Lt); L.Multiply(Lt, Lt); var evd = Lt.Evd(Symmetricity.Symmetric); // Recover eigenvectors. copy.Multiply(evd.EigenVectors, E); return(evd.EigenValues); }
/// <summary> /// Run example /// </summary> /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Scalar_multiplication">Multiply matrix by scalar</seealso> /// <seealso cref="http://reference.wolfram.com/mathematica/tutorial/MultiplyingVectorsAndMatrices.html">Multiply matrix by vector</seealso> /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product">Multiply matrix by matrix</seealso> /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_multiplication#Hadamard_product">Pointwise multiplies matrix with another matrix</seealso> /// <seealso cref="http://en.wikipedia.org/wiki/Matrix_%28mathematics%29#Basic_operations">Addition and subtraction</seealso> public void Run() { // Initialize IFormatProvider to print matrix/vector data var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone(); formatProvider.TextInfo.ListSeparator = " "; // Create matrix "A" var matrixA = new DenseMatrix(new[,] { { 1.0, 2.0, 3.0 }, { 4.0, 5.0, 6.0 }, { 7.0, 8.0, 9.0 } }); Console.WriteLine(@"Matrix A"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Create matrix "B" var matrixB = new DenseMatrix(new[,] { { 1.0, 3.0, 5.0 }, { 2.0, 4.0, 6.0 }, { 3.0, 5.0, 7.0 } }); Console.WriteLine(@"Matrix B"); Console.WriteLine(matrixB.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Multiply matrix by scalar // 1. Using operator "*" var resultM = 3.0 * matrixA; Console.WriteLine(@"Multiply matrix by scalar using operator *. (result = 3.0 * A)"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Multiply method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.Multiply(3.0); Console.WriteLine(@"Multiply matrix by scalar using method Multiply. (result = A.Multiply(3.0))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using Multiply method and updating matrix itself matrixA.Multiply(3.0, matrixA); Console.WriteLine(@"Multiply matrix by scalar using method Multiply. (A.Multiply(3.0, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Multiply matrix by vector (right-multiply) var vector = new DenseVector(new[] { 1.0, 2.0, 3.0 }); Console.WriteLine(@"Vector"); Console.WriteLine(vector.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 1. Using operator "*" var resultV = matrixA * vector; Console.WriteLine(@"Multiply matrix by vector using operator *. (result = A * vec)"); Console.WriteLine(resultV.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Multiply method and getting result into different vector instance resultV = (DenseVector)matrixA.Multiply(vector); Console.WriteLine(@"Multiply matrix by vector using method Multiply. (result = A.Multiply(vec))"); Console.WriteLine(resultV.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using Multiply method and updating vector itself matrixA.Multiply(vector, vector); Console.WriteLine(@"Multiply matrix by vector using method Multiply. (A.Multiply(vec, vec))"); Console.WriteLine(vector.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Multiply vector by matrix (left-multiply) // 1. Using operator "*" resultV = vector * matrixA; Console.WriteLine(@"Multiply vector by matrix using operator *. (result = vec * A)"); Console.WriteLine(resultV.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using LeftMultiply method and getting result into different vector instance resultV = (DenseVector)matrixA.LeftMultiply(vector); Console.WriteLine(@"Multiply vector by matrix using method LeftMultiply. (result = A.LeftMultiply(vec))"); Console.WriteLine(resultV.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using LeftMultiply method and updating vector itself matrixA.LeftMultiply(vector, vector); Console.WriteLine(@"Multiply vector by matrix using method LeftMultiply. (A.LeftMultiply(vec, vec))"); Console.WriteLine(vector.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Multiply matrix by matrix // 1. Using operator "*" resultM = matrixA * matrixB; Console.WriteLine(@"Multiply matrix by matrix using operator *. (result = A * B)"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Multiply method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.Multiply(matrixB); Console.WriteLine(@"Multiply matrix by matrix using method Multiply. (result = A.Multiply(B))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using Multiply method and updating matrix itself matrixA.Multiply(matrixB, matrixA); Console.WriteLine(@"Multiply matrix by matrix using method Multiply. (A.Multiply(B, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Pointwise multiplies matrix with another matrix // 1. Using PointwiseMultiply method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.PointwiseMultiply(matrixB); Console.WriteLine(@"Pointwise multiplies matrix with another matrix using method PointwiseMultiply. (result = A.PointwiseMultiply(B))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using PointwiseMultiply method and updating matrix itself matrixA.PointwiseMultiply(matrixB, matrixA); Console.WriteLine(@"Pointwise multiplies matrix with another matrix using method PointwiseMultiply. (A.PointwiseMultiply(B, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Pointwise divide matrix with another matrix // 1. Using PointwiseDivide method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.PointwiseDivide(matrixB); Console.WriteLine(@"Pointwise divide matrix with another matrix using method PointwiseDivide. (result = A.PointwiseDivide(B))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using PointwiseDivide method and updating matrix itself matrixA.PointwiseDivide(matrixB, matrixA); Console.WriteLine(@"Pointwise divide matrix with another matrix using method PointwiseDivide. (A.PointwiseDivide(B, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Addition // 1. Using operator "+" resultM = matrixA + matrixB; Console.WriteLine(@"Add matrices using operator +. (result = A + B)"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Add method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.Add(matrixB); Console.WriteLine(@"Add matrices using method Add. (result = A.Add(B))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using Add method and updating matrix itself matrixA.Add(matrixB, matrixA); Console.WriteLine(@"Add matrices using method Add. (A.Add(B, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Subtraction // 1. Using operator "-" resultM = matrixA - matrixB; Console.WriteLine(@"Subtract matrices using operator -. (result = A - B)"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Subtract method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.Subtract(matrixB); Console.WriteLine(@"Subtract matrices using method Subtract. (result = A.Subtract(B))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 3. Using Subtract method and updating matrix itself matrixA.Subtract(matrixB, matrixA); Console.WriteLine(@"Subtract matrices using method Subtract. (A.Subtract(B, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // Divide by scalar // 1. Using Divide method and getting result into different matrix instance resultM = (DenseMatrix)matrixA.Divide(3.0); Console.WriteLine(@"Divide matrix by scalar using method Divide. (result = A.Divide(3.0))"); Console.WriteLine(resultM.ToString("#0.00\t", formatProvider)); Console.WriteLine(); // 2. Using Divide method and updating matrix itself matrixA.Divide(3.0, matrixA); Console.WriteLine(@"Divide matrix by scalar using method Divide. (A.Divide(3.0, A))"); Console.WriteLine(matrixA.ToString("#0.00\t", formatProvider)); Console.WriteLine(); }
//def SSF.tf2ssd(num,denum,k,ts)# public static SSFilter TF2SSd(double[] Num, double[] Den, double k, double Ts) // n,num,denum = SSF::align_num_denum(num,denum) // a = [] // b = [] // c = [] // (0).upto(n-3) { // |i| // ta = NArray.float(n-1) // ta[i+1] = 1.0 // a << ta.to_a // b << [0.0] // } // ta = [] // (n-1).downto(1) { // |i| // ta << -denum[i]/denum[0] // } // a << ta // b << [1.0/denum[0]] // d = num[0]/denum[0] // c = NMatrix.float(n-1,1) // (n-1).downto(1) { // |i| // c[i-1,0] = d*(-denum[i]) + num[i] // } // am = NMatrix.rows(a) #Matrix.rows(a) // bm = NMatrix.rows(b) #Matrix.rows(b) // e = NMatrix.float(n-1,n-1).unit #Matrix.xdiag(n-1,1.0) // f = e + am*ts + am**2*ts**2/2 + am**3*ts**3/3 // r = (e*ts + am*ts**2/2 + am**2*ts**3/3)*bm #e*ts*bm // ssf = SSF.new(f,r,c,d,ts,k*denum.last/num.last) // ssf //end public static SSF C2DSS(Vector<double> num, Vector<double> denom, double gain, double timeSample) { int n = AlignNumDenom(ref num, ref denom); var a = new DenseMatrix(n - 1); var b = new DenseMatrix(n - 1, 1, 0.0); for (int i = 0; i < n-2; i++) { a[i, i + 1] = 1.0; } b[n - 2, 0] = 1/denom[0]; var d = num[0]/denom[0]; for (int i = 0; i < n-1; i++) { a[n - 2, i] = -denom[n - 1 - i]/denom[0]; } var c = new DenseMatrix(1, n - 1); for (int i = 0; i < n-1; i++) { c[0, i] = num[n - 1 - i] - denom[n - 1 - i] * d; } var e = new DenseMatrix(a.RowCount); for (int i = 0; i < a.RowCount; i++) { e[i,i] = 1.0; } var f = e + a.Multiply(timeSample) + a.Multiply(a).Multiply(timeSample * timeSample * 0.5);// + //a.Multiply(a).Multiply(a).Multiply(timeSample*timeSample*timeSample/3.0); var r = (e * timeSample + a.Multiply(timeSample * timeSample * 0.5)). // + a.Multiply(a).Multiply(Math.Pow(timeSample, 3)/3.0) ). Multiply(b); return new SSF((DenseMatrix)f, (DenseMatrix)r, (DenseMatrix)c, d, timeSample, gain * denom[n - 1] / num[n - 1]); }
/// <summary> /// Adaptive Cross Approximation (ACA) matrix compression /// the result is stored in U and V matrices like U*V /// </summary> /// <param name="acaThres">Relative error threshold to stop adding rows and columns in ACA iteration</param> /// <param name="m">Row indices of Z submatrix to compress</param> /// <param name="n">Column indices of Z submatrix to compress</param> /// <param name="U">to store result</param> /// <param name="V">to store result</param> /// <returns>pair with matrix U and V</returns> public static Tuple<Matrix, Matrix> Aca(double acaThres, List<int> m, List<int> n, Matrix U, Matrix V) { int M = m.Count; int N = n.Count; int Min = Math.Min(M, N); U = new DenseMatrix(Min, Min); V = new DenseMatrix(Min, Min); //if Z is a vector, there is nothing to compress if (M == 1 || N == 1) { U = UserImpedance(m, n); V = new DenseMatrix(1, 1); V[0, 0] = 1.0; return new Tuple<Matrix,Matrix>(U,V); } //Indices of columns picked up from Z //Vector J = new DenseVector(N); //List<int> J = new List<int>(N); List<int> J = new List<int>(new int [N]); //int[] J = new int[N]; //Indices of rows picked up from Z //Vector I = new DenseVector(M); List<int> I = new List<int>(new int [M]); //int[] I = new int[M]; //Row indices to search for maximum in R //Vector i = new DenseVector(M); List<int> i = new List<int>(); //int[] i = new int[M]; //Column indices to search for maximum in R //Vector j = new DenseVector(N); List<int> j = new List<int>(); //int[] j = new int[N]; for (int k = 1; k < M; k++) { i.Add(k); } for (int k = 0; k < N; k++) { j.Add(k); } //Initialization //Initialize the 1st row index I(1) = 1 I[0] = 0; //Initialize the 1st row of the approximate error matrix List<int> m0 = new List<int>(); m0.Add(m[I[0]]); Matrix Rik = UserImpedance(m0, n); //Find the 1st column index J(0) double max = -1.0; int col = 0; foreach (int ind in j) { if (Math.Abs(Rik[0, ind]) > max) { max = Math.Abs(Rik[0, ind]); col = ind; } } //J[0] = j[col]; J[0] = col; j.Remove(J[0]); //First row of V V = new DenseMatrix(1, Rik.ColumnCount); V.SetRow(0, Rik.Row(0).Divide(Rik[0, J[0]])); //Initialize the 1st column of the approximate error matrix List<int> n0 = new List<int>(); n0.Add(n[J[0]]); Matrix Rjk = UserImpedance(m, n0); //First column of U U = new DenseMatrix(Rjk.RowCount, 1); U.SetColumn(0, Rjk.Column(0)); // Norm of (approximate) Z, to test error double d1 = U.L2Norm(); double d2 = V.L2Norm(); double normZ = d1 * d1 * d2 * d2; //Find 2nd row index I(2) int row = 0; max = -1.0; foreach (int ind in i) { if (Math.Abs(Rjk[ind, 0]) > max) { max = Math.Abs(Rjk[ind, 0]); row = ind; } } //I[1] = i[row]; I[1] = row; i.Remove(I[1]); //Iteration for (int k = 1; k < Math.Min(M, N); k++) { //Update (Ik)th row of the approximate error matrix: List<int> t1 = new List<int>(); t1.Add(m[I[k]]); Rik = (Matrix)(UserImpedance(t1, n) - U.SubMatrix(I[k], 1, 0, U.ColumnCount).Multiply(V)); //CHECKED OK all code before works fine //Find kth column index Jk max = -1.0; col = 0; foreach (int ind in j) { if (Math.Abs(Rik[0, ind]) > max) { max = Math.Abs(Rik[0, ind]); col = ind; } } J[k] = col; j.Remove(J[k]); //Terminate if R(I(k),J(k)) == 0 if (Rik[0, J[k]] == 0) { break; } //Set k-th row of V equal to normalized error Matrix Vk = (Matrix)Rik.Divide(Rik[0, J[k]]); //Update (Jk)th column of the approximate error matrix List<int> n1 = new List<int>(); n1.Add(n[J[k]]); Rjk = (Matrix)(UserImpedance(m, n1) - U.Multiply(V.SubMatrix(0, V.RowCount, J[k], 1))); // Set k-th column of U equal to updated error Matrix Uk = Rjk; //Norm of approximate Z //Matrix s = (Matrix)(U.Transpose().Multiply(Uk)).Multiply((Vk.Multiply(V.Transpose())).Transpose()); //Matrix s = (Matrix)((U.Transpose()).Multiply(Uk)).Multiply(((Vk.Multiply(V.Transpose())).Transpose())); Matrix a = (Matrix)U.Transpose().Multiply(Uk); Matrix b = (Matrix)Vk.Multiply(V.Transpose()).Transpose(); Matrix s = (Matrix)a.PointwiseMultiply(b); double sum = 0; for (int i1 = 0; i1 < s.RowCount; i1++) { for (int j1 = 0; j1 < s.ColumnCount; j1++) { sum += s[i1, j1]; } } d1 = Uk.L2Norm(); d2 = Vk.L2Norm(); normZ += 2 * sum + d1 * d1 * d2 * d2; //Update U and V //U.SetColumn(U.ColumnCount - 1, Uk.Column(0)); //V.SetRow(V.RowCount - 1, Vk.Row(0)); U = AddColumn(U, (Vector)Uk.Column(0)); //U.SetColumn(k, Uk.Column(0)); V = AddRow(V, (Vector)Vk.Row(0)); //V.SetRow(k, Vk.Row(0)); if (d1 * d2 <= acaThres * Math.Sqrt(normZ)) { break; } if (k == Math.Min(N, M) - 1) { break; } max = -1; row = 0; foreach (int ind in i) { if (Math.Abs(Rjk[ind, 0]) > max) { max = Math.Abs(Rjk[ind, 0]); row = ind; } } I[k + 1] = row; //i = removeIndex(i,I[k+1]); i.Remove(I[k + 1]); } return new Tuple<Matrix, Matrix>(U, V); }
/// <summary> /// SkeletonFrameReady gets fired every skeleton frame update, and refreshes the LocatedSensor's /// global and relative skeleton maps /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void refreshSkeletonPositions(object sender, SkeletonFrameReadyEventArgs e) { using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) { if (skeletonFrame != null) { // First, get the relative skeletons - easy peasy Skeleton[] skeletonsR = new Skeleton[skeletonFrame.SkeletonArrayLength]; skeletonFrame.CopySkeletonDataTo(skeletonsR); this.relativeSkeletons = skeletonsR.ToList<Skeleton>(); // Now global skeletons... // First, clear our global skeletons list. // We'll be building this back up from scratch here this.globalSkeletons.Clear(); // Next, iterate through all the skeletons, applying a rotation and translation // to get us into global coordinates foreach (Skeleton skel in this.relativeSkeletons) { // Add a temporary skeleton object to store transformed // data into Skeleton tempSkel = skel; foreach (Joint j in skel.Joints){ // Make a new joint, then put it into our temporary joint // collection JointType type = j.JointType; Joint tempJoint = tempSkel.Joints[type]; // Copy the current joint state JointTrackingState tracking = j.TrackingState; tempJoint.TrackingState = tracking; // However, we transform the position of the joint at least SkeletonPoint shiftedPoint = new SkeletonPoint(); // Rotate the points DenseMatrix point = new DenseMatrix(1, 3); point[0, 0] = j.Position.X; point[0, 1] = j.Position.Y; point[0, 2] = j.Position.Z; var rotatedPoint = point.Multiply(this.rotationMatrix); // Then shift them by the global coordinates. shiftedPoint.X = (float)rotatedPoint[0, 0] + this.xOffset; shiftedPoint.Y = (float)rotatedPoint[0, 1] + this.yOffset; shiftedPoint.Z = (float)rotatedPoint[0, 2] + this.zOffset; tempJoint.Position = shiftedPoint; tempSkel.Joints[type] = tempJoint; } // Next, alter the higher-level parameters of our skeleton SkeletonPoint shiftedPosition = new SkeletonPoint(); // Rotate DenseMatrix p = new DenseMatrix(1, 3); p[0, 0] = tempSkel.Position.X; p[0, 1] = tempSkel.Position.Y; p[0, 2] = tempSkel.Position.Z; var rPoint = p.Multiply(this.rotationMatrix); // Then shift them by the global coordinates. shiftedPosition.X = (float)rPoint[0, 0] + this.xOffset; shiftedPosition.Y = (float)rPoint[0, 1] + this.yOffset; shiftedPosition.Z = (float)rPoint[0, 2] + this.zOffset; tempSkel.Position = shiftedPosition; // Now add that skeleton to our global skeleton list this.globalSkeletons.Add(tempSkel); } } } }
public void Train(DenseMatrix X, DenseVector d, DenseVector Kd) { int R = X.RowCount; int N = X.ColumnCount; int U = 0; //the number of neurons in the structure var c = new DenseMatrix(R, 1); var sigma = new DenseMatrix(R, 1); var Q = new DenseMatrix((R + 1), (R + 1)); var O = new DenseMatrix(1, (R + 1)); var pT_n = new DenseMatrix((R + 1), 1); double maxPhi = 0; int maxIndex; var Psi = new DenseMatrix(N, 1); Console.WriteLine("Running..."); //for each observation n in X for (int i = 0; i < N; i++) { Console.WriteLine(100*(i/(double) N) + "%"); var x = new DenseVector(R); X.Column(i, x); //if there are neurons in structure, //update structure recursively. if (U == 0) { c = (DenseMatrix) x.ToColumnMatrix(); sigma = new DenseMatrix(R, 1, SigmaZero); U = 1; Psi = CalculatePsi(X, c, sigma); UpdateStructure(X, Psi, d, ref Q, ref O); pT_n = (DenseMatrix) (CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) Psi.Row(i).ToRowMatrix())) .Transpose(); } else { StructureRecurse(X, Psi, d, i, ref Q, ref O, ref pT_n); } bool KeepSpinning = true; while (KeepSpinning) { //Calculate the error and if-part criteria double ee = pT_n.Multiply(O)[0, 0]; double approximationError = Math.Abs(d[i] - ee); DenseVector Phi; double SumPhi; CalculatePhi(x, c, sigma, out Phi, out SumPhi); maxPhi = Phi.Maximum(); maxIndex = Phi.MaximumIndex(); if (approximationError > delta) { if (maxPhi < threshold) { var tempSigma = new DenseVector(R); sigma.Column(maxIndex, tempSigma); double minSigma = tempSigma.Minimum(); int minIndex = tempSigma.MinimumIndex(); sigma[minIndex, maxIndex] = k_sigma*minSigma; Psi = CalculatePsi(X, c, sigma); UpdateStructure(X, Psi, d, ref Q, ref O); var psi = new DenseVector(Psi.ColumnCount); Psi.Row(i, psi); pT_n = (DenseMatrix) CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) psi.ToRowMatrix()) .Transpose(); } else { //add a new neuron and update strucutre double distance = 0; var cTemp = new DenseVector(R); var sigmaTemp = new DenseVector(R); //foreach input variable for (int j = 0; j < R; j++) { distance = Math.Abs(x[j] - c[j, 0]); int distanceIndex = 0; //foreach neuron past 1 for (int k = 1; k < U; k++) { if ((Math.Abs(x[j] - c[j, k])) < distance) { distanceIndex = k; distance = Math.Abs(x[j] - c[j, k]); } } if (distance < Kd[j]) { cTemp[j] = c[j, distanceIndex]; sigmaTemp[j] = sigma[j, distanceIndex]; } else { cTemp[j] = x[j]; sigmaTemp[j] = distance; } } //end foreach c = (DenseMatrix) c.InsertColumn(c.ColumnCount - 1, cTemp); sigma = (DenseMatrix) sigma.InsertColumn(sigma.ColumnCount - 1, sigmaTemp); Psi = CalculatePsi(X, c, sigma); UpdateStructure(X, Psi, d, ref Q, ref O); U++; KeepSpinning = false; } } else { if (maxPhi < threshold) { var tempSigma = new DenseVector(R); sigma.Column(maxIndex, tempSigma); double minSigma = tempSigma.Minimum(); int minIndex = tempSigma.MinimumIndex(); sigma[minIndex, maxIndex] = k_sigma*minSigma; Psi = CalculatePsi(X, c, sigma); UpdateStructure(X, Psi, d, ref Q, ref O); var psi = new DenseVector(Psi.ColumnCount); Psi.Row(i, psi); pT_n = (DenseMatrix) CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) psi.ToRowMatrix()) .Transpose(); } else { KeepSpinning = false; } } } } out_C = c; out_O = O; out_Sigma = sigma; Console.WriteLine("Done."); }
public void StructureRecurse(DenseMatrix X, DenseMatrix Psi, DenseVector d, int n, ref DenseMatrix Q, ref DenseMatrix O, ref DenseMatrix pT_n) { //O = O(t-1) O_enxt = O(t) //o should be a column vector ( in matrix form) var x = new DenseVector(X.RowCount); var psi = new DenseVector(Psi.ColumnCount); X.Column(n, x); Psi.Row(n, psi); DenseMatrix p_n = CalculateGreatPsi((DenseMatrix) x.ToColumnMatrix(), (DenseMatrix) psi.ToRowMatrix()); pT_n = (DenseMatrix) p_n.Transpose(); double ee = Math.Abs(d[n] - (pT_n.Multiply(O))[0, 0]); double temp = 1 + (pT_n.Multiply(Q)).Multiply(p_n)[0, 0]; double ae = Math.Abs(ee/temp); if (ee >= ae) { var L = (DenseMatrix) Q.Multiply(p_n).Multiply(1/temp); Q = (DenseMatrix) ((DenseMatrix.Identity(Q.RowCount).Subtract(L.Multiply(pT_n))).Multiply(Q)); O = (DenseMatrix) O.Add(L*ee); } else { Q = (DenseMatrix) DenseMatrix.Identity(Q.RowCount).Multiply(Q); } }
private void ComputeEssentialFundamental() { if (!IsCamLeftCalibrated || !IsCamRightCalibrated) return; // E = [T]xR -> translation/rotation from L to R frames // Al = [Rl|Tl], Al^-1 = [Rl^T | -Rl^T * Tl] (https://pl.wikipedia.org/wiki/Elementarne_macierze_transformacji) // Al->r = [R|T] = Ar * Al^-1 // [R|T] = [Rr*Rl^T | Rr * (-Rl^T * Tl) + Tr] Matrix<double> rotLR = RotationRight.Multiply(RotationLeft.Transpose()); Matrix<double> transLR = (RotationRight.Multiply( -RotationLeft.Transpose().Multiply(TranslationLeft))) .Add(TranslationRight); Matrix<double> skewTransMat = new DenseMatrix(3, 3); skewTransMat[0, 0] = 0; skewTransMat[0, 1] = -transLR[2, 0]; skewTransMat[0, 2] = transLR[1, 0]; skewTransMat[1, 0] = transLR[2, 0]; skewTransMat[1, 1] = 0; skewTransMat[1, 2] = -transLR[0, 0]; skewTransMat[2, 0] = -transLR[1, 0]; skewTransMat[2, 1] = transLR[0, 0]; skewTransMat[2, 2] = 0; Essential = skewTransMat.Multiply(rotLR); // F = Kr^-T * E * Kl^-1 Fundamental = CalibrationRight.Inverse().Transpose().Multiply(Essential).Multiply(CalibrationLeft.Inverse()); }
/// <summary> /// Train. Single iteration. /// </summary> public void Iteration() { int rowCount = _trainingData.Count; int inputColCount = _trainingData[0].Input.Length; Matrix<double> xMatrix = new DenseMatrix(rowCount, inputColCount + 1); Matrix<double> yMatrix = new DenseMatrix(rowCount, 1); for (int row = 0; row < _trainingData.Count; row++) { BasicData dataRow = _trainingData[row]; int colSize = dataRow.Input.Count(); xMatrix[row, 0] = 1; for (int col = 0; col < colSize; col++) { xMatrix[row, col + 1] = dataRow.Input[col]; } yMatrix[row, 0] = dataRow.Ideal[0]; } // Calculate the least squares solution QR qr = xMatrix.QR(); Matrix<double> beta = qr.Solve(yMatrix); double sum = 0.0; for (int i = 0; i < inputColCount; i++) sum += yMatrix[i, 0]; double mean = sum/inputColCount; for (int i = 0; i < inputColCount; i++) { double dev = yMatrix[i, 0] - mean; _sst += dev*dev; } Matrix<double> residuals = xMatrix.Multiply(beta).Subtract(yMatrix); _sse = residuals.L2Norm()*residuals.L2Norm(); for (int i = 0; i < _algorithm.LongTermMemory.Length; i++) { _algorithm.LongTermMemory[i] = beta[i, 0]; } // calculate error _errorCalculation.Clear(); foreach (BasicData dataRow in _trainingData) { double[] output = _algorithm.ComputeRegression(dataRow.Input); _errorCalculation.UpdateError(output, dataRow.Ideal, 1.0); } _error = _errorCalculation.Calculate(); }
private double[] Polyfit(double[] x, double[] y, int degree) { // Vandermonde matrix var v = new DenseMatrix(x.Length, degree + 1); for (int i = 0; i < v.RowCount; i++) for (int j = 0; j <= degree; j++) v[i, j] = Math.Pow(x[i], j); var yv = new DenseVector(y).ToColumnMatrix(); QR<double> qr = v.QR(); // Math.Net doesn't have an "economy" QR, so: // cut R short to square upper triangle, then recompute Q var r = qr.R.SubMatrix(0, degree + 1, 0, degree + 1); var q = v.Multiply(r.Inverse()); var p = r.Inverse().Multiply(q.TransposeThisAndMultiply(yv)); return p.Column(0).ToArray(); }
public static Matrix<double> WorldToImagePoints(this Matrix<double> worldPoints, DenseMatrix cameraCalibration, DenseVector posePosition, DenseMatrix poseRotation) { return cameraCalibration.Multiply( poseRotation.Inverse() .Multiply(worldPoints) .Translate(-posePosition) .ProjectCamCenteredWorldToHomogImagePoints()); /*This version is consistent with my DPixelToWorld but different from openCV ProjectPoints * cameraCalibration.Multiply( poseRotation.Inverse() .Multiply(worldPoints.Translate(-posePosition)) .ProjectCamCenteredWorldToHomogImagePoints());*/ }
public static DenseMatrix DPixelToWorld(DenseMatrix inverseCalibration, DenseMatrix homogeneousPixels, DenseMatrix depths) { return (DenseMatrix) depths.Replicate(3,1) .PointwiseMultiply(inverseCalibration.Multiply(homogeneousPixels)); }