从齐次变换矩阵得到旋转和平移矩阵

Question

我有以下代码：

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

在这里，我试图从齐次变换矩阵中获取旋转和平移矩阵。我猜平移矩阵是齐次矩阵最后一列的前三个元素，旋转矩阵是左上角的 3x3 矩阵。上面正确返回了翻译矩阵。但旋转矩阵并不完全是齐次矩阵的左上角 3x3 矩阵。

我想我在这里缺少一些基本概念。做错了什么？

Answer 1

Transform

的文档描述了不同的存储模式：

• Affine：变换存储为 (Dim+1)^2 矩阵，其中最后一行假设为 [0 ... 0 1]。
• AffineCompact：变换存储为 (Dim)x(Dim+1) 矩阵。
• Projective：变换存储为 (Dim+1)^2 矩阵，无需任何假设。
• Isometry：与Affine相同，附加假设线性部分代表旋转。利用这一假设来加速某些函数，例如

  inverse()

  和

  rotation()

  。

这实际上发生了，

Isometry

只是返回存储在构造函数中的内容：

template <typename Scalar, int Dim, int Mode, int Options>
EIGEN_DEVICE_FUNC typename Transform<Scalar, Dim, Mode, Options>::RotationReturnType
Transform<Scalar, Dim, Mode, Options>::rotation() const {
  return internal::transform_rotation_impl<Mode>::run(*this);
}

template <int Mode>
struct transform_rotation_impl {
  template <typename TransformType>
  EIGEN_DEVICE_FUNC static inline const typename TransformType::LinearMatrixType run(const TransformType& t) {
    typedef typename TransformType::LinearMatrixType LinearMatrixType;
    LinearMatrixType result;
    t.computeRotationScaling(&result, (LinearMatrixType*)0);
    return result;
  }
};
template <>
struct transform_rotation_impl<Isometry> {
  template <typename TransformType>
  EIGEN_DEVICE_FUNC static inline typename TransformType::ConstLinearPart run(const TransformType& t) {
    return t.linear();                     <===================== here
  }
};

但是，这是版本v3.4。正如@chtz在评论中指出的，这个描述和优化本身并不存在于v3.3中，

rotation

不使用“总机”，而是直接调用固定函数：

template<typename Scalar, int Dim, int Mode, int Options>
EIGEN_DEVICE_FUNC const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType
Transform<Scalar,Dim,Mode,Options>::rotation() const
{
  LinearMatrixType result;
  computeRotationScaling(&result, (LinearMatrixType*)0);
  return result;
}

computeRotationScaling

（链接方法正下方的方法）然后计算 SVD，这就是可能引入数值误差的地方：

template<typename Scalar, int Dim, int Mode, int Options>
template<typename RotationMatrixType, typename ScalingMatrixType>
EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
{
  JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);

  Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
  VectorType sv(svd.singularValues());
  sv.coeffRef(0) *= x;
  if(scaling) scaling->lazyAssign(svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint());
  if(rotation)
  {
    LinearMatrixType m(svd.matrixU());
    m.col(0) /= x;
    rotation->lazyAssign(m * svd.matrixV().adjoint());
  }
}

v3.3 中的

Isometry

实际上是一个

Affine

，其中某些部分被关闭（例如

scale

用断言阻止你，还有更多）。

我想我在这里缺少一些基本概念。做错了什么？

看来您正在使用旧版本的库。考虑尝试使用 v3.4 的代码。