import numpy as np
import cv2
from matplotlib import pyplot as plt
import math
#variables à régler pour pointer vers le bon dossier
fileLocation="D:\\etudiants\\"
sequence="imgs2024-03-03_18_32_31.576393R"
fileName=fileLocation+sequence+"\\"+"CalibResult.npz"
#paramètre de la mire
dimSquare=0.012 #dimension des carrés sur la mire en mètres-> les unités de longueur sont donc en mètres
with np.load(fileName) as X:
  mtx, dist, rvecs, tvecs = [X[i] for i in ('mtx','dist','rvecs','tvecs')]

#nbimages=len(rvecs)  
#for i in range(nbimages):   #si les différentes images d'étalonnage devaient être traitées
i=0 #pour récupérer les paramètres de l'image dans laquelle la mire est posée sur le plan horizontal
matroti, jacobian=cv2.Rodrigues(rvecs[i]) #Rodrigues permet de calculer la matrice de rotation correspondante    
axis = np.float32([[0,0,0], [3*dimSquare,0,0], [0,3*dimSquare,0], [0,0,3*dimSquare]]).reshape(-1,3)
imgpts, jac = cv2.projectPoints(axis, rvecs[i], tvecs[i], mtx,None) #pour ne pas utiliser les distorsions

#matrice de changement de repère contenant les paramètres extrinsèques
cRTw=np.concatenate((matroti,tvecs[i]),axis=1)
cRTw = np.vstack((cRTw,[0,0,0,1]))
print("cRTw: "+str(cRTw))

#paramètres intrinsèques de la caméra
alphau=mtx[0][0]
alphav=mtx[1][1]
pu=mtx[0][2]
pv=mtx[1][2]