Transformation matrices to geometry_msgs/Pose

asked 2021-05-27 13:56:04 -0500

SpaceTime
50 ●7 ●11 ●16

updated 2021-05-28 05:25:16 -0500

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 flag offensive close merge delete

add a comment

2

answered 2021-05-27 14:25:49 -0500

allenh1

3045 ●62 ●89 ●110 http://allen-software.com/

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

edit flag offensive delete link

Comments

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?

SpaceTime ( 2021-05-27 17:24:47 -0500 )edit

found a solution in python (see other answer)

SpaceTime ( 2021-05-28 05:25:46 -0500 )edit

add a comment

2

answered 2021-05-28 05:24:20 -0500

SpaceTime
50 ●7 ●11 ●16

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]

edit flag offensive delete link

Comments

Ah, great! Thanks for adding this. Good to have both.

allenh1 ( 2021-05-28 08:12:07 -0500 )edit

add a comment

Transformation matrices to geometry_msgs/Pose

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Transformation matrices to geometry_msgs/Pose edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Transformation matrices to geometry_msgs/Pose