多视角点云配准过程中,经常会涉及到空间变换参数R和T的逆运算,c++实现的代码为:
Eigen::Matrix4f inverse(Eigen::Matrix4f& RT)
{
Eigen::Matrix3f R = RT.block(0, 0, 3, 3);
Eigen::Vector3f t = RT.block(0, 3, 3, 1);
Eigen::Matrix4f inversed = Eigen::Matrix4f::Identity();
inversed.block(0, 0, 3, 3) = R.transpose().block(0, 0, 3, 3);
inversed.block(0, 3, 3, 1) = (-(R.transpose() * t)).block(0, 0, 3, 1);
return inversed;
}