public void UpdateTransformation(List <Point3D> Kinect_LH, List <Point3D> Kinect_RH, List <Point3D> Hololens_LH, List <Point3D> Hololens_RH)
{
int size = Kinect_LH.Count * 2;
Matrix <float> A = Matrix <float> .Build.Dense(3, size, 0);
Matrix <float> B = Matrix <float> .Build.Dense(3, size, 0);
//let's load the matrixes
for (int j = 0; j < size; j += 2)
{
A[0, j] = Kinect_LH[j / 2].X;
A[1, j] = Kinect_LH[j / 2].Y;
A[2, j] = Kinect_LH[j / 2].Z;
A[0, j + 1] = Kinect_RH[j / 2].X;
A[1, j + 1] = Kinect_RH[j / 2].Y;
A[2, j + 1] = Kinect_RH[j / 2].Z;
B[0, j] = Hololens_LH[j / 2].X;
B[1, j] = Hololens_LH[j / 2].Y;
B[2, j] = Hololens_LH[j / 2].Z;
B[0, j + 1] = Hololens_RH[j / 2].X;
B[1, j + 1] = Hololens_RH[j / 2].Y;
B[2, j + 1] = Hololens_RH[j / 2].Z;
}
//R1 = Matrix<float>.Build.Dense(3,3,0);
//T1 = Vector<float>.Build.Dense(3,0);
A_m = Matrix <float> .Build.Dense(3, size, 0);
B_m = Matrix <float> .Build.Random(3, size, 0);
Centroid_A = Vector <float> .Build.Random(3);
Centroid_B = Vector <float> .Build.Random(3);
for (int i = 0; i < 3; i++)
{
Centroid_A[i] = 0;
Centroid_B[i] = 0;
for (int j = 0; j < size; j++)
{
Centroid_A[i] += A[i, j];
Centroid_B[i] += B[i, j];
}
Centroid_A[i] /= (float)size;
Centroid_B[i] /= (float)size;
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < size; j++)
{
A_m[i, j] = A[i, j] - Centroid_A[i];
B_m[i, j] = B[i, j] - Centroid_B[i];
}
}
// % calculate covariance matrix (is this the corrcet term?)
//H = Am * B';
Matrix <float> H = A_m.Multiply(B.Transpose());
MathNet.Numerics.LinearAlgebra.Factorization.Svd <float> svd = H.Svd(true);
//% find rotation
//[U, S, V] = svd(H);
//R = V * U';
//Matrix<float> R = svd.VT.Transpose().Multiply(svd.U.Transpose());
R = svd.VT.Transpose().Multiply(svd.U.Transpose());
t = -R * Centroid_A + Centroid_B;
}
/*
* public void Transform(Point3D p_in, out Point3D p_out)
* {
* //Point3D p = new Point3D
* p_out
* p_out.X = 0;
* p_out.Y = 0;
* p_out.Z = 0;
*
*
* }
*/
public void OldConstructor()
{
int a = 3;
int b = 10;
A = Matrix <float> .Build.Random(a, b);
float[,] MArray = { { 0, -1.0f, 0 }, { 0, 0, 1.0f }, { 1.0f, 0, 0 } };
float[] MVector = { 100f, -10f, 40f };
Matrix <float> R1 = Matrix <float> .Build.DenseOfArray(MArray);
Vector <float> T1 = Vector <float> .Build.Dense(MVector);
B = R1.Multiply(A);
for (int i = 0; i < a; i++)
{
for (int j = 0; j < b; j++)
{
B[i, j] = B[i, j] + T1[i];
}
}
A_m = Matrix <float> .Build.Dense(a, b, 0);
B_m = Matrix <float> .Build.Random(a, b, 0);
Centroid_A = Vector <float> .Build.Random(a);
Centroid_B = Vector <float> .Build.Random(a);
for (int i = 0; i < a; i++)
{
Centroid_A[i] = 0;
Centroid_B[i] = 0;
for (int j = 0; j < b; j++)
{
Centroid_A[i] += A[i, j];
Centroid_B[i] += B[i, j];
}
Centroid_A[i] /= (float)b;
Centroid_B[i] /= (float)b;
}
for (int i = 0; i < a; i++)
{
for (int j = 0; j < b; j++)
{
A_m[i, j] = A[i, j] - Centroid_A[i];
B_m[i, j] = B[i, j] - Centroid_B[i];
}
}
// % calculate covariance matrix (is this the corrcet term?)
//H = Am * B';
Matrix <float> H = A_m.Multiply(B.Transpose());
MathNet.Numerics.LinearAlgebra.Factorization.Svd <float> svd = H.Svd(true);
//% find rotation
//[U, S, V] = svd(H);
//R = V * U';
Matrix <float> R = svd.VT.Transpose().Multiply(svd.U.Transpose());
Vector <float> t = -R * Centroid_A + Centroid_B;
Matrix <float> C = R.Multiply(A);
for (int i = 0; i < a; i++)
{
for (int j = 0; j < b; j++)
{
C[i, j] = C[i, j] + t[i];
}
}
Console.WriteLine(R.ToString());
Console.WriteLine(t.ToString());
for (int i = 0; i < a; i++)
{
for (int j = 0; j < a; j++)
{
Console.WriteLine((R1[i, j] - R[i, j]).ToString("0.0000"));
}
}
}
