OpenCV in Python cv2.solvePnP return wrong results

Question

The function cv2.solvePnP of OpenCV in Python keep return vectors that far from right.

Here is the code:

import numpy as np
import cv2

k = np.eye(3)  # I added this here for the code to run. I use some matrix from a file. it is not suppose to change the results, since we use the same matrix for the creating the data and for     cv2.solvePnP

# create 3d points in homogenous coordinates
points_3d = np.array([
    [1.0, 1.0, 8.0, 1.0],
    [1.0, -1.0, 7.9, 1.0],
    [-1.0, 1.0, 8.0, 1.0],
    [-1.0, -1.0, 9.0, 1.0],
]).T

# create trivial transformation that rotate by pi/8 around the Z axis(the camera line of sight)
transformation = np.array([
    [np.cos(np.pi/8), -np.sin(np.pi/8), 0.0, 0.0],
    [np.sin(np.pi/8), np.cos(np.pi/8), 0.0, 0.0],
    [0.0, 0.0, 1.0, 0.0],
])

# project the 3D points to the camera as pixels
pixels_2d = k @ transformation @ points_3d
pixels_2d /= pixels_2d[2, :]
pixels_2d = pixels_2d[:2, :]

# fix the format of the array according to OpenCV docs
pixels_2d = np.ascontiguousarray(pixels_2d.reshape((4, 1, 2)))
points_3d = np.ascontiguousarray(points_3d[:3, :].reshape((4, 3)))

# call cv2.solvePnP
_, r, t = cv2.solvePnP(points_3d, pixels_2d, k, None, flags=cv2.SOLVEPNP_P3P)

# The expected results suppose to be very close to the rotation and translating that create the data.

r, _ = cv2.Rodrigues(r)

print(f'rotation matrix R=\n{r}\ntranslation vector t=\n{t}')

>>>rotation matrix R=
[[ 0.33196511 -0.65448479 -0.67930025]
 [ 0.73226038  0.63276397 -0.25180249]
 [ 0.59463762 -0.41383501  0.68930884]]
translation vector t=
[[-0.10543042]
 [ 1.17869639]
 [ 4.22398241]]

I tried to change the argument I pass to the function cv2.solvePnP with no success. In most cases the function returns Error.

Python 3.11.5

opencv-python 4.9.0.80

numpy 1.26.4

Answer 1

solvePnP expects the 3d points not in homogeneous coordinates and in the specific form:

[[[a_x a_y a_z]]    
 [[b_x b_y b_z]]    
 [[c_x c_y c_z]]    
 [[d_x d_y d_z]]]

try to call pnp using:

points_3d = points_3d[:3, :] / points_3d[3, :]
cv2.solvePnP(points_3d.T.reshape((-1, 1, 3)), pixels_2d.T.reshape((-1, 1, 2)), ...)

without the 'ascontiguousarray' correction that you use.