Ask Your Question
0

How to derive a pose quaternion from orientation vectors with tf2

asked 2021-05-19 13:59:39 -0500

LukeAI gravatar image

updated 2021-05-20 04:35:35 -0500

I am trying to calculate the pose of an object using tf2.

I know the X', Y', Z' vectors defining the orientation of the object - ie. the principle axes of the target frame, expressed in the origin frame. I also know the position of the target frame. How can I calculate the pose / quaternion that rotates the origin frame to the target frame (so that I can publish a geometry_msg/Pose for the object)?

edit retag flag offensive close merge delete

Comments

A quaternion is the orientation vector, that cannot be "calculated" from the position information, that itself is its own information about what the orientation is relative to the reference frame for the full SE3 pose. You have to know your own orientation.

stevemacenski gravatar image stevemacenski  ( 2021-05-19 19:31:00 -0500 )edit

In case LukeAI meant that he had vectors for x', y', and z', as in the principle axes of the target frame, expressed in a reference frame, then the pose quaternion can be determined using standard conversions between rotation matrix notation and quaternion notation. But then position would still be missing... so I agree the question could use a bit more clarity

shonigmann gravatar image shonigmann  ( 2021-05-19 20:12:47 -0500 )edit

@shonigmann that's exactly what I meant, editing for clarity

LukeAI gravatar image LukeAI  ( 2021-05-20 03:10:15 -0500 )edit

glad you figured it out!

shonigmann gravatar image shonigmann  ( 2021-05-20 10:28:20 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-05-20 08:32:26 -0500

LukeAI gravatar image

ok worked this out - the x_vec, y_vec, z_vec form the columns of the rotation matrix so the solution is simply:

tf2::Matrix3x3 rot_mat;
tf2::Quaternion q;
rot_mat.setValue(x_vec.x, y_vec.x, z_vec.x,
                 x_vec.y, y_vec.y, z_vec.y,
                 x_vec.z, y_vec.z, z_vec.z);
rot_mat.getRotation(q);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-05-19 13:59:39 -0500

Seen: 57 times

Last updated: May 20