/// <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));
}
