public static Matrix3d FindRotationMatrix(List <Vector3d> pointsSourceTranslated, List <Vector3d> pointsTargetTranslated, ICP_VersionUsed icpVersionUsed)
{
try
{
Matrix3d H = PointCloudVertices.CorrelationMatrix(pointsSourceTranslated, pointsTargetTranslated);
Matrix3d R = CalculateRotationBySingularValueDecomposition(H, pointsSourceTranslated, icpVersionUsed);
//now scaling factor:
double c;
if (icpVersionUsed == ICP_VersionUsed.Scaling_Zinsser)
{
c = CalculateScale_Zinsser(pointsSourceTranslated, pointsTargetTranslated, ref R);
}
if (icpVersionUsed == ICP_VersionUsed.Scaling_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);
}
}
