Add code for rectification

very much wip and not working right as is
master
CptCaptain 3 years ago
parent 83d202457a
commit ba7d63a7c8
  1. 118
      data/rectify.py

@ -0,0 +1,118 @@
import numpy as np
import xmltodict
import cv2.cv2 as cv2
def load_camera_parameters(path='calibration_result.xml'):
parameters = {'proj': {}, 'cam': {}}
with open(path) as f:
cam_mat = xmltodict.parse(f.read())
# get projector and camera intrinsics
for key in parameters:
K_shape = int(cam_mat['opencv_storage'][f'{key}_int']['rows']), int(cam_mat['opencv_storage'][f'{key}_int']['cols'])
parameters[key]['K'] = np.array(cam_mat['opencv_storage'][f'{key}_int']['data'].split(), dtype=float).reshape(K_shape).T
dist_shape = int(cam_mat['opencv_storage'][f'{key}_dist']['rows']), int(cam_mat['opencv_storage'][f'{key}_dist']['cols'])
parameters[key]['dist'] = np.array(cam_mat['opencv_storage'][f'{key}_dist']['data'].split(), dtype=float).reshape(dist_shape).T
# get image size
# weird casting cause the values are str(float) (eg. '123.'), but we want int
imsize_shape = int(cam_mat['opencv_storage'][f'img_shape']['rows']), int(cam_mat['opencv_storage'][f'img_shape']['cols'])
parameters['imsize'] = np.array([float(x) for x in cam_mat['opencv_storage']['img_shape']['data'].split()], dtype='uint16').reshape(imsize_shape).T
# get extrinsics
parameters['ext'] = {}
rot_shape = int(cam_mat['opencv_storage']['rotation']['rows']), int(cam_mat['opencv_storage']['rotation']['cols'])
parameters['ext']['R'] = np.array(cam_mat['opencv_storage'][f'rotation']['data'].split(), dtype=float).reshape(rot_shape).T
# switched cols and rows for mult compat with R
trans_shape = int(cam_mat['opencv_storage']['translation']['cols']), int(cam_mat['opencv_storage']['translation']['rows'])
parameters['ext']['T'] = np.array(cam_mat['opencv_storage'][f'translation']['data'].split(), dtype=float).reshape(trans_shape).T
return parameters
params = load_camera_parameters('../connecting_the_dots/data/calibration_result.xml')
# print(params)
print(
params['cam']['K'].shape,
params['cam']['dist'].shape,
params['proj']['K'].shape,
params['proj']['dist'].shape,
params['imsize'].shape,
params['ext']['R'].shape,
params['ext']['T'].shape,
)
# print(params['imsize'].reshape((2, 1)))
params['imsize'] = params['imsize'].reshape((2, 1))
# params['imsize'] = np.array([488, 688])
# print(params['imsize'].reshape((2, 1)))
# print(np.transpose(params['ext']['T'], params['ext']['T']))
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
params['cam']['K'],
params['cam']['dist'],
params['proj']['K'],
params['proj']['dist'],
# params['imsize'],
(688, 488),
params['ext']['R'],
params['ext']['T'],
)
################# SCRATCH ##############################
import math
def isclose(x, y, rtol=1.e-5, atol=1.e-8):
return abs(x-y) <= atol + rtol * abs(y)
def euler_angles_from_rotation_matrix(R):
'''
From a paper by Gregory G. Slabaugh (undated),
"Computing Euler angles from a rotation matrix
'''
phi = 0.0
if isclose(R[2,0],-1.0):
theta = math.pi/2.0
psi = math.atan2(R[0,1],R[0,2])
elif isclose(R[2,0],1.0):
theta = -math.pi/2.0
psi = math.atan2(-R[0,1],-R[0,2])
else:
theta = -math.asin(R[2,0])
cos_theta = math.cos(theta)
psi = math.atan2(R[2,1]/cos_theta, R[2,2]/cos_theta)
phi = math.atan2(R[1,0]/cos_theta, R[0,0]/cos_theta)
return psi, theta, phi
####################################################
print('R1:\n', R1)
print(euler_angles_from_rotation_matrix(R1))
print('R2:\n', R2)
print(euler_angles_from_rotation_matrix(R2))
print('P1:\n', P1)
print('P2:\n', P2)
print('Q :\n', Q)
pattern = cv2.imread('../connecting_the_dots/data/kinect_pattern.png')
sampled_pattern = cv2.imread('../connecting_the_dots/data/sampled_kinect_pattern.png')
proj_rect_map1, proj_rect_map2 = cv2.initInverseRectificationMap(
params['proj']['K'],
params['proj']['dist'],
R1,
# None,
P1,
# (688, 488),
(1280, 1024),
cv2.CV_16SC2,
)
rect_pat = cv2.remap(pattern, proj_rect_map1, proj_rect_map2, cv2.INTER_LINEAR)
# FIXME rect_pat is always zero
cv2.imshow('get rect', rect_pat)
cv2.waitKey()
# cv2.imshow(rect_pat2)
# cv2.waitKey()
Loading…
Cancel
Save