OpenCV camera rvec tvec to ROS world pose

edit

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)

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.

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[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]))