public MultivariateSample CreateMultivariateNormalSample(ColumnVector M, SymmetricMatrix C, int n)
{
int d = M.Dimension;
MultivariateSample S = new MultivariateSample(d);
SquareMatrix A = C.CholeskyDecomposition().SquareRootMatrix();
Random rng = new Random(1);
Distribution normal = new NormalDistribution();
for (int i = 0; i < n; i++) {
// create a vector of normal deviates
ColumnVector V = new ColumnVector(d);
for (int j = 0; j < d; j++) {
double y = rng.NextDouble();
double z = normal.InverseLeftProbability(y);
V[j] = z;
}
// form the multivariate distributed vector
ColumnVector X = M + A * V;
// add it to the sample
S.Add(X);
}
return (S);
}
public void SparseSquareMatrixAgreement()
{
int d = 6;
SparseSquareMatrix A = new SparseSquareMatrix(d);
SquareMatrix B = new SquareMatrix(d);
Random rng = new Random(1);
for (int i = 0; i < 2 * d; i++) {
int r = (int) Math.Floor(rng.NextDouble() * d);
int c = (int) Math.Floor(rng.NextDouble() * d);
A[r, c] = 2.0 * rng.NextDouble() - 1.0;
B[r, c] = A[r, c];
}
RowVector u = new RowVector(d);
ColumnVector v = new ColumnVector(d);
for (int i = 0; i < d; i++) {
u[i] = 2.0 * rng.NextDouble() - 1.0;
v[i] = 2.0 * rng.NextDouble() - 1.0;
}
RowVector uA = u * A;
RowVector uB = u * B;
Assert.IsTrue(TestUtilities.IsNearlyEqual(uA, uB));
ColumnVector Av = A * v;
ColumnVector Bv = B * v;
Assert.IsTrue(TestUtilities.IsNearlyEqual(Av, Bv));
}
// one-parameter constructor
internal FitResult(double p1, double dp1, TestResult test)
{
this.parameters = new ColumnVector(new double[] {p1}, 0, 1, 1, true);
this.covarianceMatrix = new SymmetricMatrix(1);
this.covarianceMatrix[0, 0] = dp1 * dp1;
this.covarianceMatrix.IsReadOnly = true;
this.test = test;
}
// n-parameter constructor
internal FitResult(IList<double> parameters, SymmetricMatrix covariance, TestResult test)
{
Debug.Assert(parameters != null);
Debug.Assert(covariance != null);
Debug.Assert(parameters.Count == covariance.Dimension);
// This is a bit of a hack to ensure we store read-only ColumnVectors and SymmetricMatrix objects.
this.parameters = ConvertListToReadOnlyVector(parameters);
this.covarianceMatrix = covariance;
this.covarianceMatrix.IsReadOnly = true;
this.test = test;
}
public Network(int windowSize, int imagesNumber, double learningCoefficient, double maxError, int maxIterations)
{
this.windowSize = windowSize;
this.imagesNumber = imagesNumber;
this.learningCoefficient = learningCoefficient;
this.maxError = maxError;
this.maxIterations = maxIterations;
weightMatrix1 = new RectangularMatrix(imagesNumber, windowSize + imagesNumber);
CreateRandomMatrix(weightMatrix1);
weightMatrix2 = new RowVector(imagesNumber);
CreateRandomRowVector(weightMatrix2);
contextNeurons = new ColumnVector(imagesNumber);
}
public static ColumnVector ConcatVertically(ColumnVector A, ColumnVector B)
{
ColumnVector result = new ColumnVector(A.RowCount + B.RowCount);
for (int i = 0; i < A.RowCount; i++)
{
result[i] = A[i];
}
for (int i = A.RowCount; i < A.RowCount + B.RowCount; i++)
{
result[i] = B[i - A.RowCount];
}
return result;
}
/// <summary>
/// Computes the inverse of the original matrix.
/// </summary>
/// <returns>The matrix M<sup>-1</sup>.</returns>
public SquareMatrix Inverse()
{
SquareMatrix TI = new SquareMatrix(n);
for (int c = 0; c < n; c++) {
ColumnVector e = new ColumnVector(n);
e[c] = 1.0;
ColumnVector v = Solve(e);
for (int r = 0; r < n; r++) {
TI[r, c] = v[r];
}
}
return (TI);
}
public void doWork()
{
matrix.Fill((j, i) => array[i, j]);
SquareQRDecomposition qr = matrix.QRDecomposition();
SquareLUDecomposition lu = matrix.LUDecomposition();
ColumnVector qrresult = qr.Solve(vector);
ColumnVector luresult = lu.Solve(vector);
ColumnVector c6 = matrix.Column(6);
RowVector r2 = matrix.Row(2);
SquareMatrix inv = matrix.Inverse();
ColumnVector result = new ColumnVector(inv.Dimension);
for (int i = 0; i < matrix.Dimension; ++i) {
for (int j = 0; j < matrix.Dimension; ++j) {
result[i] += matrix[i, j] * qrresult[j];
}
}
}
public void Ackley()
{
// Ackley's function has many local minima, and a global minimum at (0, 0) -> 0.
Func<IList<double>, double> function = (IList<double> x) => {
double s = 0.0;
double c = 0.0;
for (int i = 0; i < x.Count; i++) {
s += x[i] * x[i];
c += Math.Cos(2.0 * Math.PI * x[i]);
}
return (-20.0 * Math.Exp(-0.2 * Math.Sqrt(s / x.Count)) - Math.Exp(c / x.Count) + 20.0 + Math.E);
};
EvaluationSettings settings = new EvaluationSettings() { AbsolutePrecision = 1.0E-8, EvaluationBudget = 10000000 };
for (int n = 2; n < 16; n = (int) Math.Round(AdvancedMath.GoldenRatio * n)) {
Console.WriteLine("n={0}", n);
Interval[] box = new Interval[n];
for (int i = 0; i < box.Length; i++) box[i] = Interval.FromEndpoints(-32.0, 32.0);
MultiExtremum minimum = MultiFunctionMath.FindGlobalMinimum(function, box, settings);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine(minimum.Value);
foreach (double coordinate in minimum.Location) Console.WriteLine(coordinate);
Assert.IsTrue(minimum.Dimension == n);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = 2.0 * minimum.Precision }));
ColumnVector solution = new ColumnVector(n);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Location, solution, new EvaluationSettings() { AbsolutePrecision = 2.0 * Math.Sqrt(minimum.Precision) }));
}
}
public void Beale()
{
// Beale is a very interesting function.
// The only local minimum is at (3,1/2) where the function value is 0.
// But along the lines (0,+\infty) and (-\infty,1) the function value decreases toward 0 at infinity.
// From the point of view of a local minimizer those are perfectly valid downhill directions and
// if the minimzer gets caught in them it will move toward infinity until its evaluation budget is
// exhausted. Starting from y > 0 will probably keep us safe, or we can do bounded optimization.
Func<IList<double>, double> fBeale = (IList<double> x) =>
MoreMath.Sqr(1.5 - x[0] + x[0] * x[1]) +
MoreMath.Sqr(2.25 - x[0] + x[0] * x[1] * x[1]) +
MoreMath.Sqr(2.625 - x[0] + x[0] * x[1] * x[1] * x[1]);
ColumnVector start = new ColumnVector(2.0, 1.0);
MultiExtremum min = MultiFunctionMath.FindLocalMinimum(fBeale, start);
Console.WriteLine(min.EvaluationCount);
Console.WriteLine(min.Value);
ColumnVector solution = new ColumnVector(3.0, 0.5);
Assert.IsTrue(min.Dimension == 2);
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = min.Precision }));
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Location, solution, Math.Sqrt(TestUtilities.TargetPrecision)));
}
public void Vardim()
{
// This function is described by Powell in his NEWOUA paper as one that caused problems for the simplest formulation of that minimizer.
Func<IList<double>, double> f = (IList<double> x) => {
double s1 = 0.0;
double s2 = 0.0;
for (int i = 0; i < x.Count; i++) {
s1 += i * (x[i] - 1.0);
s2 += MoreMath.Sqr(x[i] - 1.0);
}
return (s2 + s1 * s1 + s1 * s1 * s1 * s1);
};
for (int n = 2; n < 8; n++) {
Console.WriteLine("n = {0}", n);
ColumnVector start = new ColumnVector(n);
ColumnVector solution = new ColumnVector(n);
solution.Fill((i, j) => 1.0);
MultiExtremum min = MultiFunctionMath.FindLocalMinimum(f, start);
Console.WriteLine(min.EvaluationCount);
Console.WriteLine("{0} ({1}) ?= 0.0", min.Value, min.Precision);
Assert.IsTrue(min.Dimension == n);
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = 4.0 * min.Precision }));
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Location, solution, new EvaluationSettings() { RelativePrecision = Math.Sqrt(4.0 * min.Precision) }));
}
}
public void SumOfPowers()
{
// This test is difficult because the minimum is emphatically not quadratic.
// We do get close to the minimum but we massivly underestimate our error.
Func<IList<double>, double> function = (IList<double> x) => {
double s = 0.0;
for (int i = 0; i < x.Count; i++) {
s += MoreMath.Pow(Math.Abs(x[i]), i + 2);
}
return (s);
};
for (int n = 2; n < 8; n++) {
Console.WriteLine(n);
ColumnVector start = new ColumnVector(n);
for (int i = 0; i < n; i++) start[i] = 1.0;
EvaluationSettings settings = new EvaluationSettings() { AbsolutePrecision = 1.0E-8, EvaluationBudget = 32 * n * n * n };
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(function, start, settings);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine("{0} {1}", minimum.Value, minimum.Precision);
Console.WriteLine("|| {0} {1} ... || = {2}", minimum.Location[0], minimum.Location[1], minimum.Location.FrobeniusNorm());
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = 1.0E-4 }));
//Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Location, new ColumnVector(n), new EvaluationSettings() { AbsolutePrecision = 1.0E-2 }));
}
}
public void ThreeHumpCamel()
{
// This function has three local minima, so not at all starting points should be expected to bring us to the global minimum at the origin.
Func<IList<double>, double> function = (IList<double> x) => 2.0 * MoreMath.Pow(x[0], 2) - 1.05 * MoreMath.Pow(x[0], 4) + MoreMath.Pow(x[0], 6) / 6.0 + x[0] * x[1] + MoreMath.Pow(x[1], 2);
ColumnVector start = new ColumnVector(1.0, 1.0);
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(function, start);
Console.WriteLine("{0} ({1}) ?= 0.0", minimum.Value, minimum.Precision);
Console.WriteLine("{0} {1}", minimum.Location[0], minimum.Location[1]);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = 2.0 * minimum.Precision }));
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Location, new ColumnVector(2), new EvaluationSettings { AbsolutePrecision = 2.0 * Math.Sqrt(minimum.Precision) }));
}
private static void BuildLinearSystem(double[] values, int[] breakIndices, int intervalsCount, out SquareMatrix leftHandMatrix, out ColumnVector rightHandVector)
{
// build linear system of equations to solve the approximation problem
var systemSize = 7 * intervalsCount - 3; // matrix size
leftHandMatrix = new SquareMatrix(systemSize);
rightHandVector = new ColumnVector(systemSize);
for (int intervalIndex = 0; intervalIndex < intervalsCount; ++intervalIndex)
{
var intervalStart = intervalIndex == 0 ? 0 : breakIndices[intervalIndex - 1];
var intervalEnd = breakIndices[intervalIndex];
var pointsCount = intervalEnd - intervalStart + 1;
var baseMatrixIndex = 7 * intervalIndex;
var ts = Enumerable.Range(intervalStart, pointsCount);
// fill the 4x4 part of the matrix
foreach (var row in Enumerable.Range(baseMatrixIndex, 4))
{
foreach (var col in Enumerable.Range(baseMatrixIndex, 4))
{
var sum = ts.Select(t => Math.Pow(t, row + col - 2 * baseMatrixIndex)).Sum();
leftHandMatrix[row, col] = sum;
}
rightHandVector[row] = ts.Sum(t => values[t] * Math.Pow(t, row - baseMatrixIndex));
}
if (intervalIndex < intervalsCount - 1) // we add additional matrix elements for non-last intervals
{
// fill the 5th row and column of the matrix
foreach (var i in Enumerable.Range(baseMatrixIndex + 1, 3))
{
var relativeIdx = i - baseMatrixIndex;
var value = relativeIdx * Math.Pow(ts.Last(), relativeIdx - 1);
leftHandMatrix[i, baseMatrixIndex + 4] = value;
leftHandMatrix[i + 7, baseMatrixIndex + 4] = -value;
leftHandMatrix[baseMatrixIndex + 4, i] = value;
leftHandMatrix[baseMatrixIndex + 4, i + 7] = -value;
}
// fill the 6th row and column of the matrix
foreach (var i in Enumerable.Range(baseMatrixIndex, 4))
{
var relativeIdx = i - baseMatrixIndex;
var value = Math.Pow(ts.Last(), relativeIdx);
leftHandMatrix[i, baseMatrixIndex + 5] = value;
leftHandMatrix[i + 7, baseMatrixIndex + 5] = -value;
leftHandMatrix[baseMatrixIndex + 5, i] = value;
leftHandMatrix[baseMatrixIndex + 5, i + 7] = -value;
}
{
// fill the 7th row and column of the matrix
var i = baseMatrixIndex + 2;
var value = 2;
leftHandMatrix[i, baseMatrixIndex + 6] = value;
leftHandMatrix[i + 7, baseMatrixIndex + 6] = -value;
leftHandMatrix[baseMatrixIndex + 6, i] = value;
leftHandMatrix[baseMatrixIndex + 6, i + 7] = -value;
i = baseMatrixIndex + 3;
value = 6 * ts.Last();
leftHandMatrix[i, baseMatrixIndex + 6] = value;
leftHandMatrix[i + 7, baseMatrixIndex + 6] = -value;
leftHandMatrix[baseMatrixIndex + 6, i] = value;
leftHandMatrix[baseMatrixIndex + 6, i + 7] = -value;
}
}
}
}
//Method that calculates the calibration parameters
void calibrazione(double[] X_kinect, double[] Y_kinect, double[] Z_kinect)
{
//Grid coordinates
double[] X_Ref_B = new double[12] { -517.5, -172.5, 172.5, 517.5, -517.5, -172.5, 172.5, 517.5, -517.5, -172.5, 172.5, 517.5 };// coordinate X dei punti della griglia di riferimento (origine posta al centro della griglia stessa)
double[] Y_Ref_B = new double[12] { 345.0, 345.0, 345.0, 345.0, 0.0, 0.0, 0.0, 0.0, -345.0, -345.0, -345.0, -345.0 };// coordinate Y dei punti della griglia di riferimento (origine posta al centro della griglia stessa)
double[] Z_Ref_B = new double[12];
Sample XKINECT = new Sample(X_kinect);
Sample YKINECT = new Sample(Y_kinect);
Sample ZKINECT = new Sample(Z_kinect);
// barycenter of Kinect coordinates
double X_Bar = XKINECT.Mean;
double Y_Bar = YKINECT.Mean;
double Z_Bar = ZKINECT.Mean;
//Kinect barycentric coordinates contaneirs
double[] X_Kinect_Bar = new double[12];
double[] Y_Kinect_Bar = new double[12];
double[] Z_Kinect_Bar = new double[12];
//Kinect barycentric coordinates
for (int i = 0; i < 12; i++)
{
X_Kinect_Bar[i] = X_kinect[i] - X_Bar;
Y_Kinect_Bar[i] = Y_kinect[i] - Y_Bar;
Z_Kinect_Bar[i] = Z_kinect[i] - Z_Bar;
}
//Modello rotazione + 2 parametri di scala Yoss=AX+b oppure B=Yoss-b=AX X=B*A^-1
RectangularMatrix A = new RectangularMatrix(36, 5);
ColumnVector B = new ColumnVector(36);
ColumnVector bb = new ColumnVector(36);
ColumnVector Yoss = new ColumnVector(36);
ColumnVector X = new ColumnVector(5);
SquareMatrix X1 = new SquareMatrix(5);
ColumnVector X2 = new ColumnVector(5);
for (int i = 0; i < 12; i++)
{
Yoss[3 * i] = X_Ref_B[i];
Yoss[3 * i + 1] = Y_Ref_B[i];
Yoss[3 * i + 2] = Z_Ref_B[i];
A[3 * i, 0] = Y_Kinect_Bar[i];
A[3 * i, 1] = -Z_Kinect_Bar[i];
A[3 * i, 2] = 0.0;
A[3 * i, 3] = X_Kinect_Bar[i];
A[3 * i, 4] = 0.0;
A[3 * i + 1, 0] = -X_Kinect_Bar[i];
A[3 * i + 1, 1] = 0.0;
A[3 * i + 1, 2] = Z_Kinect_Bar[i];
A[3 * i + 1, 3] = 0.0;
A[3 * i + 1, 4] = Y_Kinect_Bar[i];
A[3 * i + 2, 0] = 0.0;
A[3 * i + 2, 1] = X_Kinect_Bar[i];
A[3 * i + 2, 2] = -Y_Kinect_Bar[i];
A[3 * i + 2, 3] = 0.0;
A[3 * i + 2, 4] = 0.0;
bb[3 * i] = X_Kinect_Bar[i];
bb[3 * i + 1] = Y_Kinect_Bar[i];
bb[3 * i + 2] = Z_Kinect_Bar[i];
B[3 * i] = X_Ref_B[i] - X_Kinect_Bar[i];
B[3 * i + 1] = Y_Ref_B[i] - Y_Kinect_Bar[i];
B[3 * i + 2] = Z_Ref_B[i] - Z_Kinect_Bar[i];
}
X1 = (SquareMatrix)(A.Transpose() * A);
X2 = (A.Transpose() * B);
X = (X1.Inverse()) * X2;
a = X[0];
b = X[1];
c = X[2];
lambdaX = X[3];
lambdaY = X[4];
this.sw9.Write("{0:#####.0000} {1:#####.0000} {2:#####.0000} {3:#####.0000} {4:#####.0000} ", a, b, c, lambdaX, lambdaY);
this.sw9.WriteLine();
}
public void Rosenbrock()
{
// This is a multi-dimensional generalization of the famous Rosenbrock, aka banana function,
// which has a narrow parabolic valley whose floor slopes only gently to the minimum.
Func<IList<double>, double> fRosenbrock = delegate(IList<double> x) {
double s = 0.0;
for (int i = 0; i < (x.Count - 1); i++) {
s += 100.0 * MoreMath.Pow(x[i + 1] - x[i] * x[i], 2) + MoreMath.Pow(1.0 - x[i], 2);
}
return (s);
};
for (int n = 2; n < 8; n++)
{
Console.WriteLine("n={0}", n);
double[] start = new double[n];
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(fRosenbrock, start);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine("{0} {1} ?= 0.0", minimum.Value, minimum.Precision);
ColumnVector solution = new ColumnVector(n);
for (int i = 0; i < solution.Dimension; i++) solution[i] = 1.0;
Assert.IsTrue(minimum.Dimension == n);
Assert.IsTrue(minimum.Precision > 0.0);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, 0.0, new EvaluationSettings() { AbsolutePrecision = 2.0 * minimum.Precision }));
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Location, solution, new EvaluationSettings() { RelativePrecision = 2.0 * Math.Sqrt(minimum.Precision) }));
}
}
private ColumnVector Normalize(ColumnVector vector)
{
double normalizationValue = 0;
for (int i = 0; i < vector.RowCount; i++)
{
normalizationValue += Math.Pow(vector[i], 2);
}
if (normalizationValue != 0)
{
ColumnVector normalizedVector = new ColumnVector(vector.RowCount);
for (int i = 0; i < vector.RowCount; i++)
{
normalizedVector[i] = vector[i] / Math.Sqrt(normalizationValue);
}
return normalizedVector;
}
else
{
return vector;
}
}
public void Perm()
{
Func<IList<double>, double> fPerm = (IList<double> x) => {
double s = 0.0;
for (int i = 1; i <= x.Count; i++) {
double t = 0.0;
for (int j = 0; j < x.Count; j++) {
t += (j + 1) * (MoreMath.Pow(x[j], i) - 1.0 / MoreMath.Pow(j + 1, i));
}
s += MoreMath.Sqr(t);
}
return (s);
};
int n = 4;
ColumnVector start = new ColumnVector(n);
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(fPerm, start);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine(minimum.Value);
for (int i = 0; i < minimum.Dimension; i++) Console.WriteLine(minimum.Location[i]);
}
// Method that calculates the calibration parameters
public void Calibration(double[] X_Kinect, double[] Y_Kinect, double[] Z_Kinect)
{
double[] X_Ref_B = new double[dim];
double[] Y_Ref_B = new double[dim];
double[] Z_Ref_B = new double[dim];
if (colonne_griglia_calibrazione % 2 == 0 && righe_griglia_calibrazione % 2 == 0)
{
for (int i = 0; i < righe_griglia_calibrazione; i++)
{
for (int j = 0; j < colonne_griglia_calibrazione; j++)
{
X_Ref_B[colonne_griglia_calibrazione*i + j] = (-colonne_griglia_calibrazione/2 + j)*step + step/2;
Y_Ref_B[colonne_griglia_calibrazione*i + j] = (righe_griglia_calibrazione/2 - i)*step - step/2;
}
}
}
if (colonne_griglia_calibrazione % 2 == 0 && righe_griglia_calibrazione % 2 != 0)
{
for (int i = 0; i < righe_griglia_calibrazione; i++)
{
for (int j = 0; j < colonne_griglia_calibrazione; j++)
{
X_Ref_B[colonne_griglia_calibrazione * i + j] = (-colonne_griglia_calibrazione / 2 + j) * step + step / 2;
Y_Ref_B[colonne_griglia_calibrazione * i + j] = ((righe_griglia_calibrazione-1)/ 2 - i) * step;
}
}
}
if (colonne_griglia_calibrazione % 2 != 0 && righe_griglia_calibrazione % 2 == 0)
{
for (int i = 0; i < righe_griglia_calibrazione; i++)
{
for (int j = 0; j < colonne_griglia_calibrazione; j++)
{
X_Ref_B[colonne_griglia_calibrazione*i + j] = (-(colonne_griglia_calibrazione-1)/2 + j)*step;
Y_Ref_B[colonne_griglia_calibrazione*i + j] = (righe_griglia_calibrazione/2 - i)*step - step/2;
}
}
}
if (colonne_griglia_calibrazione % 2 != 0 && righe_griglia_calibrazione % 2 != 0)
{
for (int i = 0; i < righe_griglia_calibrazione; i++)
{
for (int j = 0; j < colonne_griglia_calibrazione; j++)
{
X_Ref_B[colonne_griglia_calibrazione * i + j] = (-(colonne_griglia_calibrazione - 1) / 2 + j) * step;
Y_Ref_B[colonne_griglia_calibrazione * i + j] = ((righe_griglia_calibrazione - 1) / 2 - i) * step;
}
}
}
// Grid coordinates
for (int k = 0; k < dim; k++)
{
this.Win.sw5.WriteLine("{0:#####.0000} {1:#####.0000} {2:#####.0000} ", X_Ref_B[k], Y_Ref_B[k], Z_Ref_B[k]);
}
this.Win.sw5.WriteLine();
Sample XKINECT = new Sample(X_Kinect);
Sample YKINECT = new Sample(Y_Kinect);
Sample ZKINECT = new Sample(Z_Kinect);
// barycenter of Kinect coordinates
double X_Bar = XKINECT.Mean;
double Y_Bar = YKINECT.Mean;
double Z_Bar = ZKINECT.Mean;
//Kinect barycentric coordinates contaneirs
double[] X_Kinect_Bar = new double[dim];
double[] Y_Kinect_Bar = new double[dim];
double[] Z_Kinect_Bar = new double[dim];
//Kinect barycentric coordinates
for (int i = 0; i < dim; i++)
{
X_Kinect_Bar[i] = X_Kinect[i] - X_Bar;
Y_Kinect_Bar[i] = Y_Kinect[i] - Y_Bar;
Z_Kinect_Bar[i] = Z_Kinect[i] - Z_Bar;
this.Win.sw3.WriteLine("{0:#####.0000} {1:#####.0000} {2:#####.0000} ", X_Kinect_Bar[i], Y_Kinect_Bar[i], Z_Kinect_Bar[i]);
}
this.Win.sw3.WriteLine();
//Modello rotazione + 2 parametri di scala Yoss = AX+b oppure B = Yoss-bb = AX X = B*A^-1
RectangularMatrix A = new RectangularMatrix(dim * 3, 5);
ColumnVector B = new ColumnVector(dim * 3);
ColumnVector bb = new ColumnVector(dim * 3);
ColumnVector Yoss = new ColumnVector(dim * 3);
ColumnVector X = new ColumnVector(5);
SquareMatrix X1 = new SquareMatrix(5);
ColumnVector X2 = new ColumnVector(5);
for (int i = 0; i < dim; i++)
{
Yoss[3 * i ] = X_Ref_B[i];
Yoss[3 * i + 1] = Y_Ref_B[i];
Yoss[3 * i + 2] = Z_Ref_B[i];
A[3 * i, 0] = Y_Kinect_Bar[i];
A[3 * i, 1] = -Z_Kinect_Bar[i];
A[3 * i, 2] = 0.0;
A[3 * i, 3] = X_Kinect_Bar[i];
A[3 * i, 4] = 0.0;
A[3 * i + 1, 0] = -X_Kinect_Bar[i];
A[3 * i + 1, 1] = 0.0;
A[3 * i + 1, 2] = Z_Kinect_Bar[i];
A[3 * i + 1, 3] = 0.0;
A[3 * i + 1, 4] = Y_Kinect_Bar[i];
A[3 * i + 2, 0] = 0.0;
A[3 * i + 2, 1] = X_Kinect_Bar[i];
A[3 * i + 2, 2] = -Y_Kinect_Bar[i];
A[3 * i + 2, 3] = 0.0;
A[3 * i + 2, 4] = 0.0;
bb[3 * i] = X_Kinect_Bar[i];
bb[3 * i + 1] = Y_Kinect_Bar[i];
bb[3 * i + 2] = Z_Kinect_Bar[i];
B[3 * i] = X_Ref_B[i] - X_Kinect_Bar[i];
B[3 * i + 1] = Y_Ref_B[i] - Y_Kinect_Bar[i];
B[3 * i + 2] = Z_Ref_B[i] - Z_Kinect_Bar[i];
}
X1 = (SquareMatrix)(A.Transpose() * A);
X2 = (A.Transpose() * B);
X = (X1.Inverse()) * X2;
a = X[0];
b = X[1];
c = X[2];
lambdaX = X[3];
lambdaY = X[4];
this.Win.sw12.Write("{0:#####.0000} {1:#####.0000} {2:#####.0000} {3:#####.0000} {4:#####.0000} ", a, b, c, lambdaX, lambdaY);
this.Win.sw12.WriteLine();
this.Win.textBlock8.Text = string.Format("Calibration parameters:\na = {0} \nb = {1} \nc = {2} \nlambdaX = {3} \nlambdaY = {4}", a, b, c, lambdaX, lambdaY);
SquareMatrix ROT = new SquareMatrix(3);
ROT[0, 0] = 1 + lambdaX;
ROT[0, 1] = a;
ROT[0, 2] = -b;
ROT[1, 0] = -a;
ROT[1, 1] = 1 + lambdaY;
ROT[1, 2] = c;
ROT[2, 0] = b;
ROT[2, 1] = -c;
ROT[2, 2] = 1;
}
public void GoldsteinPrice()
{
// Goldstein-Price has a valley with a complicated shape and a global minimum value of 3 at (0,-1).
// It also has local minima, so we have to start close to this minimum if we expect to end at it.
Func<IList<double>, double> fGoldsteinPrice = (IList<double> v) => {
double x = v[0];
double y = v[1];
return (
(1 + MoreMath.Pow(x + y + 1, 2) * (19 - 14 * x + 3 * x * x - 14 * y + 6 * x * y + 6 * y * y)) *
(30 + MoreMath.Pow(2 * x - 3 * y, 2) * (18 - 32 * x + 12 * x * x + 48 * y - 36 * x * y + 27 * y * y))
);
};
ColumnVector start = new ColumnVector(0.5, -0.5);
MultiExtremum min = MultiFunctionMath.FindLocalMinimum(fGoldsteinPrice, start);
Console.WriteLine(min.EvaluationCount);
Console.WriteLine(min.Value);
Assert.IsTrue(min.Dimension == 2);
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Value, 3.0));
Assert.IsTrue(TestUtilities.IsNearlyEqual(min.Location, new ColumnVector(0.0, -1.0), Math.Sqrt(TestUtilities.TargetPrecision)));
MultiExtremum min2 = MultiFunctionMath.FindGlobalMinimum(fGoldsteinPrice, new Interval[] { Interval.FromEndpoints(-2.0, 2.0), Interval.FromEndpoints(-2.0, 2.0) });
Assert.IsTrue(min2.Dimension == 2);
Assert.IsTrue(TestUtilities.IsNearlyEqual(min2.Value, 3.0, new EvaluationSettings() { AbsolutePrecision = min2.Precision }));
//Assert.IsTrue(TestUtilities.IsNearlyEqual(min2.Location, new ColumnVector(0.0, -1.0), Math.Sqrt(TestUtilities.TargetPrecision)));
}
public void EasomLocal()
{
Func<IList<double>, double> function = (IList<double> x) => Math.Cos(x[0]) * Math.Cos(x[1]) * Math.Exp(-(MoreMath.Sqr(x[0] - Math.PI) + MoreMath.Sqr(x[1] - Math.PI)));
// We can't start too far from minimum, since cosines introduce many local minima.
ColumnVector start = new ColumnVector(1.5, 2.0);
MultiExtremum maximum = MultiFunctionMath.FindLocalMaximum(function, start);
Console.WriteLine(maximum.Value);
Assert.IsTrue(TestUtilities.IsNearlyEqual(maximum.Value, 1.0, new EvaluationSettings() { AbsolutePrecision = 2.0 * maximum.Precision }));
Assert.IsTrue(TestUtilities.IsNearlyEqual(maximum.Location, new ColumnVector(Math.PI, Math.PI), new EvaluationSettings { AbsolutePrecision = 2.0 * Math.Sqrt(maximum.Precision) }));
}
public void STA()
{
// Has local minimum when any coordinate is at one of two values, global when all coordinates at one of them.
Func<IList<double>, double> fStyblinskiTang = (IList<double> x) => {
double fst = 0.0;
for (int i = 0; i < x.Count; i++) {
double x1 = x[i];
double x2 = MoreMath.Sqr(x1);
fst += x2 * (x2 - 16.0) + 5.0 * x1;
}
return (fst / 2.0);
};
// Asymptotic "minima" at infinity, and (3,1/2)->0
Func<IList<double>, double> fBeale = (IList<double> x) =>
MoreMath.Sqr(1.5 - x[0] + x[0] * x[1]) +
MoreMath.Sqr(2.25 - x[0] + x[0] * x[1] * x[1]) +
MoreMath.Sqr(2.625 - x[0] + x[0] * x[1] * x[1] * x[1]);
Func<IList<double>, double> fEggholder = (IList<double> x) => {
double y47 = x[1] + 47.0;
return(-y47 * Math.Sin(Math.Sqrt(Math.Abs(y47 + x[0] / 2.0))) - x[0] * Math.Sin(Math.Sqrt(Math.Abs(x[0] - y47))));
};
// Many local minima, global minimum at (0,0)->0
Func<IList<double>, double> fAckley = (IList<double> x) => {
double s = 0.0;
double c = 0.0;
for (int i = 0; i < x.Count; i++) {
s += x[i] * x[i];
c += Math.Cos(2.0 * Math.PI * x[i]);
}
return (-20.0 * Math.Exp(-0.2 * Math.Sqrt(s / x.Count)) - Math.Exp(c / x.Count) + 20.0 + Math.E);
};
// Burkin has a narrow valley, not aligned with any axis, punctuated with many tiny "wells" along its bottom.
// The deepest well is at (-10,1)-> 0.
Func<IList<double>, double> fBurkin = (IList<double> x) => 100.0 * Math.Sqrt(Math.Abs(x[1] - 0.01 * x[0] * x[0])) + 0.01 * Math.Abs(x[0] + 10.0);
Func<IList<double>, double> threeHumpCamel = (IList<double> x) => {
double x2 = x[0] * x[0];
return (2.0 * x2 - 1.05 * x2 * x2 + x2 * x2 * x2 / 6.0 + (x[0] + x[1]) * x[1]);
};
// Easom has many lobal extra ripples and then a deep but narrow global minimum at (\pi, \pi) -> -1
Func<IList<double>, double> fEasom = (IList<double> x) => -Math.Cos(x[0]) * Math.Cos(x[1]) * Math.Exp(-(MoreMath.Sqr(x[0] - Math.PI) + MoreMath.Sqr(x[1] - Math.PI)));
// Test over [-500,500], minimum at (420.969,...) -> -418.983*d, many local minima
Func<IList<double>, double> fSchwefel = (IList<double> x) => {
double s = 0.0;
for (int i = 0; i < x.Count; i++) {
s += x[i] * Math.Sin(Math.Sqrt(Math.Abs(x[i])));
}
return (-s);
};
Func<IList<double>, double> function = fSchwefel;
int n = 4;
ColumnVector start = new ColumnVector(n);
//for (int i = 0; i < start.Dimension; i++) start[i] = 1.0;
Interval[] box = new Interval[n];
for (int i = 0; i < box.Length; i++) {
box[i] = Interval.FromEndpoints(-500.0, 500.0);
}
//box[0] = Interval.FromEndpoints(-15.0, -5.0);
//box[1] = Interval.FromEndpoints(-3.0, 3.0);
MultiFunctionMath.FindGlobalMinimum(function, box);
//MultiFunctionMath.FindExtremum_Amobea(fStyblinskiTang, start, new EvaluationSettings() { RelativePrecision = 1.0E-10, AbsolutePrecision = 1.0E-12, EvaluationBudget = 1000 });
}
public double[] Predict(double[] sequence, int predictedAmount)
{
double[] predictedSequence = new double[predictedAmount];
for (int i = 0; i < predictedAmount; i++)
{
double[] image = new double[windowSize];
if (windowSize - i > 0)
{
Array.Copy(sequence, sequence.Length - windowSize + i, image, 0, windowSize - i);
Array.Copy(predictedSequence, 0, image, windowSize - i, i);
}
else
{
Array.Copy(predictedSequence, i - windowSize, image, 0, windowSize);
}
ColumnVector imageMatrix = new ColumnVector(image.Length);
for (int j = 0; j < image.Length; j++)
{
imageMatrix[j] = image[j];
}
ColumnVector X = ConcatVertically(imageMatrix, contextNeurons);
ColumnVector Y1 = weightMatrix1 * X;
double Y2 = weightMatrix2 * Y1;
predictedSequence[i] = Y2;
}
return predictedSequence;
}
public void Schwefel()
{
// Test over [-500,500], minimum at (420.969,...) -> -418.983*d, many local minima
Func<IList<double>, double> function = (IList<double> x) => {
double s = 0.0;
for (int i = 0; i < x.Count; i++) {
s += x[i] * Math.Sin(Math.Sqrt(Math.Abs(x[i])));
}
return (-s);
};
for (int n = 2; n < 32; n = (int) Math.Round(AdvancedMath.GoldenRatio * n)) {
Console.WriteLine("n={0}", n);
Interval[] box = new Interval[n];
for (int i = 0; i < box.Length; i++) box[i] = Interval.FromEndpoints(-500.0, 500.0);
MultiExtremum minimum = MultiFunctionMath.FindGlobalMinimum(function, box);
Assert.IsTrue(minimum.Dimension == n);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine("{0} ({1}) ?= {2}", minimum.Value, minimum.Precision, -418.983 * n);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, -418.983 * n, new EvaluationSettings() { AbsolutePrecision = minimum.Precision }));
foreach (double coordinate in minimum.Location) Console.WriteLine(coordinate);
ColumnVector solution = new ColumnVector(n);
solution.Fill((int i, int j) => 420.969);
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Location, solution, new EvaluationSettings() { RelativePrecision = 2.0 * Math.Sqrt(Math.Abs(minimum.Precision / minimum.Value)) }));
}
}
private ColumnVector[] createLearningMatrix(double[] sequence)
{
ColumnVector[] learningMatrix = new ColumnVector[imagesNumber];
for (int i = 0; i < imagesNumber; i++)
{
double[] data = new double[windowSize];
Array.Copy(sequence, i, data, 0, windowSize);
ColumnVector matrix = new ColumnVector(windowSize);
for (int j = 0; j < windowSize; j++)
{
matrix[j] = data[j];
}
learningMatrix[i] = matrix;
}
return learningMatrix;
}
public void SmoothedEasom()
{
// This function is mostly flat except very near (\pi, \pi).
// For (1,1) or (0,0) "converges" to minimum of 0 at (1.30, 1.30). This is probably a local minimum of the cosine product.
//Func<IList<double>, double> function = (IList<double> x) => -Math.Exp(-(MoreMath.Sqr(x[0] - Math.PI) + MoreMath.Sqr(x[1] - Math.PI)));
Func<IList<double>, double> function = (IList<double> x) => -Math.Cos(x[0]) * Math.Cos(x[1]) * Math.Exp(-(MoreMath.Sqr(x[0] - Math.PI) + MoreMath.Sqr(x[1] - Math.PI)));
ColumnVector start = new ColumnVector(2.0, 2.0);
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(function, start);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine(minimum.Value);
Console.WriteLine("{0} {1}", minimum.Location[0], minimum.Location[1]);
}
public void GaussianIntegrals()
{
Random rng = new Random(1);
for (int d = 2; d < 4; d++) {
if (d == 4 || d == 5 || d == 6) continue;
Console.WriteLine(d);
// Create a symmetric matrix
SymmetricMatrix A = new SymmetricMatrix(d);
for (int r = 0; r < d; r++) {
for (int c = 0; c < r; c++) {
A[r, c] = rng.NextDouble();
}
// Ensure it is positive definite by diagonal dominance
A[r, r] = r + 1.0;
}
// Compute its determinant, which appears in the analytic value of the integral
CholeskyDecomposition CD = A.CholeskyDecomposition();
double detA = CD.Determinant();
// Compute the integral
Func<IList<double>, double> f = (IList<double> x) => {
ColumnVector v = new ColumnVector(x);
double s = v.Transpose() * (A * v);
return (Math.Exp(-s));
};
Interval[] volume = new Interval[d];
for (int i = 0; i < d; i++) volume[i] = Interval.FromEndpoints(Double.NegativeInfinity, Double.PositiveInfinity);
IntegrationResult I = MultiFunctionMath.Integrate(f, volume);
// Compare to the analytic result
Console.WriteLine("{0} ({1}) {2}", I.Value, I.Precision, Math.Sqrt(MoreMath.Pow(Math.PI, d) / detA));
Assert.IsTrue(TestUtilities.IsNearlyEqual(I.Value, Math.Sqrt(MoreMath.Pow(Math.PI, d) / detA), new EvaluationSettings() { AbsolutePrecision = 2.0 * I.Precision }));
}
}
public void StylblinskiTang()
{
Func<IList<double>, double> fStyblinskiTang = (IList<double> x) => {
double fst = 0.0;
for (int i = 0; i < x.Count; i++) {
double x1 = x[i];
double x2 = MoreMath.Sqr(x1);
fst += x2 * (x2 - 16.0) + 5.0 * x1;
}
return (fst / 2.0);
};
// solution coordinate is root of 5 - 32 x + 4 x^3 = 0 with negative second derivative.
// There are two such roots.
double root1 = -2.9035340277711770951;
double root2 = 2.7468027709908369925;
// tested up to n=16, works with slightly decreasing accuracy of Location
for (int n = 2; n < 8; n++) {
Console.WriteLine(n);
ColumnVector start = new ColumnVector(n);
//ColumnVector start = new ColumnVector(-1.0, -2.0, -3.0, -4.0, -5.0, -6.0);
MultiExtremum minimum = MultiFunctionMath.FindLocalMinimum(fStyblinskiTang, start);
Console.WriteLine(minimum.EvaluationCount);
Console.WriteLine(minimum.Value);
for (int i = 0; i < minimum.Dimension; i++) {
Console.WriteLine(minimum.Location[i]);
Assert.IsTrue(
TestUtilities.IsNearlyEqual(minimum.Location[i], root1, Math.Sqrt(Math.Sqrt(TestUtilities.TargetPrecision))) ||
TestUtilities.IsNearlyEqual(minimum.Location[i], root2, Math.Sqrt(Math.Sqrt(TestUtilities.TargetPrecision)))
);
}
Assert.IsTrue(TestUtilities.IsNearlyEqual(minimum.Value, fStyblinskiTang(minimum.Location)));
}
}
public void Learn(BackgroundWorker backgroundWorker, DoWorkEventArgs e, double[] sequence, bool? showIteration)
{
ColumnVector[] learningMatrix = createLearningMatrix(sequence);
double[] etalons = createEtalons(sequence);
backgroundResult = new BackgroundResult();
backgroundResult.IsComplete = false;
double totalError;
int iterations = 0;
do
{
// learn
for (int i = 0; i < learningMatrix.Length; i++)
{
ColumnVector X = ConcatVertically(learningMatrix[i], contextNeurons);
ColumnVector Xn = Normalize(X);
double norm = X.FrobeniusNorm();
ColumnVector Y1 = weightMatrix1 * Xn;
double Y2 = weightMatrix2 * Y1;
double gamma2 = (Y2 * norm) - etalons[i];
RowVector gamma1 = gamma2 * weightMatrix2;
RowVector a = learningCoefficient * gamma1;
RectangularMatrix b = Xn * a;
weightMatrix1 = weightMatrix1 - b.Transpose();
weightMatrix2 = weightMatrix2 - ((gamma2 * learningCoefficient) * Y1).Transpose();
contextNeurons = Y1;
}
totalError = 0;
// calculate total error
for (int i = 0; i < learningMatrix.Length; i++)
{
ColumnVector X = ConcatVertically(learningMatrix[i], contextNeurons);
ColumnVector Xn = Normalize(X);
double norm = X.FrobeniusNorm();
ColumnVector Y1 = weightMatrix1 * Xn;
double Y2 = weightMatrix2 * Y1;
double gamma2 = Y2 * norm - etalons[i];
totalError += Math.Pow(gamma2, 2);
}
backgroundResult.IterationNumber = iterations;
backgroundResult.Error = totalError;
backgroundResult.IsComplete = false;
if (showIteration.Equals(true))
{
backgroundWorker.ReportProgress(0, backgroundResult);
Thread.Sleep(200);
if (backgroundWorker.CancellationPending)
{
e.Cancel = true;
break;
}
}
iterations++;
}
while (totalError >= maxError && iterations <= maxIterations);
backgroundResult.IterationNumber = iterations;
backgroundResult.Error = totalError;
backgroundResult.IsComplete = true;
backgroundWorker.ReportProgress(0, backgroundResult);
}
public void RectangularMatrixAccess()
{
// create a matrix via outer product
ColumnVector cv = new ColumnVector(new double[] { 1, 2 });
RowVector rv = new RowVector(new double[] { 3, 4, 5 });
RectangularMatrix M = cv * rv;
// check dimensions
Assert.IsTrue(M.RowCount == cv.Dimension);
Assert.IsTrue(M.ColumnCount == rv.Dimension);
// check values
for (int r = 0; r < M.RowCount; r++) {
for (int c = 0; c < M.ColumnCount; c++) {
Assert.IsTrue(M[r, c] == cv[r] * rv[c]);
}
}
// extract a column
ColumnVector mc = M.Column(1);
Assert.IsTrue(mc.Dimension == M.RowCount);
for (int i = 0; i < mc.Dimension; i++) {
Assert.IsTrue(mc[i] == M[i, 1]);
}
// extract a row
RowVector mr = M.Row(1);
Assert.IsTrue(mr.Dimension == M.ColumnCount);
for (int i = 0; i < mr.Dimension; i++) {
Assert.IsTrue(mr[i] == M[1, i]);
}
// test clone
RectangularMatrix MC = M.Copy();
Assert.IsTrue(MC.RowCount == M.RowCount);
Assert.IsTrue(MC.ColumnCount == M.ColumnCount);
// test equality of clone
Assert.IsTrue(MC == M);
Assert.IsFalse(MC != M);
// test independence of clone
MC[0, 0] += 1.0;
Assert.IsFalse(MC == M);
Assert.IsTrue(MC != M);
}