public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsTarget, List <Vector3d> pointsSource, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3d centroidReference = TransformPointsUtils.CalculateCentroid(pointsTarget);
List <Vector3d> pointsTargetShift = TransformPointsUtils.CalculatePointsShiftedByCentroid(pointsTarget, centroidReference);
Vector3d centroidToBeMatched = TransformPointsUtils.CalculateCentroid(pointsSource);
List <Vector3d> pointsSourceShift = TransformPointsUtils.CalculatePointsShiftedByCentroid(pointsSource, centroidToBeMatched);
//calculate correlation matrix
Matrix3d H = TransformPointsUtils.CalculateCorrelationMatrix(pointsTargetShift, pointsSourceShift);
//Matrix3d S;
double[] eigenvalues = new double[3];
Matrix3d R = CalculateRotationBySingularValueDecomposition(H, pointsSourceShift, icpVersionUsed);
double c;
if (icpVersionUsed == ICP_VersionUsed.Scaling_Zinsser)
{
c = CalculateScale_Zinsser(pointsSourceShift, pointsTargetShift, ref R);
}
if (icpVersionUsed == ICP_VersionUsed.Scaling_Du)
{
Matrix3d C = CalculateScale_Du(pointsSourceShift, pointsTargetShift, R);
R = Matrix3d.Mult(R, C);
}
Vector3d T = SVD.CalculateTranslation(centroidReference, centroidToBeMatched, R);
Matrix4d myMatrix = SVD.PutTheMatrix4together(T, R);
//double d = myMatrix.Determinant;
return(myMatrix);
}
//other methods for SVD, for possible later usage
//MathUtils.SingularValueDecomposition3x3(Harray, Uarray, warray, VTarray);
//MathNet.Numerics.Providers.LinearAlgebra.Mkl.MklLinearAlgebraProvider svd = new MathNet.Numerics.Providers.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
//double[] a = MatrixUtilsNumerics.doubleFromArrayDouble(Harray);
//double[] u = MatrixUtilsNumerics.doubleFromArrayDouble(Uarray);
//double[] vt = MatrixUtilsNumerics.doubleFromArrayDouble(Uarray);
//double[] s = new double[3];
//svd.SingularValueDecomposition(true, a, 3, 3, s, u, vt);
private static Matrix3d CalculateRotationBySingularValueDecomposition(Matrix3d H, List <Vector3d> pointsSourceShift, ICP_VersionUsed icpVersionUsed)
{
double[,] Harray = TransformPointsUtils.DoubleArrayFromMatrix(H);
double[,] Uarray = new double[3, 3];
double[,] VTarray = new double[3, 3];
double[] eigenvalues = new double[3];
//trial 3:
alglib.svd.rmatrixsvd(Harray, 3, 3, 2, 2, 2, ref eigenvalues, ref Uarray, ref VTarray);
Matrix3d U = MatrixUtilsOpenTK.DoubleArrayToMatrix3d(Uarray);
Matrix3d VT = MatrixUtilsOpenTK.DoubleArrayToMatrix3d(VTarray);
Matrix3d R = Matrix3d.Mult(U, VT);
Matrix3d UT = Matrix3d.Transpose(U);
Matrix3d V = Matrix3d.Transpose(VT);
Matrix3d Rtest = Matrix3d.Mult(UT, V);
//R = Rtest;
Matrix3d checkShouldGiveI = Matrix3d.Mult(UT, U);
checkShouldGiveI = Matrix3d.Mult(VT, V);
Matrix3d RT = Matrix3d.Transpose(R);
checkShouldGiveI = Matrix3d.Mult(RT, R);
//see article by Umeyama
//if (H.Determinant < 0)
//{
// R[2, 2] = -R[2, 2];
// //S[2, 2] = -1;
//}
//calculate the sign matrix for using the scale factor
Matrix3d K2 = Matrix3d.Identity;
double check = U.Determinant * VT.Determinant;
//if (check < 0 && Math.Abs(check) > 1E-3)
//{
// K2[2, 2] = -1;
//}
RT = Matrix3d.Transpose(R);
checkShouldGiveI = Matrix3d.Mult(RT, R);
double scale = CalculateScale_Umeyama(pointsSourceShift, eigenvalues, K2);
R = Matrix3d.Mult(R, K2);
if (icpVersionUsed == ICP_VersionUsed.Scaling_Umeyama)
{
//R = Matrix3d.Mult(R, K2);
R = MatrixUtilsOpenTK.MultiplyScalar3D(R, scale);
}
////check eigenvectors
//Matrix3d Snew = Matrix3d.Mult(U, MatrixUtilsOpenTK.Matrix3FromMatrix3d(H));
//Snew = Matrix3d.Mult(Snew, VT);
//Matrix3d si = S.Inverted();
//Matrix3d Rnew = Matrix3d.Mult(VT, si);
//Rnew = Matrix3d.Mult(Rnew, U);
//Rnew = Matrix3d.Mult(VT, S);
//Rnew = Matrix3d.Mult(Rnew, U);
return(R);
}
