/// <summary>
/// Moore–Penrose pseudoinverse
/// If A = U • Σ • VT is the singular value decomposition of A, then A† = V • Σ† • UT.
/// For a diagonal matrix such as Σ, we get the pseudoinverse by taking the reciprocal of each non-zero element
/// on the diagonal, leaving the zeros in place, and transposing the resulting matrix.
/// In numerical computation, only elements larger than some small tolerance are taken to be nonzero,
/// and the others are replaced by zeros. For example, in the MATLAB or NumPy function pinv,
/// the tolerance is taken to be t = ε • max(m,n) • max(Σ), where ε is the machine epsilon. (Wikipedia)
/// </summary>
/// <param name="M">The matrix to pseudoinverse</param>
/// <returns>The pseudoinverse of this Matrix</returns>
public Matrix <double> PseudoInverse(Matrix <double> M)
{
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> D = M.Svd(true);
Matrix <double> W = (Matrix <double>)D.W;
Vector <double> s = (Vector <double>)D.S;
// The first element of W has the maximum value.
double tolerance = MathNet.Numerics.Precision.EpsilonOf(2) * Math.Max(M.RowCount, M.ColumnCount) * W[0, 0];
for (int i = 0; i < s.Count; i++)
{
if (s[i] < tolerance)
{
s[i] = 0;
}
else
{
s[i] = 1 / s[i];
}
}
W.SetDiagonal(s);
// (U * W * VT)T is equivalent with V * WT * UT
return((Matrix <double>)(D.U * W * D.VT).Transpose());
}
private static MathNet.Numerics.LinearAlgebra.Matrix <double> PseudoInverse2(MathNet.Numerics.LinearAlgebra.Double.Matrix matrix)
{
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svd = MathNet.Numerics.LinearAlgebra.Double.Factorization.UserSvd.Create(matrix, true);
MathNet.Numerics.LinearAlgebra.Matrix <double> w = svd.W;
MathNet.Numerics.LinearAlgebra.Vector <double> s = svd.S;
double tolerance = 008 * svd.L2Norm * System.Math.Pow(2, -53);
for (int i = 0; i < s.Count; i++)
{
s[i] = s[i] < tolerance ? 0 : 1 / s[i];
}
for (var i = 0; i < 008; i++)
{
double value = s.At(i);
w.Storage.At(i, i, value);
}
MathNet.Numerics.LinearAlgebra.Matrix <double> ddd = svd.U * w * svd.VT;
return(ddd.Transpose());
}
public static decimal[] LinearLeastSquares(decimal[,] A, decimal[] b, String method = "SVD")
{
decimal[] res = new decimal[A.GetLength(1)];
if (method == "Gauss")
{
decimal[,] B = MathDecimal.Prod(MathDecimal.Transpose(A), A);
decimal[] y = MathDecimal.Prod(MathDecimal.Transpose(A), b);
res = LinearAlgebra.Gauss(B, y);
}
else if (method == "LUDecomposition")
{
List <decimal[, ]> LU = LinearAlgebra.LUDecomposition(A);// L, U, PI1, Pi2;
//Solve system Ly=b1, where b1=PI1*b
//L^T*L*y=L^T*b1
// M=L^T * L , N=L^T*PI1*b
decimal[,] M = MathDecimal.Prod(MathDecimal.Transpose(LU[0]), LU[0]);
decimal[] N = MathDecimal.Prod(MathDecimal.Transpose(LU[0]), MathDecimal.Prod(LU[2], b));
decimal[] y = LinearAlgebra.Gauss(M, N);
//Solve system y=U*x1=U*PI2^T*x
res = MathDecimal.Prod(LU[3], LinearAlgebra.BackwardSubstitutionUpp(LU[1], y));
}
else if (method == "SVD")
{
double[,] ADouble = new double[A.GetLength(0), A.GetLength(1)];
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
ADouble[i, j] = (double)A[i, j];
}
}
Matrix <double> JMatrix = DenseMatrix.OfArray(ADouble);
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> Jsvd = JMatrix.Svd();
double[,] LDouble = Jsvd.W.ToArray();
double[,] UDouble = Jsvd.U.ToArray();
double[,] VtDouble = Jsvd.VT.ToArray();
decimal[,] L = new decimal[A.GetLength(0), A.GetLength(1)];
decimal[,] U = new decimal[A.GetLength(0), A.GetLength(0)];
decimal[,] Vt = new decimal[A.GetLength(1), A.GetLength(1)];
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
L[i, j] = (decimal)LDouble[i, j];
}
}
for (int i = 0; i < A.GetLength(0); i++)
{
for (int j = 0; j < A.GetLength(0); j++)
{
U[i, j] = (decimal)UDouble[i, j];
}
}
for (int i = 0; i < A.GetLength(1); i++)
{
for (int j = 0; j < A.GetLength(1); j++)
{
Vt[i, j] = (decimal)VtDouble[i, j];
}
}
for (int i = 0; i < L.GetLength(1); i++)
{
if (L[i, i] < 0.0000000000001M && L[i, i] > -0.0000000000001M)
{
L[i, i] = 0;
}
else
{
L[i, i] = 1 / L[i, i];
}
}
res = MathDecimal.Prod(MathDecimal.Transpose(Vt), MathDecimal.Prod(MathDecimal.Transpose(L), MathDecimal.Prod(MathDecimal.Transpose(U), b)));
}
return(res);
}
public static DenseMatrix SVDGetTransMat(DenseMatrix mat_source, DenseMatrix mat_target, ref CellIndex cellIndex, out double ErrValue)
{
//4 * NSamples
int CellsCount = 8;
DenseMatrix matP = mat_source.SubMatrix(0, 3, 0, mat_source.ColumnCount) as DenseMatrix;
if (cellIndex == null)
{
DenseMatrix matQ_init = mat_target.SubMatrix(0, 3, 0, mat_target.ColumnCount) as DenseMatrix;
cellIndex = CellIndex.GetCellIndex(matQ_init, CellsCount);
}
DenseMatrix matQ = cellIndex.DoPointMatch(matP);
DenseMatrix matErr = matQ - matP;
ErrValue = 0;
for (int i = 0; i < matErr.ColumnCount; i++)
{
ErrValue += Math.Sqrt(matErr[0, i] * matErr[0, i] + matErr[1, i] * matErr[1, i] + matErr[2, i] * matErr[2, i]);
}
DenseMatrix matP_Mean = Utils_PCA.getMeanMat(matP);
DenseMatrix matQ_Mean = Utils_PCA.getMeanMat(matQ);
DenseMatrix matT_MoveP = DenseMatrix.CreateIdentity(4);
DenseMatrix matT_MoveQ = DenseMatrix.CreateIdentity(4);
for (int i = 0; i < 3; i++)
{
matT_MoveP[i, 3] = matP_Mean[i, 0];
matT_MoveQ[i, 3] = matQ_Mean[i, 0];
}
matP = matP - matP_Mean;
matQ = matQ - matQ_Mean;
DenseMatrix matM = matP * matQ.Transpose() as DenseMatrix;
//matM.SplitUV(matU, matV, 0.01);
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svd = matM.Svd(true);
DenseMatrix matU = svd.U as DenseMatrix;
DenseMatrix matVT = svd.VT as DenseMatrix;
DenseMatrix matR = (matU * matVT).Transpose() as DenseMatrix;
DenseMatrix matT = DenseMatrix.CreateIdentity(4);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
matT[i, j] = matR[i, j];
}
}
matT = matT_MoveP.Inverse() * matT * matT_MoveQ as DenseMatrix;
return(matT);
}
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"));
}
}
}
