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