#/usr/bin/python3 import numpy as np import cv2 import glob filename1="imaobj1_rect.jpg" filename2="imaobj2_rect.jpg" img1 = cv2.imread(filename1) if(len(img1.shape)<3): img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR) img2 = cv2.imread(filename2) if(len(img2.shape)<3): img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR) h, w = img1.shape[:2] print("h:"+str(h)) print("w:"+str(w)) #------------------ def projeterPoint(M,iCw): #print("M:"+str(M)) M = np.append(M, 1) #print("M:"+str(M)) m=iCw@M #deshomogénéisation mu=m[0]/m[2] mv=m[1]/m[2] #arrondi pour avoir des positions entières mu=int(round(mu,0)) mv=int(round(mv,0)) #print("mu:"+str(mu)) #print("mv:"+str(mv)) return (mu,mv) #------------------ alphau=2149.349810656287900 alphav=2146.276553276586100 pu=412.626457723086160 pv=355.723365602020240 rvec1 = np.float32([ -3.125480e-001 , -2.316874e+000 , 1.048207e-001 ]) tvec1 = 0.001 * np.float32([ [4.813190e+001] , [-1.258122e+002] , [1.066916e+003] ]) rvec2 = np.float32([ 6.592006e-001 , -2.262313e+000 , -3.064294e-001]) tvec2 = 0.001 * np.float32([ [2.557518e+001] , [-6.888417e+001] , [1.091155e+003] ]) matR1,jac1=cv2.Rodrigues(rvec1) print("matR1:"+str(matR1)) matR2,jac2=cv2.Rodrigues(rvec2) print("matR2:"+str(matR2)) #affichage comme TD synthese cRT1w=np.hstack((matR1,tvec1)) cRT1w = np.vstack((cRT1w,[0,0,0,1])) cRT2w=np.hstack((matR2,tvec2)) cRT2w = np.vstack((cRT2w,[0,0,0,1])) print("cRTw: "+str(cRT1w)) #print("mtx: "+str(mtx)) iCc=np.float32([[alphau,0,pu,0],[0,alphav,pv,0],[0,0,1,0]]) print("iCc:"+str(iCc)) iC1w=iCc@cRT1w print("iC1w:"+str(iC1w)) iC2w=iCc@cRT2w print("iC2w:"+str(iC2w)) #--------------------------- #TODO: afficher un repère de 10cm de coté sur chaque image sur chaque image #--------------------------- dimRepere=0.10 axis = np.float32([[0,0,0], [dimRepere,0,0], [0,dimRepere,0], [0,0,dimRepere]]).reshape(-1,3) img1 = cv2.line(img1, projeterPoint(axis[0],iC1w),projeterPoint(axis[1],iC1w), (0,0,255), 1) img1 = cv2.line(img1, projeterPoint(axis[0],iC1w),projeterPoint(axis[2],iC1w), (0,255,0), 1) img1 = cv2.line(img1, projeterPoint(axis[0],iC1w),projeterPoint(axis[3],iC1w), (255,0,0), 1) img2 = cv2.line(img2, projeterPoint(axis[0],iC2w),projeterPoint(axis[1],iC2w), (0,0,255), 1) img2 = cv2.line(img2, projeterPoint(axis[0],iC2w),projeterPoint(axis[2],iC2w), (0,255,0), 1) img2 = cv2.line(img2, projeterPoint(axis[0],iC2w),projeterPoint(axis[3],iC2w), (255,0,0), 1) #liste des points pour les 2 image, mu puis mv listem1 = np.int_([[490,96], [329,83] , [510,119] , [359,107], [493,156],[339,151],[525,189],[373,180],[507,230], [353,223],[534,253],[385,250],[514,302],[361,304], [544,333],[393,336],[523,377],[369,383], [400,252], # ce point n'est pas observé dans l'image [254,264]]) listem2 = np.int_([[448,194], [290,184] , [450,233] , [297,228], [420,249],[265,243],[427,297],[272,294],[391,311], [241,308],[399,349],[250,354],[366,364],[212,367], [371,408],[216,417],[338,416],[179,420], [316,263], # ce point n'est pas observé dans l'image [162,228]]) if listem2.shape[0]!=listem1.shape[0]: print("les 2 listes de points doivent avoir le même nombre de points") exit(-1) listeM=np.zeros((listem2.shape[0],3), np.float32) #valeur par defaut pour le point 3D reconstruit X = np.zeros((3,1), np.float32) Xz0 = np.zeros((3,1), np.float32) for i,(mu,mv) in enumerate(listem1): img1 = cv2.circle(img1,(mu,mv), 3,(255,0,255), -1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(img1,str(i),(mu+10,mv), font, 1,(255,0,255),2,cv2.LINE_AA) #récupère le point correspondant dans l'image 2 mu2=listem2[i][0] mv2=listem2[i][1] img2 = cv2.circle(img2,(mu2,mv2), 3,(255,0,255), -1) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(img2,str(i),(mu2+10,mv2), font, 1,(255,0,255),2,cv2.LINE_AA) #--------------------------- #TODO: Remplir listeM[i][:] avec les coordonnées 3D du point i #--------------------------- monoculaire=True if monoculaire: #monoculaire if i%2==0: #point 3D dans le plan de la mire z=0 A = np.zeros((3,3), np.float32) B = np.zeros((3,1), np.float32) n=0 A[n][0]=iC1w[2][0]*mu-iC1w[0][0] A[n][1]=iC1w[2][1]*mu-iC1w[0][1] A[n][2]=iC1w[2][2]*mu-iC1w[0][2] B[n]= -iC1w[2][3]*mu+iC1w[0][3] n=1 A[n][0]=iC1w[2][0]*mv-iC1w[1][0] A[n][1]=iC1w[2][1]*mv-iC1w[1][1] A[n][2]=iC1w[2][2]*mv-iC1w[1][2] B[n]= -iC1w[2][3]*mv+iC1w[1][3] n=2 A[n][0]=0 A[n][1]=0 A[n][2]=1 B[n]= 0 X=np.linalg.inv(A)@B Xz0=X.copy() else: #point 3D à la verticale du point précédent #A = np.zeros((4,3), np.float32) #B = np.zeros((4,1), np.float32) A = np.zeros((3,3), np.float32) B = np.zeros((3,1), np.float32) n=0 A[n][0]=iC1w[2][0]*mu-iC1w[0][0] A[n][1]=iC1w[2][1]*mu-iC1w[0][1] A[n][2]=iC1w[2][2]*mu-iC1w[0][2] B[n]= -iC1w[2][3]*mu+iC1w[0][3] n=1 A[n][0]=iC1w[2][0]*mv-iC1w[1][0] A[n][1]=iC1w[2][1]*mv-iC1w[1][1] A[n][2]=iC1w[2][2]*mv-iC1w[1][2] B[n]= -iC1w[2][3]*mv+iC1w[1][3] #Solution utilisant une altitude connue pour les points de la face supérieure #n=2 #A[n][0]=0 #A[n][1]=0 #A[n][2]=1 #B[n]= 0.1 #10cm #Solution utilisant le point d'indice précédent dans le plan z=0 pour déterminer une des coordonnées (x ici) du point courant n=2 A[n][0]=1 A[n][1]=0 A[n][2]=0 B[n]= Xz0[0] #ou bien #A[n][0]=0 #A[n][1]=1 #A[n][2]=0 #B[n]= Xz0[1] print("A:" +str(A)) print("B:" +str(B)) #X= (np.linalg.inv(A.transpose() @ A) @ A.transpose() ) @ B X=np.linalg.inv(A)@B else: #stereo A = np.zeros((4,3), np.float32) B = np.zeros((4,1), np.float32) n=0 A[n][0]=iC1w[2][0]*mu-iC1w[0][0] A[n][1]=iC1w[2][1]*mu-iC1w[0][1] A[n][2]=iC1w[2][2]*mu-iC1w[0][2] B[n]= -iC1w[2][3]*mu+iC1w[0][3] n=1 A[n][0]=iC1w[2][0]*mv-iC1w[1][0] A[n][1]=iC1w[2][1]*mv-iC1w[1][1] A[n][2]=iC1w[2][2]*mv-iC1w[1][2] B[n]= -iC1w[2][3]*mv+iC1w[1][3] n=2 A[n][0]=iC2w[2][0]*mu2-iC2w[0][0] A[n][1]=iC2w[2][1]*mu2-iC2w[0][1] A[n][2]=iC2w[2][2]*mu2-iC2w[0][2] B[n]= -iC2w[2][3]*mu2+iC2w[0][3] n=3 A[n][0]=iC2w[2][0]*mv2-iC2w[1][0] A[n][1]=iC2w[2][1]*mv2-iC2w[1][1] A[n][2]=iC2w[2][2]*mv2-iC2w[1][2] B[n]= -iC2w[2][3]*mv2+iC2w[1][3] X= (np.linalg.inv(A.transpose() @ A) @ A.transpose() ) @ B print("X( "+str(i)+" ): "+str(X)) listeM[i][:]=X.transpose() #image composée à partir des 2 images 1 et 2 imagecomposee = np.hstack((img1,img2)) filenameout="imaobj_rect-out.jpg" cv2.imwrite(filenameout,imagecomposee) cv2.imshow('dst',imagecomposee) cv2.waitKey(-1) cv2.destroyAllWindows() #----------------------------------------------- #matplot 3D #https://www.geeksforgeeks.org/three-dimensional-plotting-in-python-using-matplotlib/ #mettre à True pour activer le rendu 3D après fermeture de la fenêtre opencv if False: # importing mplot3d toolkits from mpl_toolkits import mplot3d import numpy as np import matplotlib.pyplot as plt fig = plt.figure() # syntax for 3-D projection ax = plt.axes(projection ='3d') # defining axes x = listeM[:,0] y = listeM[:,1] z = listeM[:,2] ax.scatter(x, y, z) ax.axis('equal') # syntax for plotting ax.set_title('3d Scatter plot geeks for geeks') plt.show()