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