从齐次变换矩阵提取旋转与平移矩阵的异常问题排查

我尝试从齐次变换矩阵中提取旋转矩阵和平移向量，认为平移向量是矩阵最后一列的前三个元素，旋转矩阵是左上3x3部分。当前平移向量提取正确，但用Eigen的Isometry3f转换后得到的旋转矩阵和原矩阵左上3x3部分不完全一致，想知道哪里出错了？

代码实现 #

Eigen::Matrix4f transformation = myMethod(); // homogenous transformation matrix
std::cout << "transformation = " << std::endl << transformation <<  std::endl << std::endl;
Eigen::Isometry3f estimate = Eigen::Isometry3f(transformation);
r = estimate.rotation(); // rotation matrix
t = estimate.translation(); // translation matrix
std::cout<<"r = " << std::endl << r << std::endl << std::endl;
std::cout<<"t = " << std::endl << t << std::endl << std::endl;

运行输出 #

transformation = 
     1.0000002384185791  4.9265406687482027e-07 -4.5500800638365035e-07  -2.384185791015625e-07
-5.2062489430682035e-07      1.0000003576278687  5.4357258250092855e-07 -3.5762786865234375e-07
 4.9866633844430908e-07 -4.6970637868071208e-07      1.0000002384185791  -2.384185791015625e-07
                      0                       0                       0                       1

r = 
     1.0000001192092896  7.2572328235764871e-07 -3.9811814644963306e-07
-6.7004600623477018e-07     0.99999994039535522  4.5586514829665248e-07
  5.422648428066168e-07 -4.4537293319990567e-07     0.99999994039535522

t = 
 -2.384185791015625e-07
-3.5762786865234375e-07
 -2.384185791015625e-07

原因与解决办法 #

• 核心原因：Isometry3f会强制修正为严格正交旋转矩阵
  你的原齐次矩阵左上3x3部分并不是严格的正交矩阵——从输出能看到对角元素都略大于1，非对角元素也存在微小不对称性，不符合旋转矩阵R^T * R = I且行列式为1的要求。Eigen在构造Isometry3f时，会自动对这部分做正交化处理（一般通过QR或SVD分解得到最接近的正交矩阵），所以最终得到的r和原矩阵左上3x3有差异。

• 如果要直接提取原矩阵的左上3x3部分，跳过Isometry3f构造
  直接用Eigen的矩阵块操作提取即可：

  Eigen::Matrix3f r = transformation.block<3,3>(0,0); // 直接取左上3x3区域
  Eigen::Vector3f t = transformation.block<3,1>(0,3); // 直接取最后一列前3个元素
  

  这样得到的r就和原矩阵完全一致，但要注意此时r可能不是严格的旋转矩阵，如果后续计算需要正交性，得手动做正交化处理。

内容的提问来源于stack exchange，提问作者MsA