// 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;
}
//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();
}
private void TeachNeuralNetwork(ImageRectangle[] rectangles, BackgroundWorker worker, DoWorkEventArgs doWorkEvent, bool test)
{
int N = n * m * 3;
RectangularMatrix weightMatrix = new RectangularMatrix(N, p);
Random rand = new Random();
for (int i = 0; i < N; i++)
{
for (int j = 0; j < p; j++)
{
weightMatrix[i, j] = rand.NextDouble() * 0.1;
}
}
RectangularMatrix secondWeightMatrix = weightMatrix.Transpose();
double totalError = e + 1;
int totalIterationNumber = 0;
state = new CurrentState();
RowVector[] vectors = new RowVector[rectangles.Length];
for (int i = 0; i < rectangles.Length; i++)
{
vectors[i] = new RowVector(((ImageRectangle)rectangles.GetValue(i)).GetVector());
}
while (totalError > e && totalIterationNumber < iterationNumber)
{
totalError = 0;
for (int i = 0; i < rectangles.Length; i++)
{
RowVector xVector = vectors[i];
RowVector yVector = xVector * weightMatrix;
RowVector xSecondVector = yVector * secondWeightMatrix;
RowVector deltaXVector = xSecondVector - xVector;
weightMatrix = weightMatrix - a * xVector.Transpose() * deltaXVector * secondWeightMatrix.Transpose();
secondWeightMatrix = secondWeightMatrix - a * yVector.Transpose() * deltaXVector;
}
for (int i = 0; i < rectangles.Length; i++)
{
RowVector xVector = vectors[i];
RowVector yVector = xVector * weightMatrix;
RowVector xSecondVector = yVector * secondWeightMatrix;
RowVector deltaXVector = xSecondVector - xVector;
for (int j = 0; j < deltaXVector.ColumnCount; j++)
{
totalError += deltaXVector[0, j] * deltaXVector[0, j];
}
}
totalIterationNumber++;
state.CurentError = totalError;
state.IterationNumber = totalIterationNumber;
if (!test)
{
worker.ReportProgress(0, state);
if (worker.CancellationPending)
{
doWorkEvent.Cancel = true;
break;
}
}
}
for (int i = 0; i < rectangles.Length; i++)
{
RowVector xVector = vectors[i];
RowVector yVector = xVector * weightMatrix;
RowVector xSecondVector = yVector * secondWeightMatrix;
((ImageRectangle)rectangles.GetValue(i)).SetPixelMatrixFromVector(xSecondVector.ToArray());
}
state.Compressing = CalculateCompressing(rectangles.Length, N);
state.FirstWeightMatrix = GetMatrixString(weightMatrix);
state.SecondWeightMatrix = GetMatrixString(secondWeightMatrix);
}