# 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.

edit retag close merge delete

Sort by » oldest newest most voted 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);

more

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?

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
p.position.y = matrix
p.position.z = matrix
p.orientation.x = q
p.orientation.y = q
p.orientation.z = q
p.orientation.w = q

more