How do I convert to camera pose in world from rvec and tvec of cv2.SolvePnP

373 Views Asked by At

I'm trying to get quaternions and translations out of rvec and tvec with code like this:

import cv2
import numpy as np
from scipy.spatial.transform import Rotation
# 2d
image_points_2D = [[1331.284,610],[1473.253,468.6115], [1191.823,748.8911],[1473.253,751.3885],[1191.823,471.1088],[1245.413,347.5563],[1501.027,477.5027],[1487.591,610]]
# 3d
figure_points_3D =[[ 0. ,0., -0.5 ], [ 0.5, 0.5, -0.5 ],[-0.5,-0.5,-0.5 ],[ 0.5 , -0.5, -0.5 ],[-0.5 ,  0.5, -0.5 ],[-0.45,  1. ,  0.4 ],[ 0.5 ,  0.5,  0.5 ],[ 0.5 ,  0. ,  0.  ]]
# camera
distortion_coeffs = np.zeros((4,1))
matrix_camera = np.array([[flx, 0, center_x],
                          [0, fly, center_y],
                          [0, 0, 1         ]], dtype = "double")
# PnP
success, vector_rotation, vector_translation = cv2.solvePnP(figure_points_3D, image_points_2D, matrix_camera, distortion_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
# get loss
reprojected_points, _ = cv2.projectPoints(figure_points_3D, vector_rotation, vector_translation, matrix_camera, distortion_coeffs)
loss = np.mean(np.abs(image_points_2D - reprojected_points.reshape(image_points_2D.shape)))
print("loss", loss)

# get pose
rotation = Rotation.from_rotvec(vector_rotation.flatten())
print("quat (x,y,z,w): ", rotation.as_quat())
print("translation", vector_translation.flatten())

Here the Loss value is displayed as 0, so I think the rvec and tvec values are correct.

On Unity (Game Engine), the camera look the target at the origin position (0,0,0) on Unity from camera position (-2, 0, -15) and pose (XYZW: 0, -0.131, 0, 0.991). The value of tvec is correct. However, the quaternion that converted from the rvec values have their y and w components inverted. Why is this?

Expected quaternion (Unity): (XYZW: 0, -0.131, 0, 0.991)

Output quaternion (solvePnP): (XYZW: 0, -0.991, 0, 0.131)

0

There are 0 best solutions below