Combining multiple transformations in Eigen into one transformation matrix

Question

I have several transformations in Eigen, in the form of translations (Eigen::Vector3f) and rotations (Eigen::Quaternionf). I would like to combine all these transformations, in an order of my choosing, into a 4x4 transformation matrix Eigen::Matrix4f.

For example, I would like to apply the following transformations in the order A, B, C, D, E:

Eigen::Vector3f translation_A;
Eigen::Quaternionf rotation_B;
Eigen::Quaternionf rotation_C;
Eigen::Quaternionf rotation_D;
Eigen::Vector3f translation_E;

What is the simplest way of achieving this? I have so far been applying them individually to my final data like so:

pcl::transformPointCloud(*cloud_in, *cloud_out, translation_A, IDENTITY_QUATERNION);
pcl::transformPointCloud(*cloud_out, *cloud_out, ZERO_TRANSLATION, rotation_B);
pcl::transformPointCloud(*cloud_out, *cloud_out, ZERO_TRANSLATION, rotation_C);
pcl::transformPointCloud(*cloud_out, *cloud_out, ZERO_TRANSLATION, rotation_D);
pcl::transformPointCloud(*cloud_out, *cloud_out, translation_E, IDENTITY_QUATERNION);

...but there must be a simple (built-in?) way of combining these transformations using Eigen, into a final 4x4 transformation matrix.

Answer 1

There is a good tutorial in the Eigen docs: here

In your case, this should look like:

Eigen::Vector3f trans_vec_A;
//note that you have to create a Translation because multiplying a 
//Transform with a vector will _apply_ the transform to the vector
Eigen::Translation<float,3> translation_A(trans_vec_A);
Eigen::Quaternionf rotation_B;
Eigen::Quaternionf rotation_C;
Eigen::Quaternionf rotation_D;
Eigen::Vector3f trans_vec_E;
Eigen::Translation<float,3> translation_E(trans_vec_E);
Eigen::Transform<float,3,Affine> combined = 
      translation_A * rotation_B * rotation_C * rotation_D * translation_E;

---

Note that

combined = A*B*C*D*E

so combined applied to a vector v is

combined*v = A*B*C*D*E*v = A*(B*(C*(D*(E*v))))

that is, E is applied first, then D, and so on. In my mind, this is the correct order, but that may be a matter of taste.