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