public static bool CheckRotationMatrix(this Matrix3d R)
{
//test
Matrix3d RT = R.Copy();
RT.Transpose();
RT = Matrix3d.Mult(R, RT);
return(RT.CompareMatrices(Matrix3d.Identity, 1e-10f));
//RT should be unit matrix
}
private void CheckSVD_NotNormalized(Matrix3d C)
{
//check transformation
Matrix3d R = Matrix3d.Mult(U_NotNormalized, VT_NotNormalized);
R = R.MultiplyDiagonalElements(EV_NotNormalized);
//R should be now C
R.CompareMatrices(C, 1e-2f);
//PointCloud.Rotate(pointsSource, VT);
}
private void CheckSVD(Matrix3d C)
{
//check transformation
Matrix3d R = Matrix3d.Mult(U, VT);
Matrix3d testRight = VT.MultiplyDiagonalElements(EV);
//R should be now C
Matrix3d testleft = Matrix3d.Mult(U, C);
testleft = Matrix3d.Mult(testleft, VT);
testRight.CompareMatrices(testleft, 1e-2f);
//PointCloud.Rotate(pointsSource, VT);
}
private static void CheckSVD(Matrix3d H, double threshold)
{
//check : H = U * EV * VT
Matrix3d EVMat = new Matrix3d();
EVMat = EVMat.FromVector(EV);
//check
// EV * VT
Matrix3d test = Matrix3d.Mult(EVMat, VT);
// U * EV * VT
test = Matrix3d.Mult(U, test);
test.CompareMatrices(H, threshold);
}
