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().
T * T.inverse()
to improve the result, you could do this iteratively to get incrementally better precision. Now it only depends at what point you stop that iteration. And eventually you are limited by precision ofdouble
R
is a rotation matrix, whose inverse is equal to its transpose. But it would be good if this were clarified in the question.