Obtaining rotation and translation matrix from homogeneous transformation matrix

Question

I have following piece of code:

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;

It prints:

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

Here, I am trying to obtain rotation and translation matrices from homogeneous transformation matrix. I guess translation matrix is the first three elements of the last column of homogeneous matrix and rotation matrix is top left 3x3 matrix. Translation matrix is returned correctly above. But rotation matrix is not exactly the top left 3x3 matrix of the homogeneous matrix.

I guess I am missing some basic concept here. What am doing wrong?

Answer 1

The documentation of Transform describes the different storage modes:

• Affine: the transformation is stored as a (Dim+1)^2 matrix, where the last row is assumed to be [0 ... 0 1].
• AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix.
• Projective: the transformation is stored as a (Dim+1)^2 matrix without any assumption.
• Isometry: same as Affine with the additional assumption that the linear part represents a rotation. This assumption is exploited to speed up some functions such as inverse() and rotation().

And this actually happens, Isometry simply returns what was stored in the constructor:

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
  }
};

But, this is version v3.4. As @chtz points out in the comment, this description and the optimization itself are not present in v3.3, rotation uses no "switchboard", but directly calls a fixed function:

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 (the method directly below the linked one) then calculates SVD, and that's where numerical inaccuracies can get introduced:

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());
  }
}

Isometry in v3.3 is practically an Affine with some parts switched off (like scale stops you with an assert, and there are some more).

I guess I am missing some basic concept here. What am doing wrong?

Seemingly you're using an older version of the library. Consider trying the code with v3.4.