[Python] Using PnP (P3P) from OpenCV, with calibration data from OCamCalib method

I have an event-based camera with a fisheye lens, which I calibrated using a Scaramuzza's method using the Python implementation py-OCamCalib (modified to work with a calibration lattice of LEDs instead of a checkerboard). I now need to use a PnP algorithm to detect the location of a UAV which was marked with UV LEDs, but OpenCV's implementation uses a different kind of calibration.

I can convert the image points to the world points using cam2world and world2cam, but I do not understand, how can I leverage these functions to get a result.

# the positions of the leds on the UAV are:
# 2 ------ 1
# | |
# | |
# 4 ------ 3
# where the square side is 400 mm

object_points = np.array([
[425, 0, 0], # LED 2
[425, 425, 0], # LED 3
[0, 425, 0] # LED 4
], dtype=np.float32)

image_points = np.array([
[518, 352], # LED 2
[732, 353], # LED 3
[639, 412] # LED 4
], dtype=np.float32)

# is this actually ok?
cx, cy = (646.9874839279291, 361.1465654590792)
fx = 1280/(2*np.tan(93.7))
fy = 720/(2*np.tan(93.7))

camera_matrix = np.array([
[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]
], dtype=np.float32)

# I do not have the distortion parameters needed for PnP

led2 = (518, 352)
led3 = (732, 353)
led4 = (639, 412)

from pyocamcalib.modelling.camera import Camera

cam = Camera().load_parameters_json("calibration.json")

leds = cam.cam2world(np.array([led2, led3, led4], dtype=np.float64))



How can I use the 3D camera ray coordinates from cam2world in the PnP algorithm?

Continue reading...