Eigen combine rotation and translation into one matrix

DaedalusAlpha picture DaedalusAlpha · Aug 26, 2014 · Viewed 36.6k times · Source

I have a rotation matrix rot (Eigen::Matrix3d) and a translation vector transl (Eigen::Vector3d) and I want them both together in a 4x4 transformation matrix. I just for the life of me can't figure out how to do this in Eigen. I think Affine can be used somehow but I don't understand how it works.

Essentially I want a combination of How translation a matrix(4x4) in Eigen? and Multiplying Transform and Matrix types in Eigen

My code (that doesn't compile as I don't understand how Affine works) looks like this:

Eigen::Affine3d r(rot);
Eigen::Affine3d t(transl);
Eigen::Matrix4d m = t.matrix();
m *= r.matrix();

Answer

Blaz Bratanic picture Blaz Bratanic · Aug 26, 2014

You didn't post the compilation errors, nor what are rot and transl. Below is a working sample showing, how you can create a 4x4 transformation matrix.

#include <Eigen/Geometry>

Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az) {
  Eigen::Affine3d rx =
      Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
  Eigen::Affine3d ry =
      Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
  Eigen::Affine3d rz =
      Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
  return rz * ry * rx;
}

int main() {
  Eigen::Affine3d r = create_rotation_matrix(1.0, 1.0, 1.0);
  Eigen::Affine3d t(Eigen::Translation3d(Eigen::Vector3d(1,1,2)));

  Eigen::Matrix4d m = (t * r).matrix(); // Option 1

  Eigen::Matrix4d m = t.matrix(); // Option 2
  m *= r.matrix();
  return 0;
}