public static void Reset()
{
ICPVersion = ICP_VersionUsed.Scaling_Umeyama;
SimulatedAnnealing = false;
NormalsCheck = false;
FixedTestPoints = false;
MaximumNumberOfIterations = 100;
ResetVertexToOrigin = true;
IterativeClosestPointTransform.DistanceOptimization = false;
}
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);
}
public static Matrix4d FindTransformationMatrix_WithoutCentroids(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
Matrix3d R = FindRotationMatrix(pointsSource, pointsTarget, icpVersionUsed);
//Vector3d T = new Vector3d();
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.FromMatrix3d(R);
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
//Vector3d centroidTarget_Double = new Vector3d(pointsTarget.CentroidVector.X, pointsTarget.CentroidVector.Y, pointsTarget.CentroidVector.Z);
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix_MinimumDistance(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed, float minimumDistance)
{
minimumDistance *= minimumDistance * 2;
List <Vector3> sourceList = new List <Vector3>();
List <Vector3> targetList = new List <Vector3>();
int numberOfVectorsNotTaken = 0;
for (int i = 0; i < pointsSource.Vectors.Length; i++)
{
float distance = pointsSource.Vectors[i].DistanceSquared(pointsTarget.Vectors[i]);
if (distance < minimumDistance)
{
sourceList.Add(pointsSource.Vectors[i]);
targetList.Add(pointsTarget.Vectors[i]);
}
else
{
numberOfVectorsNotTaken++;
}
}
if (numberOfVectorsNotTaken > 0)
{
System.Diagnostics.Debug.WriteLine("Ignored vectors in SVD because too far: " + numberOfVectorsNotTaken.ToString() + " : out of : " + pointsSource.Count.ToString());
}
PointCloud pSourceNew = PointCloud.FromListVector3(sourceList);
PointCloud pTargetNew = PointCloud.FromListVector3(targetList);
if (pSourceNew == null || pTargetNew == null)
{
return(Matrix4d.Identity);
}
return(FindTransformationMatrix(pSourceNew, pTargetNew, icpVersionUsed));
}
public static Matrix3d FindRotationMatrix(PointCloud pointsSourceTranslated, PointCloud pointsTargetTranslated, ICP_VersionUsed icpVersionUsed)
{
try
{
Matrix3d H = PointCloud.CorrelationMatrix_Double(pointsSourceTranslated, pointsTargetTranslated);
Matrix3d R = CalculateRotationBySingularValueDecomposition(H, pointsSourceTranslated, icpVersionUsed);
//now scaling factor:
double c;
if (icpVersionUsed == ICP_VersionUsed.Zinsser)
{
c = CalculateScale_Zinsser(pointsSourceTranslated, pointsTargetTranslated, ref R);
}
if (icpVersionUsed == ICP_VersionUsed.Du)
{
Matrix3d C = CalculateScale_Du(pointsSourceTranslated, pointsTargetTranslated, R);
R = Matrix3d.Mult(R, C);
}
return(R);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in finding rotation matrix: " + err.Message);
return(Matrix3d.Identity);
}
}
//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, PointCloud pointsSourceTranslated, ICP_VersionUsed icpVersionUsed)
{
Eigenvalues_Helper(H);
//gives R, the rotation matrix, U - the left eigenvectors, VT - right transposed eigenvectors, EV - the eigenvalues
//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;
//}
double scale = CalculateScale_Umeyama(pointsSourceTranslated, EV, K2);
R = Matrix3d.Mult(R, K2);
if (icpVersionUsed == ICP_VersionUsed.Umeyama)
{
//R = Matrix3d.Mult(R, K2);
R = R.MultiplyScalar(scale);
}
return(R);
}
public static Matrix4 FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3 centroidTarget = pointsTarget.CentroidVector;
PointCloud pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3 centroidSource = pointsSource.CentroidVector;
PointCloud pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3 R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3 T = SVD_Float.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4 myMatrix = new Matrix4();
myMatrix = myMatrix.PutTheMatrix4together(T, R);
return(myMatrix);
}
public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, ICP_VersionUsed icpVersionUsed)
{
//shift points to the center of mass (centroid)
Vector3d centroidTarget = pointsTarget.CalculateCentroid();
List <Vector3d> pointsTargetTranslated = pointsTarget.Clone();
pointsTargetTranslated.SubtractVector(centroidTarget);
Vector3d centroidSource = pointsSource.CalculateCentroid();
List <Vector3d> pointsSourceTranslated = pointsSource.Clone();
pointsSourceTranslated.SubtractVector(centroidSource);
Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);
Vector3d T = SVD.CalculateTranslation(centroidSource, centroidTarget, R);
Matrix4d myMatrix = new Matrix4d();
myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);
return(myMatrix);
}
