Ask Your Question

OpenCV camera rvec tvec to ROS world pose

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

veilkrand gravatar image

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

2 Answers

Sort by ยป oldest newest most voted

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



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

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]],
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]

edit flag offensive delete link more



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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



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

Seen: 1,111 times

Last updated: Mar 13