Add code for rectification

very much wip and not working right as is

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()
+