I have a transformation matrix of type Eigen::Matrix4d. And I would like to get its inverse. I write a function my self to compute it by the following formular.
.
And here is my code:
Eigen::Matrix4d inverseTransformation(Eigen::Matrix4d T)
{
Eigen::MatrixX3d R;
Eigen::Vector3d t;
R = T.block<3, 3>(0, 0);
t = T.block<3, 1>(0, 3);
Eigen::Matrix4d result;
result.setIdentity();
result.block<3, 3>(0, 0) = R.transpose();
result.block<3, 1>(0, 3) = -R.transpose() * t;
return result;
}
std::cout<<"Input transformation matrix is:\n" << T << std::endl;
std::cout << "inverse of T , my implementation:\n" << inverseTransformation(T) << std::endl << std::endl;
std::cout << "inverse of T , Eigen implementation::\n" << T.inverse() << std::endl << std::endl;
std::cout << "T * T^(-1), my implementation\n " << T * inverseTransformation(T) << std::endl << std::endl;
std::cout << "T * T^(-1), eigen's implementation\n " << T * T.inverse() << std::endl << std::endl;
Ideally, T * T^(1) should be I. However, there is some error in my result (the red part in the following picture.)
By contrast, the result from T * T.inverse() is much more accurate.
Could someone please tell me why? Thanks a lot in advance!
Update: Inverse of a rotation matrix is its transpose. The result will be more accurate if I replace R.tranpose() with R.inverse().
Thanks to the comments. Since I would like to close this question, I will answer it my self.
Theoretically, R.inverse() equals to R.tranpose(). But this is not correct in floating point system. Once I use R.inverse(), the result is more accurate.