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); } }