/// <summary>
/// Warp a matrix M as R * M * R^T
/// </summary>
public static MatrixXD WarpMatrix(Quaternion R, MatrixXD M)
{
MatrixXD mout = new DenseMatrixXD(3, 3);
mout.SetRow(0, ToVectorXD(R * ToVector3(M.Row(0))));
mout.SetRow(1, ToVectorXD(R * ToVector3(M.Row(1))));
mout.SetRow(2, ToVectorXD(R * ToVector3(M.Row(2))));
mout.SetColumn(0, ToVectorXD(R * ToVector3(mout.Column(0))));
mout.SetColumn(1, ToVectorXD(R * ToVector3(mout.Column(1))));
mout.SetColumn(2, ToVectorXD(R * ToVector3(mout.Column(2))));
return(mout);
}
public void build_prob_map()
{
Normal N_x = new Normal(X / 2, STD_X);
Normal N_y = new Normal(Y / 2, STD_Y);
DenseMatrix M_x = new DenseMatrix(Y, X, 0.0);
DenseMatrix M_y = new DenseMatrix(Y, X, 0.0);
DenseVector V_x = new DenseVector(X);
for (int i = 0; i < X; i++)
{
V_x[i] = N_x.Density(i);
}
for (int j = 0; j < Y; j++)
{
M_x.SetRow(j, V_x);
}
DenseVector V_y = new DenseVector(Y);
for (int i = 0; i < Y; i++)
{
V_y[i] = N_y.Density(i);
}
for (int j = 0; j < X; j++)
{
M_y.SetColumn(j, V_y);
}
DenseMatrix MULT = (DenseMatrix)M_x.PointwiseMultiply(M_y);
double s = MULT.Data.Sum();
MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(Y, X, s));
//this.dataGridView1.DataSource = MULT;
//Console.WriteLine(MULT.Data.Sum());
PROB_MAP_ORIG = MULT;
s = MULT[Y / 2, X / 2];
MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(Y, X, s));
/*
for (int i = 0; i < Y; i++)
{
Console.Write(i + " - ");
for (int j = 0; j < X; j++)
{
Console.Write(MULT[i, j] + " ");
}
Console.WriteLine();
Console.WriteLine();
}
*/
PROB_MAP = MULT;
}
public void Smooth(ref double[,] inputValues)
{
// TODO: Using the matrix works, but does a lot of data accesses. Can improve by working out all the data access myself? I might be able to cut down on number of data accesses, but not sure.
var inputMatrix = new DenseMatrix(inputValues.GetLength(0), inputValues.GetLength(1));
for (int i = 0; i < inputMatrix.RowCount; i++)
{
inputMatrix.SetRow(i, Smooth(inputMatrix.Row(i).ToArray()));
}
for (int i = 0; i < inputMatrix.ColumnCount; i++)
{
inputMatrix.SetColumn(i, Smooth(inputMatrix.Column(i).ToArray()));
}
inputValues = inputMatrix.ToArray();
}
private Matrix<double> CameraMatrixFromParameterization(int n0)
{
// Parameters:
// Full: [fx, fy, s, px, py, eaX, eaY, eaZ, Cx, Cy, Cz]
// Center fixed: [fx, fy, s, eaX, eaY, eaZ, Cx, Cy, Cz]
// Aspect fixed: [f, eaX, eaY, eaZ, Cx, Cy, Cz]
//
// Camera : M = KR[I|-C]
double fx = ParametersVector.At(n0 + _fxIdx);
double fy = ParametersVector.At(n0 + _fyIdx);
double sk = ParametersVector.At(n0 + _skIdx);
double px = ParametersVector.At(n0 + _pxIdx);
double py = ParametersVector.At(n0 + _pyIdx);
double eX = ParametersVector.At(n0 + _euXIdx);
double eY = ParametersVector.At(n0 + _euYIdx);
double eZ = ParametersVector.At(n0 + _euZIdx);
Vector<double> euler = new DenseVector(new double[3] { eX, eY, eZ });
double cX = ParametersVector.At(n0 + _cXIdx);
double cY = ParametersVector.At(n0 + _cYIdx);
double cZ = ParametersVector.At(n0 + _cZIdx);
Vector<double> center = new DenseVector(new double[3] { -cX, -cY, -cZ });
Matrix<double> intMat = new DenseMatrix(3, 3);
intMat.At(0, 0, fx);
intMat.At(0, 1, sk);
intMat.At(0, 2, px);
intMat.At(1, 1, fy);
intMat.At(1, 2, py);
intMat.At(2, 2, 1.0);
Matrix<double> rotMat = new DenseMatrix(3, 3);
RotationConverter.EulerToMatrix(euler, rotMat);
Matrix<double> extMat = new DenseMatrix(3, 4);
extMat.SetSubMatrix(0, 0, rotMat);
extMat.SetColumn(3, rotMat * center);
return intMat * extMat;
}
/// <summary>
/// Run example
/// </summary>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create square matrix
var matrix = new DenseMatrix(5);
var k = 0;
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrix[i, j] = k++;
}
}
Console.WriteLine(@"Initial matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Create vector
var vector = new DenseVector(new[] { 50.0, 51.0, 52.0, 53.0, 54.0 });
Console.WriteLine(@"Sample vector");
Console.WriteLine(vector.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Insert new column
var result = matrix.InsertColumn(3, vector);
Console.WriteLine(@"1. Insert new column");
Console.WriteLine(result.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Insert new row
result = matrix.InsertRow(3, vector);
Console.WriteLine(@"2. Insert new row");
Console.WriteLine(result.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Set column values
matrix.SetColumn(2, (Vector)vector);
Console.WriteLine(@"3. Set column values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 4. Set row values.
matrix.SetRow(3, (double[])vector);
Console.WriteLine(@"4. Set row values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 5. Set diagonal values. SetRow/SetColumn/SetDiagonal accepts Vector and double[] as input parameter
matrix.SetDiagonal(new[] { 5.0, 4.0, 3.0, 2.0, 1.0 });
Console.WriteLine(@"5. Set diagonal values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 6. Set submatrix values
matrix.SetSubMatrix(1, 3, 1, 3, DenseMatrix.Identity(3));
Console.WriteLine(@"6. Set submatrix values");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Permutations.
// Initialize a new instance of the Permutation class. An array represents where each integer is permuted too:
// indices[i] represents that integer "i" is permuted to location indices[i]
var permutations = new Permutation(new[] { 0, 1, 3, 2, 4 });
// 7. Permute rows 3 and 4
matrix.PermuteRows(permutations);
Console.WriteLine(@"7. Permute rows 3 and 4");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 8. Permute columns 1 and 2, 3 and 5
permutations = new Permutation(new[] { 1, 0, 4, 3, 2 });
matrix.PermuteColumns(permutations);
Console.WriteLine(@"8. Permute columns 1 and 2, 3 and 5");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 9. Concatenate the matrix with the given matrix
var append = matrix.Append(matrix);
// Concatenate into result matrix
matrix.Append(matrix, append);
Console.WriteLine(@"9. Append matrix to matrix");
Console.WriteLine(append.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 10. Stack the matrix on top of the given matrix matrix
var stack = matrix.Stack(matrix);
// Stack into result matrix
matrix.Stack(matrix, stack);
Console.WriteLine(@"10. Stack the matrix on top of the given matrix matrix");
Console.WriteLine(stack.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 11. Diagonally stack the matrix on top of the given matrix matrix
var diagoinalStack = matrix.DiagonalStack(matrix);
// Diagonally stack into result matrix
matrix.DiagonalStack(matrix, diagoinalStack);
Console.WriteLine(@"11. Diagonally stack the matrix on top of the given matrix matrix");
Console.WriteLine(diagoinalStack.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
/// <summary>
/// adds new column to matrix
/// </summary>
/// <param name="dest">matrix which add column</param>
/// <param name="colToAdd">column added to matrix</param>
/// <returns>new matrix</returns>
private static Matrix AddColumn(Matrix dest, Vector colToAdd)
{
Matrix res = new DenseMatrix(dest.RowCount, dest.ColumnCount + 1);
res.SetSubMatrix(0, dest.RowCount, 0, dest.ColumnCount, dest);
res.SetColumn(res.ColumnCount - 1, colToAdd);
return res;
}
/// <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);
}
public void placeCompensatorPoles(double value)
{
// Check for dimensions
if (A.ColumnCount != A.RowCount || A.ColumnCount <= 0) {
// TODO: Throw exception
}
int n = A.ColumnCount;
// Calculate controllability matrix
Matrix<double> controllabilityMatrix = new DenseMatrix(n, n);
for (int i = 0; i < n; i++) {
Vector<double> vec = B.Column(0);
for (int j = 0; j < i; j++) {
vec = A * vec;
}
controllabilityMatrix.SetColumn(i, vec);
}
// Unity vector
Matrix<double> unityVector = new DenseMatrix(1,n);
for (int i = 0; i < n; i++) {
// Set 1 at last index
unityVector.At(0, i,
(i == n-1 ? 1 : 0)
);
}
// Coefficients matrix
Matrix<double> preparedMatrix = A.Clone();
for (int i = 0; i < n; i++) {
// Substract value from diagonal
preparedMatrix.At(i, i,
preparedMatrix.At(i, i) - value
);
}
Matrix<double> coefficientsMatrix = preparedMatrix.Clone();
for (int i = 0; i < n-1; i++) {
// Multiply n-1 times
coefficientsMatrix = preparedMatrix * coefficientsMatrix;
}
// Calculate new K using Ackermann's formula
K = unityVector * controllabilityMatrix.Inverse() * coefficientsMatrix;
}
public void CreateCameraMatrices()
{
_K_L = new DenseMatrix(3, 3);
_K_L[0, 0] = 10.0; // fx
_K_L[1, 1] = 10.0; // fy
_K_L[0, 1] = 0.0; // s
_K_L[0, 2] = 300.0; // x0
_K_L[1, 2] = 250.0; // y0
_K_L[2, 2] = 1.0; // 1
_K_R = new DenseMatrix(3, 3);
_K_R[0, 0] = 10.0; // fx
_K_R[1, 1] = 10.5; // fy
_K_R[0, 1] = 0.0; // s
_K_R[0, 2] = 300.0; // x0
_K_R[1, 2] = 200.0; // y0
_K_R[2, 2] = 1.0; // 1
_R_L = DenseMatrix.CreateIdentity(3);
_R_R = DenseMatrix.CreateIdentity(3);
_C_L = new DenseVector(3);
_C_L[0] = 50.0;
_C_L[1] = 50.0;
_C_L[2] = 0.0;
_C_R = new DenseVector(3);
_C_R[0] = 0.0;
_C_R[1] = 40.0;
_C_R[2] = 10.0;
Matrix<double> Ext_l = new DenseMatrix(3, 4);
Ext_l.SetSubMatrix(0, 0, _R_L);
Ext_l.SetColumn(3, -_R_L * _C_L);
Matrix<double> Ext_r = new DenseMatrix(3, 4);
Ext_r.SetSubMatrix(0, 0, _R_R);
Ext_r.SetColumn(3, -_R_R * _C_R);
_CM_L = _K_L * Ext_l;
_CM_R = _K_R * Ext_r;
}
/// <summary>
/// Generate a random n-class classification problem.
/// </summary>
/// <param name="nSamples">The number of samples.</param>
/// <param name="nFeatures">The total number of features. These comprise <paramref name="nInformative"/>
/// informative features, <paramref name="nRedundant"/> redundant features, <paramref name="nRepeated"/>
/// dupplicated features and `<paramref name="nFeatures"/>-<paramref name="nInformative"/>-<paramref name="nRedundant"/>-
/// <paramref name="nRepeated"/>` useless features drawn at random.</param>
/// <param name="nInformative">The number of informative features. Each class is composed of a number
/// of gaussian clusters each located around the vertices of a hypercube
/// in a subspace of dimension <paramref name="nInformative"/>. For each cluster,
/// informative features are drawn independently from N(0, 1) and then
/// randomly linearly combined in order to add covariance. The clusters
/// are then placed on the vertices of the hypercube.</param>
/// <param name="nRedundant">The number of redundant features. These features are generated as
/// random linear combinations of the informative features.</param>
/// <param name="nRepeated"> The number of dupplicated features, drawn randomly from the informative
/// and the redundant features.
/// </param>
/// <param name="nClasses">The number of classes (or labels) of the classification problem.</param>
/// <param name="nClustersPerClass">The number of clusters per class.</param>
/// <param name="weights">The proportions of samples assigned to each class. If None, then
/// classes are balanced. Note that if `len(weights) == n_classes - 1`,
/// then the last class weight is automatically inferred.
/// </param>
/// <param name="flipY">The fraction of samples whose class are randomly exchanged.</param>
/// <param name="classSep">The factor multiplying the hypercube dimension.</param>
/// <param name="hypercube">If True, the clusters are put on the vertices of a hypercube. If
/// False, the clusters are put on the vertices of a random polytope.</param>
/// <param name="shift">Shift all features by the specified value. If None, then features
/// are shifted by a random value drawn in [-class_sep, class_sep].</param>
/// <param name="scale">Multiply all features by the specified value. If None, then features
/// are scaled by a random value drawn in [1, 100]. Note that scaling
/// happens after shifting.
/// </param>
/// <param name="shuffle">Shuffle the samples and the features.</param>
/// <param name="randomState">Random generator.</param>
/// <returns>array of shape [n_samples]
/// The integer labels for class membership of each sample.</returns>
/// <remarks>
/// The algorithm is adapted from Guyon [1] and was designed to generate
/// the "Madelon" dataset.
/// References
/// ----------
/// .. [1] I. Guyon, "Design of experiments for the NIPS 2003 variable
/// selection benchmark", 2003.
/// </remarks>
public static Classification MakeClassification(
int nSamples = 100,
int nFeatures = 20,
int nInformative = 2,
int nRedundant = 2,
int nRepeated = 0,
int nClasses = 2,
int nClustersPerClass = 2,
List<double> weights = null,
double flipY = 0.01,
double classSep = 1.0,
bool hypercube = true,
double? shift = 0.0,
double? scale = 1.0,
bool shuffle = true,
Random randomState = null)
{
var generator = randomState ?? new Random();
// Count features, clusters and samples
if (nInformative + nRedundant + nRepeated > nFeatures)
{
throw new ArgumentException("Number of informative, redundant and repeated " +
"features must sum to less than the number of total" +
" features");
}
if (nInformative * nInformative < nClasses * nClustersPerClass)
{
throw new ArgumentException(
"n_classes * n_clusters_per_class must" +
"be smaller or equal 2 ** n_informative");
}
if (weights != null && !new[] { nClasses, nClasses - 1 }.Contains(weights.Count))
{
throw new ArgumentException("Weights specified but incompatible with number of classes.");
}
int nUseless = nFeatures - nInformative - nRedundant - nRepeated;
int nClusters = nClasses * nClustersPerClass;
if (weights != null && weights.Count == nClasses - 1)
{
weights.Add(1.0 - weights.Sum());
}
if (weights == null)
{
weights = Enumerable.Repeat(1.0 / nClasses, nClasses).ToList();
weights[weights.Count - 1] = 1.0 - weights.Take(weights.Count - 1).Sum();
}
var nSamplesPerCluster = new List<int>();
for (int k = 0; k < nClusters; k++)
{
nSamplesPerCluster.Add(
(int)(nSamples * weights[k % nClasses] / nClustersPerClass));
}
for (int i = 0; i < nSamples - nSamplesPerCluster.Sum(); i++)
{
nSamplesPerCluster[i % nClusters] += 1;
}
// Intialize X and y
Matrix x = new DenseMatrix(nSamples, nFeatures);
int[] y = new int[nSamples];
// Build the polytope
Matrix c = new DenseMatrix(1 << nInformative, nInformative);
for (int i = 0; i < 1 << nInformative; i++)
{
var row = new DenseVector(nInformative);
for (int bitN = 0; bitN < nInformative; bitN++)
{
row[bitN] = (i & (1 << bitN)) == 1 ? classSep : -classSep;
}
c.SetRow(i, row);
}
if (!hypercube)
{
for (int k = 0; k < nClusters; k++)
{
c.SetRow(k, c.Row(k) * generator.NextDouble());
}
for (int f = 0; f < nInformative; f++)
{
c.SetColumn(f, c.Column(f) * generator.NextDouble());
}
}
// todo:
// generator.shuffle(C)
// Loop over all clusters
int pos = 0;
int posEnd = 0;
for (int k = 0; k < nClusters; k++)
{
// Number of samples in cluster k
int nSamplesK = nSamplesPerCluster[k];
// Define the range of samples
pos = posEnd;
posEnd = pos + nSamplesK;
// Assign labels
for (int l = pos; l < posEnd; l++)
{
y[l] = k % nClasses;
}
// Draw features at random
var subMatrix = DenseMatrix.CreateRandom(
nSamplesK,
nInformative,
new Normal { RandomSource = generator });
x.SetSubMatrix(pos, nSamplesK, 0, nInformative, subMatrix);
// Multiply by a random matrix to create co-variance of the features
var uniform = new ContinuousUniform(-1, 1) { RandomSource = generator };
Matrix a = DenseMatrix.CreateRandom(nInformative, nInformative, uniform);
x.SetSubMatrix(
pos,
nSamplesK,
0,
nInformative,
x.SubMatrix(pos, nSamplesK, 0, nInformative) * a);
// Shift the cluster to a vertice
var v = x.SubMatrix(pos, nSamplesK, 0, nInformative).AddRowVector(c.Row(k));
x.SetSubMatrix(pos, nSamplesK, 0, nInformative, v);
}
// Create redundant features
if (nRedundant > 0)
{
var uniform = new ContinuousUniform(-1, 1) { RandomSource = generator };
Matrix b = DenseMatrix.CreateRandom(nInformative, nRedundant, uniform);
x.SetSubMatrix(
0,
x.RowCount,
nInformative,
nRedundant,
x.SubMatrix(0, x.RowCount, 0, nInformative) * b);
}
// Repeat some features
if (nRepeated > 0)
{
int n = nInformative + nRedundant;
for (int i = 0; i < nRepeated; i++)
{
int r = (int)((generator.Next(nRepeated) * (n - 1)) + 0.5);
x.SetColumn(i, x.Column(r));
}
}
// Fill useless features
var denseMatrix = DenseMatrix.CreateRandom(nSamples, nUseless, new Normal { RandomSource = generator });
x.SetSubMatrix(0, nSamples, nFeatures - nUseless, nUseless, denseMatrix);
// Randomly flip labels
if (flipY >= 0.0)
{
for (int i = 0; i < nSamples; i++)
{
if (generator.NextDouble() < flipY)
{
y[i] = generator.Next(nClasses);
}
}
}
// Randomly shift and scale
bool constantShift = shift != null;
bool constantScale = scale != null;
for (int f = 0; f < nFeatures; f++)
{
if (!constantShift)
{
shift = ((2 * generator.NextDouble()) - 1) * classSep;
}
if (!constantScale)
{
scale = 1 + (100 * generator.NextDouble());
}
x.SetColumn(f, (x.Column(f) + shift.Value) * scale.Value);
}
// Randomly permute samples and features
// todo:
/*
if (shuffle)
{
X, y = util_shuffle(X, y, random_state=generator)
indices = np.arange(n_features)
generator.shuffle(indices)
X[:, :] = X[:, indices]
}*/
return new Classification { X = x, Y = y };
}
private DenseMatrix GetBaseMatrix(DenseMatrix matrix, List<int> baseJ)
{
var resM = new DenseMatrix(baseJ.Count);
int i = 0;
foreach (var j in baseJ)
{
resM.SetColumn(i, matrix.Column(j));
i++;
}
return resM;
}
public void Test_Rectification()
{
var Fi = new DenseMatrix(3); // Target F
Fi[1, 2] = -1.0;
Fi[2, 1] = 1.0;
var K_l = new DenseMatrix(3, 3);
K_l[0, 0] = 10.0; // fx
K_l[1, 1] = 10.0; // fy
K_l[0, 1] = 0.0; // s
K_l[0, 2] = 300.0; // x0
K_l[1, 2] = 250.0; // y0
K_l[2, 2] = 1.0; // 1
var K_r = new DenseMatrix(3, 3);
K_r[0, 0] = 12.0; // fx
K_r[1, 1] = 12.5; // fy
K_r[0, 1] = 0.0; // s
K_r[0, 2] = 300.0; // x0
K_r[1, 2] = 200.0; // y0
K_r[2, 2] = 1.0; // 1
var R_l = DenseMatrix.CreateIdentity(3);
var R_r = DenseMatrix.CreateIdentity(3);
Vector<double> C_l = new DenseVector(3);
C_l[0] = 50.0;
C_l[1] = 50.0;
C_l[2] = 0.0;
Vector<double> C_r = new DenseVector(3);
C_r[0] = 40.0;
C_r[1] = 40.0;
C_r[2] = 10.0;
Matrix<double> Ext_l = new DenseMatrix(3, 4);
Ext_l.SetSubMatrix(0, 0, R_l);
Ext_l.SetColumn(3, -R_l * C_l);
Matrix<double> Ext_r = new DenseMatrix(3, 4);
Ext_r.SetSubMatrix(0, 0, R_r);
Ext_r.SetColumn(3, -R_r * C_r);
var CM_l = K_l * Ext_l;
var CM_r = K_r * Ext_r;
// Find e_R = P_R*C_L, e_L = P_L*C_R
var epi_r = CM_r * new DenseVector(new double[] { C_l[0], C_l[1], C_l[2], 1.0 });
var epi_l = CM_l * new DenseVector(new double[] { C_r[0], C_r[1], C_r[2], 1.0 });
var ex_l = new DenseMatrix(3, 3);
ex_l[0, 0] = 0.0;
ex_l[1, 0] = epi_l[2];
ex_l[2, 0] = -epi_l[1];
ex_l[0, 1] = -epi_l[2];
ex_l[1, 1] = 0.0;
ex_l[2, 1] = epi_l[0];
ex_l[0, 2] = epi_l[1];
ex_l[1, 2] = -epi_l[0];
ex_l[2, 2] = 0.0;
var ex_r = new DenseMatrix(3, 3);
ex_r[0, 0] = 0.0;
ex_r[1, 0] = epi_r[2];
ex_r[2, 0] = -epi_r[1];
ex_r[0, 1] = -epi_r[2];
ex_r[1, 1] = 0.0;
ex_r[2, 1] = epi_r[0];
ex_r[0, 2] = epi_r[1];
ex_r[1, 2] = -epi_r[0];
ex_r[2, 2] = 0.0;
// F = [er]x * Pr * pseudoinv(Pl)
var F = ex_r * (CM_r * CM_l.PseudoInverse());
int rank = F.Rank();
if(rank == 3)
{
// Need to ensure rank 2, so set smallest singular value to 0
var svd = F.Svd();
var E = svd.W;
E[2, 2] = 0;
var oldF = F;
F = svd.U * E * svd.VT;
var diff = F - oldF; // Difference should be very small if all is correct
}
// Scale F, so that F33 = 1
F = F.Divide(F[2, 2]);
ImageRectification_ZhangLoop rect = new ImageRectification_ZhangLoop();
// Assume image of size 640x480
rect.ImageHeight = 480;
rect.ImageWidth = 640;
rect.FundamentalMatrix = F;
rect.EpiCrossLeft = ex_l;
rect.ComputeRectificationMatrices();
// Test H'^T * Fi * H should be very close to F
var H_r = rect.RectificationRight;
var H_l = rect.RectificationLeft;
var eF = H_r.Transpose() * Fi * H_l;
double err = (eF - F).FrobeniusNorm();
Assert.IsTrue(err < 1e-6);
}
public void Test_EstimateCameraMatrix_NoisedLinear()
{
PrepareCameraMatrix();
var points = GenerateCalibrationPoints_Random();
PrepareCalibrator(
AddNoise(points, _varianceReal, _varianceImage));
_calib.HomoPoints();
_calib.NormalizeImagePoints();
_calib.NormalizeRealPoints();
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
_calib.DenormaliseCameraMatrix();
_calib.DecomposeCameraMatrix();
_eCM = _calib.CameraMatrix;
double scaleK = 1.0 / _calib.CameraInternalMatrix[2, 2];
_eCM.MultiplyThis(-scaleK);
var eK = _calib.CameraInternalMatrix.Multiply(scaleK);
var eR = -_calib.CameraRotationMatrix;
var eC = -(_eCM.SubMatrix(0, 3, 0, 3).Inverse() * _eCM.Column(3));
Matrix<double> eExt = new DenseMatrix(3, 4);
eExt.SetSubMatrix(0, 0, eR);
eExt.SetColumn(3, -eR * eC);
var eCM = eK * eExt;
var errVec = _CM.PointwiseDivide_NoNaN(_eCM);
double err = errVec.L2Norm();
Assert.IsTrue(
Math.Abs(err - Math.Sqrt(12)) < Math.Sqrt(12) / 50.0 || // max 2% diffrence
(_eCM - _CM).FrobeniusNorm() < 1e-3);
for(int p = 0; p < _pointsCount; ++p)
{
var cp = points[p];
Vector<double> rp = new DenseVector(4);
rp[0] = cp.RealX;
rp[1] = cp.RealY;
rp[2] = cp.RealZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
Assert.IsTrue((ip - cp.Img).Length() < cp.Img.Length() / 10.0);
}
}
public void Test_EstimateCameraMatrix_Minimised()
{
PrepareCameraMatrix();
var points = GenerateCalibrationPoints_Random(100);
var noisedPoints = AddNoise(points, _varianceReal, _varianceImage, 200);
PrepareCalibrator(noisedPoints);
_calib.HomoPoints();
_calib.NormalizeImagePoints();
_calib.NormalizeRealPoints();
_calib._pointsNormalised = true;
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
// _calib.FindNormalisedVariances();
_calib.DenormaliseCameraMatrix();
_calib._pointsNormalised = false;
_eCM = _calib.CameraMatrix;
double totalDiff = 0.0;
for(int p = 0; p < _pointsCount; ++p)
{
var cp = points[p];
Vector<double> rp = new DenseVector(4);
rp[0] = cp.RealX;
rp[1] = cp.RealY;
rp[2] = cp.RealZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
totalDiff += (ip - cp.Img).Length();
Assert.IsTrue((ip - cp.Img).Length() < 1.0,
"Point after linear estimation too far : " + (ip - cp.Img).Length());
}
_calib.HomoPoints();
_calib.NormalizeImagePoints();
_calib.NormalizeRealPoints();
_calib.CameraMatrix = _calib.FindLinearEstimationOfCameraMatrix();
_calib._pointsNormalised = true;
_calib.FindNormalisedVariances();
_calib.UseCovarianceMatrix = true;
var lecm = _eCM.Clone();
// Disturb camera matrix a little
// _calib.CameraMatrix = AddNoise(_calib.CameraMatrix);
_calib._miniAlg.DoComputeJacobianNumerically = true;
_calib._miniAlg.NumericalDerivativeStep = 1e-4;
_calib.MinimizeError();
// _calib.DenormaliseCameraMatrix();
_calib.DecomposeCameraMatrix();
_eCM = _calib.CameraMatrix;
var errVec = _eCM.PointwiseDivide_NoNaN(lecm);
double err = errVec.L2Norm();
double scaleK = 1.0 / _calib.CameraInternalMatrix[2, 2];
_eCM.MultiplyThis(-scaleK);
var eK = _calib.CameraInternalMatrix.Multiply(scaleK);
var eR = -_calib.CameraRotationMatrix;
var eC = -(_eCM.SubMatrix(0, 3, 0, 3).Inverse() * _eCM.Column(3));
Matrix<double> eExt = new DenseMatrix(3, 4);
eExt.SetSubMatrix(0, 0, eR);
eExt.SetColumn(3, -eR * eC);
var eCM = eK * eExt;
// var errVec = _CM.PointwiseDivide_NoNaN(_eCM);
// double err = errVec.L2Norm();
// Assert.IsTrue(
// Math.Abs(err - Math.Sqrt(12)) < Math.Sqrt(12) / 1000.0 || // max 0.1% diffrence
// (_eCM - _CM).FrobeniusNorm() < 1e-3);
double estDiff = 0;
for(int p = 0; p < _pointsCount; ++p)
{
var cp = points[p];
Vector<double> rp = new DenseVector(4);
rp[0] = cp.RealX;
rp[1] = cp.RealY;
rp[2] = cp.RealZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
Vector2 ip = new Vector2(imagePoint[0] / imagePoint[2], imagePoint[1] / imagePoint[2]);
estDiff += (ip - cp.Img).Length();
Assert.IsTrue((ip - cp.Img).Length() < 1.5,
"Point after error minimalisation too far : " + (ip - cp.Img).Length());
}
var minialg = _calib._miniAlg;
// Test conovergence :
// ||mX-rX|| = ||mX-eX|| + ||rX-eX|| (or squared??)
// rX - real point from 'points'
// mX - measured point, noised
// eX = estimated X from result vector for 3d points and ePeX for image point
double len2_mr = 0;
double len2_me = 0;
double len2_re = 0;
for(int i = 0; i < points.Count; ++i)
{
double rX = points[i].RealX;
double rY = points[i].RealY;
double rZ = points[i].RealZ;
double rx = points[i].ImgX;
double ry = points[i].ImgY;
double mX = noisedPoints[i].RealX;
double mY = noisedPoints[i].RealY;
double mZ = noisedPoints[i].RealZ;
double mx = noisedPoints[i].ImgX;
double my = noisedPoints[i].ImgY;
double eX = minialg.BestResultVector[3 * i + 12];
double eY = minialg.BestResultVector[3 * i + 13];
double eZ = minialg.BestResultVector[3 * i + 14];
Vector<double> rp = new DenseVector(4);
rp[0] = eX;
rp[1] = eY;
rp[2] = eZ;
rp[3] = 1.0;
var imagePoint = _eCM * rp;
double ex = imagePoint[0] / imagePoint[2];
double ey = imagePoint[1] / imagePoint[2];
len2_re += (rX - eX) * (rX - eX);
len2_re += (rY - eY) * (rY - eY);
len2_re += (rZ - eZ) * (rZ - eZ);
len2_re += (rx - ex) * (rx - ex);
len2_re += (ry - ey) * (ry - ey);
len2_me += (mX - eX) * (mX - eX);
len2_me += (mY - eY) * (mY - eY);
len2_me += (mZ - eZ) * (mZ - eZ);
len2_me += (mx - ex) * (mx - ex);
len2_me += (my - ey) * (my - ey);
len2_mr += (rX - mX) * (rX - mX);
len2_mr += (rY - mY) * (rY - mY);
len2_mr += (rZ - mZ) * (rZ - mZ);
len2_mr += (rx - mx) * (rx - mx);
len2_mr += (ry - my) * (ry - my);
}
Assert.IsTrue( Math.Abs(len2_mr - len2_re - len2_me) < len2_mr/100.0 ||
Math.Abs(Math.Sqrt(len2_mr) - Math.Sqrt(len2_re) - Math.Sqrt(len2_me)) < Math.Sqrt(len2_mr) / 100.0,
"Triangle test failed."+" Points distances: LinearDiff = " + totalDiff +
". MiniDiff = " + estDiff);
Assert.IsTrue(estDiff < totalDiff,
"Points after minimalisation are too far. LinearDiff = " + totalDiff +
". MiniDiff = " + estDiff);
}
public void PrepareCameraMatrix()
{
Matrix<double> K = new DenseMatrix(3);
K[0, 0] = 10.0; // fx
K[1, 1] = 10.0; // fy
K[0, 1] = 0.0; // s
K[0, 2] = 0.0; // x0
K[1, 2] = 0.0; // y0
K[2, 2] = 1.0; // 1
Matrix<double> R = DenseMatrix.CreateIdentity(3);
Vector<double> C = new DenseVector(3);
C[0] = 50.0;
C[1] = 50.0;
C[2] = 0.0;
Matrix<double> Ext = new DenseMatrix(3, 4);
Ext.SetSubMatrix(0, 0, R);
Ext.SetColumn(3, -R * C);
_CM = K * Ext;
}
private void optimize(DenseMatrix coefficients, DenseVector objFunValues, bool artifical)
{
//for calculations on the optimal solution row
int cCounter,
width = coefficients.ColumnCount;
DenseVector cBVect = new DenseVector(basics.Count);
//Sets up the b matrix
DenseMatrix b = new DenseMatrix(basics.Count, 1);
//basics will have values greater than coefficients.ColumnCount - 1 if there are still artificial variables
//or if Nathan is bad and didn't get rid of them correctly
foreach (int index in basics)
{
b = (DenseMatrix)b.Append(DenseVector.OfVector(coefficients.Column(index)).ToColumnMatrix());
}
// removes the first column
b = (DenseMatrix)b.SubMatrix(0, b.RowCount, 1, b.ColumnCount - 1);
double[] cPrimes = new double[width];
double[] rhsOverPPrime;
DenseMatrix[] pPrimes = new DenseMatrix[width];
DenseMatrix bInverse;
int newEntering, exitingRow;
bool optimal = false;
if(artifical)
{
rhsOverPPrime = new double[numConstraints + 1];
}
else
{
rhsOverPPrime = new double[numConstraints];
}
while (!optimal)
{
//calculates the inverse of b for this iteration
bInverse = (DenseMatrix)b.Inverse();
//updates the C vector with the most recent basic variables
cCounter = 0;
foreach (int index in basics)
{
cBVect[cCounter++] = objFunValues.At(index);
}
//calculates the pPrimes and cPrimes
for (int i = 0; i < coefficients.ColumnCount; i++)
{
if (!basics.Contains(i))
{
pPrimes[i] = (DenseMatrix)bInverse.Multiply((DenseMatrix)coefficients.Column(i).ToColumnMatrix());
//c' = objFunVals - cB * P'n
//At(0) to turn it into a double
cPrimes[i] = objFunValues.At(i) - (pPrimes[i].LeftMultiply(cBVect)).At(0);
}
else
{
pPrimes[i] = null;
}
}
//RHS'
xPrime = (DenseMatrix)bInverse.Multiply((DenseMatrix)rhsValues.ToColumnMatrix());
//Starts newEntering as the first nonbasic
newEntering = -1;
int iter = 0;
while(newEntering == -1)
{
if(!basics.Contains(iter))
{
newEntering = iter;
}
iter++;
}
//new entering becomes the small cPrime that corresponds to a non-basic value
for (int i = 0; i < cPrimes.Length; i++)
{
if (cPrimes[i] < cPrimes[newEntering] && !basics.Contains(i))
{
newEntering = i;
}
}
//if the smallest cPrime is >= 0, ie they are all positive
if (cPrimes[newEntering] >= 0)
{
optimal = true;
}
else
{
//fix me to deal with if all these values are negative
exitingRow = 0;
for (int i = 0; i < xPrime.RowCount; i++)
{
double[,] pPrime = pPrimes[newEntering].ToArray();
rhsOverPPrime[i] = xPrime.ToArray()[i, 0] / pPrime[i, 0];
if (rhsOverPPrime[i] < rhsOverPPrime[exitingRow] && rhsOverPPrime[i] > 0 )
{
exitingRow = i;
}
}
//translates from the index in the basics list to the actual row
exitingRow = basics[exitingRow];
//make sure you're not being stupid here!!!!
int tempIndex = basics.IndexOf(exitingRow);
basics.Remove(exitingRow);
basics.Insert(tempIndex, newEntering);
b.SetColumn(basics.IndexOf(newEntering), coefficients.Column(newEntering));
}
}
}
private bool GomoriIteration()
{
// Шаг 1
_writer.WriteLine("Iteration: {0}", iterationNumber);
var simplexMethod = new SimplexMethod(_writer);
simplexMethod.Solve(_task); // Решаем задачу симплекс методом
_writer.WriteLine("Optimal plan is found: {0}", _task.xo);
_writer.WriteLine("Target function value = {0}", _task.c * _task.xo);
// Шаг 2
//var artJToRemoveRow = -1;
//var artJToRemoveColumn = -1;
//artJToRemoveRow = -1;
//artJToRemoveColumn = -1;
//foreach (var artJ in _artJ)
//{
// if (_task.Jb.Contains(artJ.Column))
// {
// var rowToRemove = artJ.Row; // TODO probably need to rewrite row selection
// var ai = _task.A.Row(rowToRemove); // Выбираем строку с искусственым ограничением
// ai = -ai / ai[artJ.Column];
// var rowList = ai.ToList();
// rowList.RemoveAt(artJ.Column);
// ai = DenseVector.OfEnumerable(rowList);
// var aj = _task.A.Column(artJ.Column); // Выбираем столбец с искусственным ограничением
// var columnList = aj.ToList();
// var bCoef = _task.b[rowToRemove] / columnList[rowToRemove];
// columnList.RemoveAt(rowToRemove);
// aj = DenseVector.OfEnumerable(columnList);
// var newA = DenseMatrix.Create(_task.A.RowCount - 1, _task.A.ColumnCount - 1,
// (i, j) => _task.A[i < rowToRemove ? i : i + 1, j < artJ.Column ? j : j + 1]);
// newA += DenseMatrix.OfMatrix(aj.ToColumnMatrix() * ai.ToRowMatrix()); // Удаляем искусственные строку
// _task.A = newA; // и столбец из матрицы А
// _task.b = DenseVector.Create(_task.b.Count - 1, i => i < rowToRemove ? _task.b[i] : _task.b[i + 1]);
// _task.b += bCoef * aj;
// _task.c = DenseVector.Create(_task.c.Count - 1, i => i < artJ.Column ? _task.c[i] : _task.c[i + 1]); // Удаляем искусственную переменную из вектора с
// _task.xo = DenseVector.Create(_task.xo.Count - 1, i => i < artJ.Column ? _task.xo[i] : _task.xo[i + 1]); // Удаляем искусственную переменную из xo
// _task.Jb.Remove(artJ.Column);
// artJToRemoveColumn = artJ.Column;
// artJToRemoveRow = artJ.Row;
// break;
// }
//}
//if (artJToRemoveRow > 0) // Удаляем искусственную переменную из базисных
//{
// _artJ.RemoveAll(x => x.Row == artJToRemoveRow);
// for (int i = 0; i < _artJ.Count; i++)
// {
// if (_artJ[i].Row > artJToRemoveRow)
// {
// _artJ[i].Row--; // Сдвигаем индексы базисных переменных на один
// _artJ[i].Column--; // После удаления искусственной переменной
// }
// }
// for (int i = 0; i < _task.Jb.Count; i++)
// {
// _task.Jb[i] = _task.Jb[i] > artJToRemoveColumn ? _task.Jb[i] - 1 : _task.Jb[i];
// }
//}
// Шаг 3
var falseIndex = -1;
var maxFract = 0d;
for (int i = 0; i < _task.xo.Count(); i++)
{
if (Math.Abs(Math.Round(_task.xo[i]) - _task.xo[i]) > Eps)
{
var fract = Math.Abs(_task.xo[i] - Math.Floor(_task.xo[i])); // Находим базисную переменную
if (_task.Jb.Contains(i) && fract > Eps) // С максимальной дробной частью
{ // и запоминаем ее индекс
if (fract > maxFract)
{
maxFract = fract;
falseIndex = i;
}
}
}
}
if (falseIndex < 0) // Если все переменные целые - решение найдено
{
return false; // Прерываем выполнение метода
}
_writer.WriteLine("Jk = {0}", falseIndex);
// Шаг 4
var aB = new DenseMatrix(_task.Jb.Count());
int index = 0;
foreach (var j in _task.Jb)
{
aB.SetColumn(index, _task.A.Column(j)); // Формируем матрицу Ab из базисных столбцов А
index++;
}
_writer.Write("Jb: ");
_task.Jb.ForEach(x => _writer.Write("{0} ", x));
_writer.WriteLine();
_writer.WriteLine("Basis matrix: {0}", aB);
var y = DenseMatrix.Identity(_task.A.RowCount).Column(_task.Jb.IndexOf(falseIndex)) * aB.Inverse(); //Находим e'*Ab
var newRow = new DenseVector(_task.A.ColumnCount + 1);
newRow.SetSubVector(0, _task.A.ColumnCount, y * _task.A); // Находим данные для нового отсекающего ограничения
_writer.WriteLine("Data for new limitation: {0}", newRow);
for (int i = 0; i < newRow.Count; i++) // Формируем новое отсекающее ограничение
{
if (i < _task.A.ColumnCount)
{
if (Math.Abs(newRow[i]) < Eps)
{
newRow[i] = 0;
}
else
{
newRow[i] = newRow[i] > 0
? -(newRow[i] - Math.Floor(newRow[i]))
: -(Math.Ceiling(Math.Abs(newRow[i])) - Math.Abs(newRow[i]));
}
}
else
{
newRow[i] = 1;
}
}
newRow[falseIndex] = 0;
_writer.WriteLine("New limitation: {0}", newRow);
var newb = (y * _task.b); // Находим новый элемент вектора b
newb = newb > 0 ? -(newb - Math.Floor(newb)) : -(Math.Ceiling(Math.Abs(newb)) - Math.Abs(newb)); // TODO probably need to rewrite this
_writer.WriteLine("New b = {0}", newb);
// Шаг 5
var newMatrix = new DenseMatrix(_task.A.RowCount + 1, _task.A.ColumnCount + 1); // Формируем новую
newMatrix.SetSubMatrix(0, _task.A.RowCount, 0, _task.A.ColumnCount, _task.A); // матрицу А
newMatrix.SetRow(_task.A.RowCount, newRow);
newMatrix[_task.A.RowCount, _task.A.ColumnCount] = 1;
var newBVector = new DenseVector(_task.b.Count + 1); // Формируем новый
newBVector.SetSubVector(0, _task.b.Count, _task.b); // вектор b
newBVector[_task.b.Count] = newb;
var newCVector = new DenseVector(_task.c.Count + 1); // Добавляем новую
newCVector.SetSubVector(0, _task.c.Count, _task.c); // компоненту вектора с
var newJb = _task.Jb.ToList();
newJb.Add(newJb[newJb.Count - 1] + 1);
_artJ.Add(new ArtJEntry { Column = newMatrix.ColumnCount - 1, Row = newMatrix.RowCount - 1 });
_task.A = newMatrix.Clone(); // Создаем
_task.b = newBVector.Clone(); // новую задачу
_task.c = newCVector.Clone(); // для следующей итерации
_task.Jb = newJb;
iterationNumber++; // Присваиваем новый номер итерации
return true;
}
private bool Iterate()
{
// Step 1
var cb = new DenseVector(_task.Jb.Count());
for (int i = 0; i < _task.Jb.Count(); i++)
{
cb[i] = _task.c[_task.Jb[i]];
}
Matrix<double> bMatrix = new DenseMatrix(_task.Jb.Count());
for (int i = 0; i < bMatrix.ColumnCount; i++)
{
bMatrix.SetColumn(i, _task.A.Column(_task.Jb[i]));
}
bMatrix = bMatrix.Inverse();
var u = cb * bMatrix;
var deltas = DenseVector.Create(
_task.c.Count, i => _task.Jb.Contains(i) ? double.PositiveInfinity : u * _task.A.Column(i) - _task.c[i]);
//Step 2
if (deltas.ToList().TrueForAll(x => x > 0 || Math.Abs(x) < Eps))
{
// STOP vector is optimal
//_writer.WriteLine("Optimal plan is found: {0}", _task.xo);
//_writer.WriteLine("Target function value = {0}", _task.c * _task.xo);
return false;
}
// Step 3
int j0 = 0;
for (int j = 0; j < deltas.Count; j++)
{
if (!_task.Jb.Contains(j) && deltas[j] < 0 && Math.Abs(deltas[j]) >= Eps)
{
j0 = j;
break;
}
}
var z = bMatrix * _task.A.Column(j0);
if (z.ToList().TrueForAll(x => x < -Eps || Math.Abs(x) < Eps ))
{
// STOP target function is unlimited
_writer.WriteLine("Target function is unlimited");
return false;
}
// Step 4
var tetta0 = double.PositiveInfinity;
var s = -1;
for (int i = 0; i < z.Count; i++)
{
if (z[i] > 0 && Math.Abs(z[i]) > Eps)
{
var tetta = _task.xo[_task.Jb[i]]/z[i];
if(double.IsPositiveInfinity(tetta0) || (Math.Abs(tetta0 - tetta) > Eps && tetta < tetta0))
{
tetta0 = tetta;
s = i;
}
}
}
// Step 5
var newXo = DenseVector.Create(_task.xo.Count, i => 0);
for (int i = 0; i < _task.Jb.Count(); i++)
{
newXo[_task.Jb[i]] = _task.xo[_task.Jb[i]] - tetta0 * z[i];
}
newXo[j0] = tetta0;
_task.xo = newXo;
_task.Jb[s] = j0;
// Step 6 is unneccesary
return true;
}
/*
* Layer 4: weighted Layer.
*
*/
public DenseMatrix CalculateGreatPsi(DenseMatrix X, DenseMatrix Psi)
{
int N = Psi.RowCount;
int U = Psi.ColumnCount;
int R = X.RowCount; //the number of inputs per observation
var GreatPsi = new DenseMatrix(U*(R + 1), N);
//foreach observation
for (int i = 0; i < N; i++)
{
var x = new DenseVector(R);
X.Column(i, x);
var GreatPsiCol = new DenseVector(U*(R + 1));
//foreach neuron
for (int j = 0; j < U; j++)
{
var temp = new DenseVector(x.Count + 1, 1);
temp.SetSubVector(1, x.Count, x);
GreatPsiCol.SetSubVector(j*(temp.Count), temp.Count, Psi[i, j]*temp);
}
GreatPsi.SetColumn(i, GreatPsiCol);
}
return GreatPsi;
}
private void disp_button_Click(object sender, EventArgs e)
{
/*
Normal N = new Normal(0.0, 1.0);
DenseVector VALS = new DenseVector(20, 0);
for (int i = -10; i < 10; i++)
{
VALS[i + 10] = N.Density(i);
Console.WriteLine(VALS[i + 10]);
}
*/
/*
//double[] d = new double[]{0,0};
DenseMatrix mu = new DenseMatrix(2, 1, new[] { 0.0, 0.0 });
DenseMatrix K = new DenseMatrix(1, 1, new[] { 1.0 });
DenseMatrix V = new DenseMatrix(2,2, new[] {4.0, 0.0, 0.0, 1.0 });
MatrixNormal mN = new MatrixNormal(mu, V, K);
Console.WriteLine(mu.RowCount);
Console.WriteLine(mu.ColumnCount);
Console.WriteLine(mN.Sample());
Console.WriteLine(mN.Sample());
DenseMatrix sample_pt = new DenseMatrix(2, 1, new[] { 0.0, 0.1 });
double pt = mN.Density(sample_pt);
*/
int M = 150;
int N = 200;
Normal N_x = new Normal(N/2, 50);
Normal N_y = new Normal(M/2, 30);
DenseMatrix M_x = new DenseMatrix(M, N, 0.0);
DenseMatrix M_y = new DenseMatrix(M, N, 0.0);
DenseVector V_x = new DenseVector(N);
for (int i = 0; i < N; i++)
{
V_x[i] = N_x.Density(i);
}
for (int j = 0; j < M; j++)
{
M_x.SetRow(j, V_x);
}
DenseVector V_y = new DenseVector(M);
for (int i = 0; i < M; i++)
{
V_y[i] = N_y.Density(i);
}
for (int j = 0; j < N; j++)
{
M_y.SetColumn(j, V_y);
}
DenseMatrix MULT = (DenseMatrix)M_x.PointwiseMultiply(M_y);
double s = MULT.Data.Sum();
MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(M,N,s));
//this.dataGridView1.DataSource = MULT;
//Console.WriteLine(MULT.Data.Sum());
s = MULT[M / 2, N / 2];
MULT = (DenseMatrix)MULT.PointwiseDivide(new DenseMatrix(M, N, s));
/*
for (int i = 0; i < M; i++)
{
Console.Write(i + " - ");
for (int j = 0; j < N; j++)
{
Console.Write(MULT[i, j] + " ");
}
Console.WriteLine();
Console.WriteLine();
}
*/
Console.WriteLine(Environment.ProcessorCount);
}
private void CalcBaseAandC()
{
BaseA = new DenseMatrix(CurrBaseJ.Count);
BaseC = new DenseVector(CurrBaseJ.Count);
int i = 0;
foreach (var j in CurrBaseJ)
{
BaseA.SetColumn(i, A.Column(j));
BaseC[i] = c[j];
i++;
}
InvBaseA = (DenseMatrix)BaseA.Inverse();
CurrNonBaseJ = new List<int>(Enumerable.Range(0, A.ColumnCount).Except(CurrBaseJ));
}
