Ask Your Question
1

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

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

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: 1,111 times

Last updated: Mar 13