public static Matrix3 FindRotationMatrix(PointCloud pointsSourceTranslated, PointCloud pointsTargetTranslated, ICP_VersionUsed icpVersionUsed)
{
try
{
Matrix3 H = PointCloud.CorrelationMatrix(pointsSourceTranslated, pointsTargetTranslated);
Matrix3 R = CalculateRotationBySingularValueDecomposition(H, pointsSourceTranslated, icpVersionUsed);
//now scaling factor:
float c;
if (icpVersionUsed == ICP_VersionUsed.Zinsser)
{
c = CalculateScale_Zinsser(pointsSourceTranslated, pointsTargetTranslated, ref R);
}
if (icpVersionUsed == ICP_VersionUsed.Du)
{
Matrix3 C = CalculateScale_Du(pointsSourceTranslated, pointsTargetTranslated, R);
R = Matrix3.Mult(R, C);
}
return(R);
}
catch (Exception err)
{
System.Windows.Forms.MessageBox.Show("Error in finding rotation matrix: " + err.Message);
return(Matrix3.Identity);
}
}
