0
$\begingroup$

Rosanswers logo

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


Originally posted by LukeAI on ROS Answers with karma: 131 on 2021-05-19

Post score: 0


Original comments

Comment by stevemacenski on 2021-05-19:
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.

Comment by shonigmann on 2021-05-19:
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

Comment by LukeAI on 2021-05-20:
@shonigmann that's exactly what I meant, editing for clarity

Comment by shonigmann on 2021-05-20:
glad you figured it out!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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

Originally posted by LukeAI with karma: 131 on 2021-05-20

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.