private static void Initialize(MnMatrix graph, double[] verticesXLabel, double[] verticesYLabel)
{
_maximumEdge = 0;
for (int i = 0; i < verticesXLabel.Length; i++)
{
double max = 0;
for (int j = 0; j < verticesYLabel.Length; j++)
{
if (graph.At(i, j) > max)
{
max = graph.At(i, j);
}
}
verticesXLabel[i] = max;
if (max > _maximumEdge)
{
_maximumEdge = max;
}
}
for (int j = 0; j < verticesYLabel.Length; j++)
{
verticesYLabel[j] = 0;
}
}
/// <summary>
/// The function creates a new population of strings, which rises after
/// 1- to 4-point crossover operation of all (couples of) strings of the old
/// population. The selection of strings into couples is either random or
/// the neighbouring strings are selected, depending on the parameter sel.
/// </summary>
/// <param name="Oldpop">old population</param>
/// <param name="pts">the number of crossover points from 1 to 4</param>
/// <param name="sel">type of the string-couple selection:
/// 0 - random
/// 1 - neighbouring strings in the population</param>
/// <returns>Newpop - new population</returns>
public matica crossov(matica OldPop, int pts, int sel)
{
int i = 0, j=0;
Random rand = new Random();
matica NewPop = OldPop;
int lstring = OldPop.RowCount;
int lpop=OldPop.ColumnCount;
int num;
int[] flag = new int[lpop]; //vytvorim pole flagov ci bol retazec z populacie vybrany
num = Convert.ToInt32(Math.Truncate((double)lpop / 2.0));
for (int cyk = 0; cyk < num; cyk++)
{
if (sel == 0)
{
while (flag[i] != 0) //najde prveho s flagom 0, co este nebol vybrany (index i)
{
i += 1;
}
flag[i] = 1;
j = Convert.ToInt32(Math.Ceiling((double)lpop * rand.NextDouble())); //nahodne najde retazec s flagom 0, co este nebol vybrany (index j)
while (flag[j] != 0)
{
j = Convert.ToInt32(Math.Ceiling((double)lpop * rand.NextDouble()));
}
flag[j] = 2;
}
else if (sel == 1)
{
i = 2 * cyk - 1;
j = i + 1;
}
if (pts > 4)
pts = 4;
int[] v=new int[pts];
double n = lstring * (1 - (pts - 1) * 0.15);
int p = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * n));
if (p == lstring)
{
p = lstring - 1;
}
v[0] = p;
for (int k = 0; k < pts - 1; k++)
{
int h = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * n));
if (h == 1)
h = 2;
p = p + h;
if (p >= lstring)
break;
v[k + 1] = p; //zapis bodu krizenia do pola
}
//krizenie
NewPop = skriz(OldPop, NewPop, v, lstring, i, j);
}
return NewPop;
}
public FedKF(KF[] filters, DenseMatrix dcm, DenseMatrix covariances)
{
_filters = filters;
_filteredSignals = new DenseMatrix(_filters.Length, 1);
_dcm = dcm;
_dcmt = dcm.Transpose();
_cInv = covariances.Inverse();
}
public KF(SSF model, DenseMatrix measCov, DenseMatrix procCov)
{
_model = model;
_p = new DenseMatrix(_model.A.ColumnCount);
_mCov = measCov;
_pCov = procCov;
_e = DenseMatrix.Identity(_model.A.ColumnCount);
_at = new DenseMatrix(_model.A.Transpose().ToArray());
_ct = new DenseMatrix(_model.C.Transpose().ToArray());
}
public Matrix<double> Step(DenseMatrix measurement, DenseMatrix input)
{
var pProp = _model.A * _p * _at + _pCov;
var k = pProp * _ct * (_model.C * pProp * _ct + _mCov).Inverse();
var ekc = _e - k * _model.C;
_p = ekc * pProp;
var xProp = _model.A * _model.State; // +_model.B * input;
var x = xProp + k * (measurement - _model.C * xProp);
x.CopyTo(_model.State);
return _model.C * x;
}
private void LogMatrix(Matrix preparedMatrix)
{
Console.WriteLine();
foreach (var row in preparedMatrix.EnumerateRows())
{
foreach (var elem in row)
{
Console.Write($"{elem} ");
}
Console.WriteLine();
}
}
public void TestInitialize()
{
//dx Set up a test problem
g = DenseMatrix.Identity(n);
for (int jj = 0; jj <5; jj++)
{
Vector gg = Normal(size : n);
var hh = gg.PointwiseMultiply(gg);
g.AddRowVector(hh, g);
g.AddRowVector(Normal(size : n).PointwiseMultiply( Normal(size : n)), g);
}
}
/// <summary>
///
/// </summary>
/// <param name="inputsCount">Number of inputs to each vector</param>
/// <param name="outputsCount">Same as neuron count in this layer</param>
/// <param name="activationFunction"></param>
/// <param name="initialWeightsRange"></param>
public HiddenLayer(
int inputsCount,
int outputsCount,
ActivationFunction activationFunction,
double initialWeightsRange
)
{
CurrentActivationFunction = activationFunction;
InputsCount = inputsCount;
NeuronsCount = outputsCount;
InitialWeightsRange = initialWeightsRange;
Weights = GetNewWeightsMatrix(false);
Biases = GetNewBiasesVector(false);
}
private static DenseMatrix CreateEqualityGraph(MnMatrix graph, double[] verticesXLabel, double[] verticesYLabel)
{
DenseMatrix subGraph = new DenseMatrix(verticesXLabel.Length, verticesYLabel.Length);
for (int i = 0; i < verticesXLabel.Length; i++)
{
for (int j = 0; j < verticesYLabel.Length; j++)
{
if (Math.Abs(graph.At(i, j) - (verticesXLabel[i] + verticesYLabel[j])) < EPSILON)
{
subGraph.At(i, j, 1);
}
}
}
return(subGraph);
}
/// <summary>
/// Copy the specified input pattern to the weight matrix. This causes an
/// output neuron to learn this pattern "exactly". This is useful when a
/// winner is to be forced.
/// </summary>
/// <param name="matrix">The matrix that is the target of the copy.</param>
/// <param name="outputNeuron">The output neuron to set.</param>
/// <param name="input">The input pattern to copy.</param>
private void CopyInputPattern(Matrix matrix, int outputNeuron,
double[] input)
{
for (var inputNeuron = 0; inputNeuron < _inputNeuronCount; inputNeuron++)
{
matrix[outputNeuron, inputNeuron] = input[inputNeuron];
}
}
/// <summary>
/// Pointwise raise this matrix to an exponent and store the result into the result matrix.
/// </summary>
/// <param name="exponent">The exponent to raise this matrix values to.</param>
/// <param name="result">The vector to store the result of the pointwise power.</param>
protected override void DoPointwisePower(Matrix<double> exponent, Matrix<double> result)
{
Map2(Math.Pow, result, Zeros.Include);
}
/// <summary>
/// Pointwise remainder (% operator), where the result has the sign of the dividend,
/// of this matrix with another matrix and stores the result into the result matrix.
/// </summary>
/// <param name="divisor">The pointwise denominator matrix to use</param>
/// <param name="result">The result of the modulus.</param>
protected override void DoPointwiseRemainder(Matrix<double> divisor, Matrix<double> result)
{
Map2(Euclid.Remainder, divisor, result, Zeros.Include);
}
/// <summary>
/// The function mutates the population of strings with the intensity
/// proportional to the parameter rate from interval <0;1>. Only a few genes
/// from a few strings are mutated in the population. The mutated values are
/// selected from the bounded real-number space, which is defined by the two-row
/// matrix Space. The first row of the matrix represents the lower boundaries and the
/// second row represents the upper boundaries of corresponding genes in the strings.
/// </summary>
/// <param name="Oldpop">old population</param>
/// <param name="Amps">vector of absolute values of real-number boundaries</param>
/// <param name="Space">matrix of boundaries in the form: [vector of lower limits of genes;
/// vector of upper limits of genes];</param>
/// <param name="factor">mutation intensity, 0 =< rate =< 1</param>
/// <returns> Newpop - new mutated population</returns>
public matica mutx(matica OldPop, double factor, matica Space)
{
Random rand = new Random();
int lpop = OldPop.ColumnCount;
int lstring = OldPop.RowCount;
if (factor > 1)
factor = 1.0;
if (factor < 0)
factor = 0.0;
int n = Convert.ToInt32(Math.Ceiling(lpop * lstring * factor * rand.NextDouble())); //nahodne generujem pocet mutovanych genov podla miery mutacie (factor)
matica NewPop = OldPop;
for (int i = 0; i < n; i++)
{
int r = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * lpop));
int s = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * lstring));
double d = Space[2, s] - Space[1, s];
NewPop[r, s] = rand.NextDouble() * d + Space[1, s]; //dany gen nahradim novym z def. intervalu
if (NewPop[r, s] < Space[1, s])
NewPop[r, s] = Space[1, s];
if (NewPop[r, s] > Space[2, s])
NewPop[r, s] = Space[2, s];
}
}
/// <summary>
/// The function generates a population of random real-coded strings
/// which items (genes) are limited by a two-row matrix Space. The first
/// row of the matrix Space consists of the lower limits and the second row
/// consists of the upper limits of the possible values of genes.
/// The length of the string is equal to the length of rows of the matrix Space.
/// </summary>
/// <param name="popSize">required number of strings in the population</param>
/// <param name="Space">2-row matrix, which 1-st row is the vector of the lower limits
/// and the 2-nd row is the vector of the upper limits of the
/// gene values.</param>
/// <returns> Newpop - random generated population</returns>
public matica genrPop(int popSize, matica Space)
{
Random random = new Random();
int lstring = Space.RowCount;
matica NewPop = new DenseMatrix(popSize, lstring);
for (int i = 0; i < popSize; i++)
{
for (int j = 0; j < lstring; j++)
{
double d = Space[2, j] - Space[1, j];
NewPop[i, j] = random.NextDouble() * d + Space[1, j];
//podmienky na ohranicenie hodnoty genu
if (NewPop[i, j] < Space[1, j])
NewPop[i, j] = Space[1, j];
if (NewPop[i, j] > Space[2, j])
NewPop[i, j] = Space[2, j];
}
}
return NewPop;
}
protected override void DoPointwiseCosh(Matrix<double> result)
{
Map(Math.Cosh, result, Zeros.Include);
}
protected override void DoPointwiseTanh(Matrix<double> result)
{
Map(Math.Tanh, result, Zeros.AllowSkip);
}
protected override void DoPointwiseMaximum(double scalar, Matrix<double> result)
{
Map(x => Math.Max(scalar, x), result, scalar <= 0d ? Zeros.AllowSkip : Zeros.Include);
}
protected override void DoPointwiseSign(Matrix<double> result)
{
Map(x => (double)Math.Sign(x), result, Zeros.AllowSkip);
}
protected override void DoPointwiseSqrt(Matrix<double> result)
{
Map(Math.Sqrt, result, Zeros.AllowSkip);
}
protected override void DoPointwiseRound(Matrix<double> result)
{
Map(Math.Round, result, Zeros.AllowSkip);
}
protected override void DoPointwiseLog10(Matrix<double> result)
{
Map(Math.Log10, result, Zeros.Include);
}
protected override void DoPointwiseFloor(Matrix<double> result)
{
Map(Math.Floor, result, Zeros.AllowSkip);
}
/// <summary>
/// Train for the specified synapse and BMU.
/// </summary>
/// <param name="bmu">The best matching unit for this input.</param>
/// <param name="matrix">The synapse to train.</param>
/// <param name="input">The input to train for.</param>
private void Train(int bmu, Matrix matrix, double[] input)
{
// adjust the weight for the BMU and its neighborhood
for (var outputNeuron = 0; outputNeuron < _outputNeuronCount; outputNeuron++)
{
TrainPattern(matrix, input, outputNeuron, bmu);
}
}
protected override void DoPointwiseAbsoluteMinimum(double scalar, Matrix<double> result)
{
double absolute = Math.Abs(scalar);
Map(x => Math.Min(absolute, Math.Abs(x)), result, Zeros.AllowSkip);
}
/// <summary>
/// The constructor.
/// </summary>
/// <param name="inputCount">Number of input neurons</param>
/// <param name="outputCount">Number of output neurons</param>
public SelfOrganizingMap(int inputCount, int outputCount)
{
_weights = DenseMatrix.Create(outputCount, inputCount, 0);
}
protected override void DoPointwiseAbsoluteMaximum(double scalar, Matrix<double> result)
{
double absolute = Math.Abs(scalar);
Map(x => Math.Max(absolute, Math.Abs(x)), result, Zeros.Include);
}
protected override void DoPointwiseAcos(Matrix<double> result)
{
Map(Math.Acos, result, Zeros.Include);
}
protected override void DoPointwiseMaximum(Matrix<double> other, Matrix<double> result)
{
Map2(Math.Max, other, result, Zeros.AllowSkip);
}
public static void RunAlgorithm(MnMatrix graph, int[] copulationVerticesX, int[] copulationVerticesY)
{
double min = graph.ToColumnWiseArray().Min();
if (min < 0)
{
for (int i = 0; i < graph.ColumnCount; i++)
{
for (int j = 0; j < graph.RowCount; j++)
{
graph[j, i] = graph[j, i] - i + 1;
}
}
}
int limit = 0;
if (copulationVerticesX.Length > copulationVerticesY.Length)
{
limit = copulationVerticesX.Length - copulationVerticesY.Length;
}
double[] verticesXLabel = new double[copulationVerticesX.Length];
double[] verticesYLabel = new double[copulationVerticesY.Length];
Initialize(graph, verticesXLabel, verticesYLabel);
while (true)
{
for (int i = 0; i < copulationVerticesX.Length; i++)
{
copulationVerticesX[i] = -1;
}
for (int i = 0; i < copulationVerticesY.Length; i++)
{
copulationVerticesY[i] = -1;
}
DenseMatrix equalityGraph = CreateEqualityGraph(graph, verticesXLabel, verticesYLabel);
int[,] vertices;
HungarianAlgorithm.RunAlgorithm(equalityGraph, copulationVerticesX, copulationVerticesY, out vertices);
if (CountCopulation(copulationVerticesX) == limit)
{
return;
}
//if (CountCopulation(copulationVerticesY) == 0)
//{
// return;
//}
var F1UF2 = new List <int>();
var L2 = new List <int>();
var L1UL3 = new List <int>();
for (int i = 0; i < copulationVerticesX.Length; i++)
{
if (vertices[0, i] == 0)
{
F1UF2.Add(i);
}
}
for (int i = 0; i < copulationVerticesY.Length; i++)
{
if (vertices[1, i] == 0)
{
L2.Add(i);
}
else
{
L1UL3.Add(i);
}
}
double minDelta = _maximumEdge + 1;
foreach (var i in F1UF2)
{
foreach (var j in L1UL3)
{
// if (graph[i, j] == -1) continue;
double delta = verticesXLabel[i] + verticesYLabel[j] - graph.At(i, j);
if (delta < minDelta)
{
minDelta = delta;
}
}
}
foreach (var i in F1UF2)
{
verticesXLabel[i] -= minDelta;
}
foreach (var i in L2)
{
verticesYLabel[i] += minDelta;
}
}
}
protected override void DoPointwiseAbsoluteMaximum(Matrix<double> other, Matrix<double> result)
{
Map2((x, y) => Math.Max(Math.Abs(x), Math.Abs(y)), other, result, Zeros.AllowSkip);
}
/// <summary>
/// The function mutates the population of strings with the intensity
/// proportional to the parameter rate from interval <0;1>. Only a few genes
/// from a few strings are mutated in the population. The mutations are realized
/// by addition or substraction of random real-numbers to the mutated genes. The
/// absolute values of the added constants are limited by the vector Amp.
/// Next the mutated strings are limited using boundaries defined in
/// a two-row matrix Space. The first row of the matrix represents the lower
/// boundaries and the second row represents the upper boundaries of corresponding
/// genes.
/// </summary>
/// <param name="Oldpop">old population</param>
/// <param name="Amps">vector of absolute values of real-number boundaries</param>
/// <param name="Space">matrix of gene boundaries in the form:
/// [real-number vector of lower limits of genes
/// real-number vector of upper limits of genes];</param>
/// <param name="factor">mutation intensity, 0 =< rate =< 1</param>
/// <returns> Newpop - new, mutated population</returns>
public matica muta(matica OldPop, double factor, double[] Amps, matica Space)
{
Random rand = new Random();
int lpop = OldPop.ColumnCount;
int lstring = OldPop.RowCount;
if (factor > 1)
factor = 1.0;
if (factor < 0)
factor = 0.0;
int n = Convert.ToInt32(Math.Ceiling(lpop * lstring * factor * rand.NextDouble())); //nahodne generujem pocet mutovanych genov podla miery mutacie (factor)
matica NewPop = OldPop;
for (int i = 0; i < n; i++) //cyklus na pocet mutacii
{
int r = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * lpop));
int s = Convert.ToInt32(Math.Ceiling(rand.NextDouble() * lstring));
NewPop[r, s] = OldPop[r, s] + (2 * rand.NextDouble() - 1) * Amps[s]; //k danemu genu pripocitam nahodne cislo z def. intervalu
if (NewPop[r, s] < Space[1, s])
NewPop[r, s] = Space[1, s];
if (NewPop[r, s] > Space[2, s])
NewPop[r, s] = Space[2, s];
}
return NewPop;
}
/// <summary>
/// Puts the conjugate transpose of this matrix into the result matrix.
/// </summary>
public sealed override void ConjugateTranspose(Matrix<double> result)
{
Transpose(result);
}
//skrizi jedincov
private matica skriz(matica OldPop, matica NewPop, int[] v, int lstring, int i, int j)
{
List<double[]> listPrvkov1 = new List<double[]>();
List<double[]> listPrvkov2 = new List<double[]>();
int[] newV = new int[v.Length + 2];
v.CopyTo(newV,1);
newV[0]=0;
newV[newV.Length-1]=lstring;
for (int a = 0; a < newV.Length - 1; a++)
{
if (a % 2 == 0)
{
listPrvkov1.Add(OldPop.Row(i, v[a], v[a + 1]).ToArray());
listPrvkov2.Add(OldPop.Row(j, v[a], v[a + 1]).ToArray());
}
else
{
listPrvkov1.Add(OldPop.Row(j, v[a], v[a + 1]).ToArray());
listPrvkov2.Add(OldPop.Row(i, v[a], v[a + 1]).ToArray());
}
}
double[] row = spoj(listPrvkov1);
NewPop.SetRow(i, row);
row = spoj(listPrvkov2);
NewPop.SetRow(j, row);
return NewPop;
}
protected override void DoPointwiseAtan2(Matrix<double> other, Matrix<double> result)
{
Map2(Math.Atan2, other, result, Zeros.Include);
}
/// <summary>
/// Pointwise canonical modulus, where the result has the sign of the divisor,
/// of this matrix with another matrix and stores the result into the result matrix.
/// </summary>
/// <param name="divisor">The pointwise denominator matrix to use</param>
/// <param name="result">The result of the modulus.</param>
protected override void DoPointwiseModulus(Matrix<double> divisor, Matrix<double> result)
{
Map2(Euclid.Modulus, divisor, result, Zeros.Include);
}
/// <summary>
/// Pointwise raise this matrix to an exponent and store the result into the result matrix.
/// </summary>
/// <param name="exponent">The exponent to raise this matrix values to.</param>
/// <param name="result">The matrix to store the result of the pointwise power.</param>
protected override void DoPointwisePower(double exponent, Matrix<double> result)
{
Map(x => Math.Pow(x, exponent), result, exponent > 0.0 ? Zeros.AllowSkip : Zeros.Include);
}
/// <summary>
/// Create an instance of competitive training.
/// </summary>
/// <param name="network">The network to train.</param>
/// <param name="learningRate">The learning rate, how much to apply per iteration.</param>
/// <param name="training">The training set (unsupervised).</param>
/// <param name="neighborhood">The neighborhood function to use.</param>
public BasicTrainSOM(SelfOrganizingMap network, double learningRate,
IList<BasicData> training, INeighborhoodFunction neighborhood)
{
_neighborhood = neighborhood;
_training = training;
LearningRate = learningRate;
_network = network;
_inputNeuronCount = network.InputCount;
_outputNeuronCount = network.OutputCount;
ForceWinner = false;
_error = 0;
// setup the correction matrix
_correctionMatrix = DenseMatrix.Create(_outputNeuronCount, _inputNeuronCount, 0);
// create the BMU class
_bmuUtil = new BestMatchingUnit(network);
}
protected override void DoPointwiseAtan(Matrix<double> result)
{
Map(Math.Atan, result, Zeros.AllowSkip);
}
/// <summary>
/// Force any neurons that did not win to off-load patterns from overworked
/// neurons.
/// </summary>
/// <param name="matrix">The synapse to modify.</param>
/// <param name="won">An array that specifies how many times each output neuron has "won".</param>
/// <param name="leastRepresented">The training pattern that is the least represented by this neural network.</param>
/// <returns>True if a winner was forced.</returns>
private bool ForceWinners(Matrix matrix, int[] won, double[] leastRepresented)
{
var maxActivation = double.NegativeInfinity;
var maxActivationNeuron = -1;
var output = Compute(_network, leastRepresented);
// Loop over all of the output neurons. Consider any neurons that were
// not the BMU (winner) for any pattern. Track which of these
// non-winning neurons had the highest activation.
for (var outputNeuron = 0; outputNeuron < won.Length; outputNeuron++)
{
// Only consider neurons that did not "win".
if (won[outputNeuron] == 0)
{
if ((maxActivationNeuron == -1)
|| (output[outputNeuron] > maxActivation))
{
maxActivation = output[outputNeuron];
maxActivationNeuron = outputNeuron;
}
}
}
// If a neurons was found that did not activate for any patterns, then
// force it to "win" the least represented pattern.
if (maxActivationNeuron != -1)
{
CopyInputPattern(matrix, maxActivationNeuron, leastRepresented);
return true;
}
return false;
}
private void GetPointcloud(Image colorImage, Image depth, Image xyzImage, robotState state)
{
flag = false;
times++; //make sure this program has been processed once
//give out the tcp value and the tranformation matrix
if (state.cartesianInfo != null)
{
tcp = new Vector6 <double>();
ROT = new rot();
tcp = state.cartesianInfo.tcp;
ROT.Rx = state.cartesianInfo.tcp.Rx;
ROT.Ry = state.cartesianInfo.tcp.Ry;
ROT.Rz = state.cartesianInfo.tcp.Rz;
matrix_R = RobMath.RotVec2Matrix(ROT);
matrix_T_tcp2base.SetSubMatrix(0, 3, 0, 3, matrix_R);
matrix_T_tcp2base[3, 0] = 0;
matrix_T_tcp2base[3, 1] = 0;
matrix_T_tcp2base[3, 2] = 0;
matrix_T_tcp2base[3, 3] = 1;
matrix_T_tcp2base[0, 3] = tcp.X;
matrix_T_tcp2base[1, 3] = tcp.Y;
matrix_T_tcp2base[2, 3] = tcp.Z;
matrix_T = (DenseMatrix)(matrix_T_tcp2base * matrix_T_tool2tcp);
//info.Text = tcp.ToString();
//info.Text = matrix_T_tcp2base.ToString();
BGRA[] colorArray = colorImage.GetPixels <BGRA>().ToArray();
Short3[] xyzArray = xyzImage.GetPixels <Short3>().ToArray();
MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P1 = new DenseMatrix(4, 1);
MathNet.Numerics.LinearAlgebra.Double.Matrix matrix_P0 = new DenseMatrix(4, 1);
Vector3[] vertices = new Vector3[num];
Color32[] colors = new Color32[num];
//indices = new int[num];
//描画する点の配列番号を記録。(全ての点を描画)
//for (int i = 0; i < num; i++)
//{
// indices[i] = i;
//}
for (int i = 0; i < num; i++)
{
//頂点座標の代入
vertices[i].x = xyzArray[i].X * 0.001f;
vertices[i].y = xyzArray[i].Y * 0.001f;
vertices[i].z = xyzArray[i].Z * 0.001f;
//色の代入
colors[i].b = colorArray[i].B;
colors[i].g = colorArray[i].G;
colors[i].r = colorArray[i].R;
colors[i].a = 255;
////for each point-1 , use the T-matrix to tansform it to point-0
matrix_P1[0, 0] = vertices[i].x;
matrix_P1[1, 0] = vertices[i].y;
matrix_P1[2, 0] = vertices[i].z;
matrix_P1[3, 0] = 1;
matrix_P0 = (DenseMatrix)(matrix_T * matrix_P1);
//according to the position of the arm to save the pointcloud data
switch (position)
{
case 1:
{
vertices_0_b1[i].x = (float)matrix_P0[0, 0];
vertices_0_b1[i].y = (float)matrix_P0[1, 0];
vertices_0_b1[i].z = (float)matrix_P0[2, 0];
colors_0_b1[i] = colors[i];
}
break;
case 2:
{
vertices_0_b2[i].x = (float)matrix_P0[0, 0];
vertices_0_b2[i].y = (float)matrix_P0[1, 0];
vertices_0_b2[i].z = (float)matrix_P0[2, 0];
colors_0_b2[i] = colors[i];
}
break;
case 3:
{
vertices_0_b3[i].x = (float)matrix_P0[0, 0];
vertices_0_b3[i].y = (float)matrix_P0[1, 0];
vertices_0_b3[i].z = (float)matrix_P0[2, 0]; colors_0_b1[i] = colors[i];
colors_0_b3[i] = colors[i];
}
break;
case 4:
{
vertices_0_b4[i].x = (float)matrix_P0[0, 0];
vertices_0_b4[i].y = (float)matrix_P0[1, 0];
vertices_0_b4[i].z = (float)matrix_P0[2, 0];
colors_0_b4[i] = colors[i];
}
break;
case 5:
{
vertices_0_b5[i].x = (float)matrix_P0[0, 0];
vertices_0_b5[i].y = (float)matrix_P0[1, 0];
vertices_0_b5[i].z = (float)matrix_P0[2, 0];
colors_0_b5[i] = colors[i];
}
break;
}
}
info.Text = "times:" + times.ToString() + "," + "position:" + position.ToString() + "," + "scanning and processing done!";
}
else
{
info.Text = "times:" + times.ToString() + "state.cartesianInfo is null!";
}
}
/// <summary>
/// Train for the specified pattern.
/// </summary>
/// <param name="matrix">The synapse to train.</param>
/// <param name="input">The input pattern to train for.</param>
/// <param name="current">The current output neuron being trained.</param>
/// <param name="bmu">The best matching unit, or winning output neuron.</param>
private void TrainPattern(Matrix matrix, double[] input,
int current, int bmu)
{
for (var inputNeuron = 0; inputNeuron < _inputNeuronCount; inputNeuron++)
{
var currentWeight = matrix[current, inputNeuron];
var inputValue = input[inputNeuron];
var newWeight = DetermineNewWeight(currentWeight,
inputValue, current, bmu);
_correctionMatrix[current, inputNeuron] = newWeight;
}
}
internal Vector SolveInternal(Matrix A, Vector b, Vector c, StartingBasis B, Matrix AB, Vector bB)
{
//Init simplex
var m = A.RowCount;
var ABi = AB.Inverse();
//X is a starting vector
var x = AB.LU().Solve(bB);
while (true)
{
iteration++;
//Compute lambda (cT*AB.inv())T
var lambda = (c.ToRowMatrix()*ABi).Transpose().ToRowWiseArray();
//check for optimality
if (lambda.All(l => l >= 0)) return (Vector) x;
//Find leaving index r (first index where component < 0)
var r = lambda.Select((i, index) => new {i, index})
.Where((i, index) => i.i < 0)
.First().index;
//compute direction to move int - take r-th column
var d = ABi.Column(r)*(-1);
//Determine the set K (all indexes of positive values of lambda)
//all k that a(k).T*d>0, 1 <= i <=m
var K = new List<int>();
for (int k = 0; k < m; k++)
{
var val = A.Row(k)*d;
if (val > 0 && !val.FloatEquals(0))
K.Add(k);
}
if (K.Count == 0)
throw new SimplexException("Problem is unbounded") {Iteration = iteration};
//Find entering index e
int e = 0;
var v = double.MaxValue;
foreach (var k in K)
{
var w = (b[k] - A.Row(k)*x)/(A.Row(k)*d);
if (!(w < v)) continue;
v = w;
e = k;
}
//Update basis
B.InequalityIndexes[r] = e;
AB.SetRow(r, A.Row(e));
bB[r] = b[e];
//Trick - lets update inverse AB in a smart way - sinse there is only one new inequality we only need to
//compute new inversed row (should drop complexity of whole algo to n*n)
var f = AB.Row(r)*ABi;
var g = -f;
g[r] = 1;
g /= f[r];
g[r] -= 1;
ABi = ABi.Add(ABi.Column(r).ToColumnMatrix()*g.ToRowMatrix());
//Compute new x
x = x + v*d;
}
}
/// <summary>
/// Negate each element of this matrix and place the results into the result matrix.
/// </summary>
/// <param name="result">The result of the negation.</param>
protected override void DoNegate(Matrix<double> result)
{
Map(x => -x, result, Zeros.AllowSkip);
}
protected override void DoPointwiseCeiling(Matrix<double> result)
{
Map(Math.Ceiling, result, Zeros.AllowSkip);
}
/// <summary>
/// Create a new diagonal matrix as a copy of the given other matrix.
/// This new matrix will be independent from the other matrix.
/// The matrix to copy from must be diagonal as well.
/// A new memory block will be allocated for storing the matrix.
/// </summary>
public static DiagonalMatrix OfMatrix(Matrix <double> matrix)
{
return(new DiagonalMatrix(DiagonalMatrixStorage <double> .OfMatrix(matrix.Storage)));
}
/// <summary>
/// Pointwise divide this matrix by another matrix and stores the result into the result matrix.
/// </summary>
/// <param name="divisor">The matrix to pointwise divide this one by.</param>
/// <param name="result">The matrix to store the result of the pointwise division.</param>
protected override void DoPointwiseDivide(Matrix<double> divisor, Matrix<double> result)
{
Map2((x, y) => x/y, divisor, result, Zeros.Include);
}
