齐次矩阵的逆矩阵
2.使用 eigen 求解
/**
* 求齐次矩阵的逆矩阵
* @param T
* @return
*/
Matrix4d homogeneousInverse(Matrix4d matrix4d) {
Matrix4d resultmatrix4d;
if(matrix4d.rows()==4 && matrix4d.cols()==4)
return resultmatrix4d;
Matrix3d R;//旋转矩阵
R = matrix4d.block<3, 3>(0, 0);//block3行3列的矩阵,从第0 行第0列开始分割0-3行,0-3列的数据
//获取平移矩阵
Eigen::Matrix<double, 3,1> t = matrix4d.block<3, 1>(0, 3); //block3行0列的矩阵,从第1行第3列开始分割所有行(0-3行)的第3列数据
Matrix3d Rt= R.transpose();//转置
Eigen::Matrix<double, 3,1> tinv = -Rt * t;
resultmatrix4d.setIdentity();
resultmatrix4d.block<3, 3>(0,