private Matrix4d Helper_FindTransformationMatrix(List <Vertex> pointsTarget, List <Vertex> pointsSource)
{
Matrix4d myMatrix;
if (ICPVersion == ICP_VersionUsed.Horn)
{
MatrixUtilsNew.FindTransformationMatrix(Vertices.ConvertToVector3dList(pointsSource), Vertices.ConvertToVector3dList(pointsTarget), this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
myMatrix = SVD.FindTransformationMatrix(Vertices.ConvertToVector3dList(pointsTarget), Vertices.ConvertToVector3dList(pointsSource), ICPVersion);
}
return(myMatrix);
}
private Matrix4d Helper_FindTransformationMatrix(PointCloudVertices pointsSource, PointCloudVertices pointsTarget)
{
Matrix4d myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(pointsSource), PointCloudVertices.ToVectors(pointsTarget), ICPSettings.ICPVersion);
}
return(myMatrix);
}
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);
}
private Matrix4 Helper_FindTransformationMatrix(PointCloud resultKDTree)
{
Matrix4 myMatrix;
if (ICPSettings.ICPVersion == ICP_VersionUsed.Horn)
{
MathUtilsVTK.FindTransformationMatrix(pointsSource, resultKDTree, this.LandmarkTransform);
myMatrix = LandmarkTransform.Matrix;
}
else
{
if (ICPSettings.IgnoreFarPoints)
{
myMatrix = SVD.FindTransformationMatrix_MinimumDistance(pointsSource, resultKDTree, ICPSettings.ICPVersion, this.MeanDistance).ToMatrix4();
}
else
{
myMatrix = SVD.FindTransformationMatrix(pointsSource, resultKDTree, ICPSettings.ICPVersion).ToMatrix4();
}
}
return(myMatrix);
}