import numpy as np
import cv2
from matplotlib import pyplot as plt
import math


fileLocation="D:\\etudiants\\Vision_MILI_MAY\\"
sequence="imgs2024-03-03_18_32_31.576393R"
fileName=fileLocation+sequence+"\\"+"CalibResult.npz"
 
dimSquare=0.012
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):  
    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
    cRTw=np.concatenate((matroti,tvecs[i]),axis=1)
    cRTw = np.vstack((cRTw,[0,0,0,1]))
    print("cRTw: "+str(cRTw))
    alphau=mtx[0][0]
    alphav=mtx[1][1]
    pu=mtx[0][2]
    pv=mtx[1][2]