ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# OpenCV camera rvec tvec to ROS world pose

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

Sort by » oldest newest most voted

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.pose.position.x = tvec
pose.pose.position.y = tvec
pose.pose.position.z = tvec
pose.pose.orientation.x = quaternion
pose.pose.orientation.y = quaternion
pose.pose.orientation.z = quaternion
pose.pose.orientation.w = quaternion

pose_publisher.publish(pose)

more

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)

I think you mean:

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


rvec is a 3x3 matrix

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.

Yes, you're correct. rvec is a vector. Thanks for the correction! 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.

more

1

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

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

from tf.transformations import quaternion_about_axis

...

tf_stamped.transform.translation.x = tvec
tf_stamped.transform.translation.y = tvec
tf_stamped.transform.translation.z = tvec
angle = cv2.norm(r)  // or use euclidean distance of each element added