#!/usr/bin/env python # -*- coding: utf-8 -*- ''' ################################################## ## TOML CALIBRATION TO OPENCAP CALIBRATION ## ################################################## Convert an OpenCV .toml calibration file to OpenCap .pickle calibration files. One file will be created for each camera. Usage: from Pose2Sim.Utilities import calib_toml_to_opencap; calib_toml_to_opencap.calib_toml_to_opencap_func(r'') OR python -m calib_toml_to_opencap -t input_toml_file OR python -m calib_toml_to_opencap -t input_toml_file -o output_calibration_folder> ''' ## INIT import os import pickle import argparse import numpy as np import toml import cv2 ## AUTHORSHIP INFORMATION __author__ = "David Pagnon" __copyright__ = "Copyright 2021, Pose2Sim" __credits__ = ["David Pagnon"] __license__ = "BSD 3-Clause License" __version__ = '0.4' __maintainer__ = "David Pagnon" __email__ = "contact@david-pagnon.com" __status__ = "Development" ## FUNCTIONS def world_to_camera_persp(r, t): ''' Converts rotation R and translation T from Qualisys object centered perspective to OpenCV camera centered perspective and inversely. Qc = RQ+T --> Q = R-1.Qc - R-1.T ''' r = r.T t = - r @ t return r, t def rotate_cam(r, t, ang_x=0, ang_y=0, ang_z=0): ''' Apply rotations around x, y, z in cameras coordinates Angle in radians ''' r,t = np.array(r), np.array(t) if r.shape == (3,3): rt_h = np.block([[r,t.reshape(3,1)], [np.zeros(3), 1 ]]) elif r.shape == (3,): rt_h = np.block([[cv2.Rodrigues(r)[0],t.reshape(3,1)], [np.zeros(3), 1 ]]) r_ax_x = np.array([1,0,0, 0,np.cos(ang_x),-np.sin(ang_x), 0,np.sin(ang_x),np.cos(ang_x)]).reshape(3,3) r_ax_y = np.array([np.cos(ang_y),0,np.sin(ang_y), 0,1,0, -np.sin(ang_y),0,np.cos(ang_y)]).reshape(3,3) r_ax_z = np.array([np.cos(ang_z),-np.sin(ang_z),0, np.sin(ang_z),np.cos(ang_z),0, 0,0,1]).reshape(3,3) r_ax = r_ax_z @ r_ax_y @ r_ax_x r_ax_h = np.block([[r_ax,np.zeros(3).reshape(3,1)], [np.zeros(3), 1]]) r_ax_h__rt_h = r_ax_h @ rt_h r = r_ax_h__rt_h[:3,:3] t = r_ax_h__rt_h[:3,3] return r, t def read_toml(toml_path): ''' Read an OpenCV .toml calibration file Returns 5 lists of size N (N=number of cameras): - S (image size), - D (distorsion), - K (intrinsic parameters), - R (extrinsic rotation), - T (extrinsic translation) ''' calib = toml.load(toml_path) C, S, D, K, R, T = [], [], [], [], [], [] for cam in list(calib.keys()): if cam != 'metadata': C += [calib[cam]['name']] S += [np.array(calib[cam]['size'])] D += [np.array(calib[cam]['distortions'])] K += [np.array(calib[cam]['matrix'])] R += [np.array(calib[cam]['rotation'])] T += [np.array(calib[cam]['translation'])] return C, S, D, K, R, T def write_opencap_pickle(output_calibration_folder, C, S, D, K, R, T): ''' Writes OpenCap .pickle calibration files Extrinsics in OpenCap are calculated with a vertical board for the world frame. As we want the world frame to be horizontal, we need to rotate cameras by -Pi/2 around x in the world frame. T is good the way it is. INPUTS: - Path of the output calibration folder - C: list of camera names - S: list of image sizes - D: list of distortion coefficients - K: list of intrinsic parameters - R (extrinsic rotation), - T (extrinsic translation) ''' for i in range(len(C)): # Transform rotation for vertical frame of reference (checkerboard vertical with OpenCap) R_mat = cv2.Rodrigues(R[i])[0] # transform in matrix R_w, T_w = world_to_camera_persp(R_mat, T[i]) # transform in world centered perspective R_w_90, T_w_90 = rotate_cam(R_w, T_w, ang_x=-np.pi/2, ang_y=0, ang_z=np.pi) # rotate cam wrt world frame R_c, T_c = world_to_camera_persp(R_w_90, T_w_90) # transform in camera centered perspective # retrieve data calib_data = {'distortion': np.append(D[i],np.array([0])), 'intrinsicMat': K[i], 'imageSize': np.expand_dims(S[i][::-1], axis=1), 'rotation': R_c, 'translation': np.expand_dims(T[i], axis=1)*1000, 'rotation_EulerAngles': cv2.Rodrigues(R_c)[0] # OpenCap calls these Euler angles but they are actually the Rodrigues vector (Euler is ambiguous) } # write pickle with open(os.path.join(output_calibration_folder, f'cam{i:02d}.pickle'), 'wb') as f_out: pickle.dump(calib_data, f_out) def calib_toml_to_opencap_func(*args): ''' Convert an OpenCV .toml calibration file to OpenCap .pickle calibration files. One file will be created for each camera. Usage: from Pose2Sim.Utilities import calib_toml_to_opencap; calib_toml_to_opencap.calib_toml_to_opencap_func(r'') OR python -m calib_toml_to_opencap -t input_toml_file OR python -m calib_toml_to_opencap -t input_toml_file -o output_calibration_folder ''' try: toml_path = os.path.realpath(args[0].get('toml_file')) # invoked with argparse if args[0]['output_calibration_folder'] == None: output_calibration_folder = os.path.dirname(toml_path) else: output_calibration_folder = os.path.realpath(args[0]['output_calibration_folder']) except: toml_path = os.path.realpath(args[0]) # invoked as a function output_calibration_folder = os.path.dirname(toml_path) C, S, D, K, R, T = read_toml(toml_path) write_opencap_pickle(output_calibration_folder, C, S, D, K, R, T) print(f'OpenCap calibration files generated at {output_calibration_folder}.\n') if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('-t', '--toml_file', required = True, help='Input OpenCV .toml calibration file') parser.add_argument('-o', '--output_calibration_folder', required = False, help='OpenCap calibration folder') args = vars(parser.parse_args()) calib_toml_to_opencap_func(args)