private Matrix4d AdjustSourceTargetByTranslation(Matrix4d myMatrixFound, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
{
Matrix3d R = myMatrixFound.ExtractMatrix3d();
Vector3d T = SVD.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R);
myMatrixFound = myMatrixFound.AddTranslation(T);
return(myMatrixFound);
}
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);
}