private Matrix4 AdjustSourceTargetByTranslation(Matrix4 myMatrixFound, PointCloud pointCloudSource, PointCloud pointCloudTarget)
{
Matrix3 R = myMatrixFound.ExtractMatrix3();
Vector3 T = SVD_Float.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R);
myMatrixFound = myMatrixFound.AddTranslation(T);
return(myMatrixFound);
}
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);
}