Ask Your Question
1

OpenCV camera rvec tvec to ROS world pose

asked 2019-02-05 23:43:47 -0500

I'm detecting Aruco tags using the OpenCV bridge and I have tvec and rvec returned with the tag pose related to the camera using OpenCV axis notations. I would like convert them to a ROS pose related to the camera frame, so I can visualize the tag pose in the world frame using RVIZ. What would be the best approach?

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
4

answered 2020-03-12 11:14:26 -0500

Felix Widmaier gravatar image

updated 2020-03-13 06:06:30 -0500

tvec is a simple translation vector which can be used directly, rvec is a Rodrigues vector which first needs to be converted to a quaternion.

After I had to figure this out myself, here is some actual (Python) code based on PeteBlackerThe3rd's answer:

import numpy as np
import cv2
import tf
from geometry_msgs.msg import PoseStamped

rvec, tvec = cv2.some_function()

# we need a homogeneous matrix but OpenCV only gives us a 3x3 rotation matrix
rotation_matrix = np.array([[0, 0, 0, 0],
                            [0, 0, 0, 0],
                            [0, 0, 0, 0],
                            [0, 0, 0, 1]],
                            dtype=float)
rotation_matrix[:3, :3], _ = cv2.Rodrigues(rvec)

# convert the matrix to a quaternion
quaternion = tf.transformations.quaternion_from_matrix(rotation_matrix)

# To visualize in rviz, you can, for example, publish a PoseStamped message:
pose = PoseStamped()
pose.header.frame_id = "camera_frame"
pose.pose.position.x = tvec[0]
pose.pose.position.y = tvec[1]
pose.pose.position.z = tvec[2]
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]

pose_publisher.publish(pose)
edit flag offensive delete link more

Comments

1

I appreciate that you posted example code. Note that you can make the pose assignment shorter like this:

pose.pose.position = geometry_msgs.msg.Point(*tvec)
pose.pose.orientation = geometry_msgs.msg.Quaternion(*rvec)

fvd gravatar image fvd  ( 2020-03-13 08:49:25 -0500 )edit

I think you mean:

pose.pose.orientation = geometry_msgs.msg.Quaternion(*quaternion)

rvec is a 3x3 matrix

votecoffee gravatar image votecoffee  ( 2021-05-14 14:05:16 -0500 )edit
1

@votecoffee: Your comment regarding quaternion is correct, however, rvec is not a matrix but a rotation vector. It is pointing along the axis of rotation and it's length gives the rotation angle around this axis. It can be converted to a 3x3 rotation matrix with cv2.Rodrigues as shown above.

Felix Widmaier gravatar image Felix Widmaier  ( 2021-06-10 11:09:53 -0500 )edit

Yes, you're correct. rvec is a vector. Thanks for the correction!

votecoffee gravatar image votecoffee  ( 2021-06-10 16:10:29 -0500 )edit
3

answered 2019-02-06 04:42:08 -0500

The frame of the pose openCV detects is already the camera frame, so you will just need to convert the tvec and rvec into a position and orientation repsectively.

The tvec is simply a translation vector so it's elements can be copied directly to the x, y & z values of the position in a pose message. Rvec is slightly more complicated however, you will need to use the openCV Rodrigues function to convert this into a 3x3 roation matrix. Then put the 9 values from the cv::Mat object into an equivalent tf2:::Matrix3x3 object. This object has a method getRotation which will return a quaternion of this rotation, now you have this quaternion you can copy its elements into the pose method.

Hope this makes sense.

edit flag offensive delete link more

Comments

1

What's the Python class to build a quaternion from a 3x3 rotation matrix?

veilkrand gravatar image veilkrand  ( 2019-02-06 05:38:07 -0500 )edit
0

answered 2021-06-27 22:36:17 -0500

r0gi gravatar image

updated 2021-06-27 23:04:07 -0500

tvec is pretty straightforward, it is a translation vector which can be used directly.

rvec describes a rotation about an axis in three dimensional space. It is simply a vector pointing in some direction in three dimensional space, and its length is equal to the angle of rotation around the vector.

angle axis vector

ROS expects orientations/rotations to be in quaternions, and there's an easier way to convert them to this format. Just use the tf.transformations.quaternion_about_axis function. All you need to do is find the magnitude of rvec, which you can do yourself manually, or just call cv2.norm(rvec) to get the magnitude. You may have rvec as a numpy 1x3 ndarray, so access it as rvec[0].

from tf.transformations import quaternion_about_axis

...

tf_stamped.transform.translation.x = tvec[0][0]
tf_stamped.transform.translation.y = tvec[0][1]
tf_stamped.transform.translation.z = tvec[0][2]
angle = cv2.norm(r[0])  // or use euclidean distance of each element added
tf_stamped.transform.rotation = Quaternion(*quaternion_about_axis(angle, r[0]))
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

3 followers

Stats

Asked: 2019-02-05 23:43:47 -0500

Seen: 3,021 times

Last updated: Jun 27