# Transformation matrices to geometry_msgs/Pose

Hello everyone,

I am trying to calculate the pose of an object in the format for geometry_msgs/pose, therefore position in (x,y,z) and orientation in quaternion (x, z, z, w). I already got the pose in a different format, that is to say a 3 * 4 matrix (Edit: I found out later on they actually add a (0,0,0,1) to make the matrix in homogenous coordinates). Somebody told me the matrix represents the pose as follows:

• first 3 columns: 3x3 rotation matrix
• 4th column: translation vector

How can I transform between these two formats in both direction? Is there maybe some function already available? Thanks in advance!

EDIT: In the original question I forgot to answer that I was looking for a solution using Python. The accepted answer is using c++. Please refer to the additional answer I posted for the python version.

Originally posted by SpaceTime on ROS Answers with karma: 50 on 2021-05-27

Post score: 0

I'm not aware of any existing functions, but I'm sure you could google around some for the proper code snippet.

But, I would just construct atf2::Matrix3x3 from the rotational components, then create a tf2::Vector3 from the rest.

Then, you can just convert to the message.

tf2::Vector3 position;  /* obtained from 4th column */
tf2::Matrix3x3 rot_robot;  /* obtained from first three columns */
tf2::Quaternion orientation;
rot_robot.getRotation(orientation);

geometry_msgs::Pose out;
tf2::toMsg(position, out.position);
tf2::toMsg(orientation, out.orientation);


Originally posted by allenh1 with karma: 3055 on 2021-05-27

This answer was ACCEPTED on the original site

Post score: 2

Comment by SpaceTime on 2021-05-27:
Thank you for your answer, this already helps me out a lot! As I am using ROS in python and am still a beginner, I was wondering if the same components exist for rospy? I could not find a comparable snippet in c++. So I tried something along the lines of tf.Matrix3x3(). However when running, I get the error 'module' object has no attribute Matrix3x3. Is this only available in c++ or am I doing something wrong?

Comment by SpaceTime on 2021-05-28:
found a solution in python (see other answer)

I think I found the equivalent for Python, so I am leaving it here for future reference

cur_matrix = matrix.reshape(3,4)
cur_matrix_homo = np.vstack((cur_matrix, np.array([0, 0, 0, 1]))) # to homogenous coordinates

q = tf.transformations.quaternion_from_matrix(cur_matrix_homo)

p = Pose()
p.position.x = matrix[0][3]
p.position.y = matrix[1][3]
p.position.z = matrix[2][3]
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]


Originally posted by SpaceTime with karma: 50 on 2021-05-28

This answer was NOT ACCEPTED on the original site

Post score: 2