#B. Vandeportaele 2022 #Squelette de programme pour TP OML LIDAR #IUT GEII TOULOUSE #Importation librairie pour fabs, sqrt, cos, sin ... import math #Importation librairie pour le calcul numérique, matrices etc... et affichage de la version import numpy as np print("numpy version "+np.__version__) #Importation librairie pour l'affichage #https://matplotlib.org/stable/tutorials/introductory/pyplot.html import matplotlib.pyplot as plt ############################################################################################## # Fonction fournie pour l'affichage des informations pour une matrice np.ndarray # Il n'est pas nécessaire de regarder le code, il suffit d'uiliser la fonction affiche(...) # en lui passant en premier paramètre la matrice à afficher # le second paramètre optionnel est une chaine de caractères pour contextualiser l'affichage import inspect def affiche(mat,chaine_prefix=""): def mod_retrieve_name(var): #récupéré sur https://stackoverflow.com/questions/18425225/getting-the-name-of-a-variable-as-a-string et https://ideone.com/ym3bkD callers_local_vars = inspect.currentframe().f_back.f_back.f_locals.items() return [var_name for var_name, var_val in callers_local_vars if var_val is var] #ajoute un espace après la chaine de prefix si besoin if chaine_prefix!="" and chaine_prefix[-1]!=" ": chaine_prefix=chaine_prefix+" " matName= mod_retrieve_name(mat) if isinstance(mat,np.ndarray): print(chaine_prefix + str(matName) + " shape: "+str(mat.shape) + " ndim: "+str(mat.ndim) + " dtype: "+str(mat.dtype)+ " valeurs: \n"+str(mat)) else: print(chaine_prefix + str(matName) + " n'est pas une matrice") if isinstance(mat,str): print(mat) ############################################################################################## ############################################################################################## # Fonction fournie pour l'affichage d'une droite # Il n'est pas nécessaire de regarder ce code # définition des limites pour l'affichage par défaut: # xmin,xmax,ymin,ymax=-100,100,-100,100 def affiche_droite(da,db,dc,style='r-',xmin=-100,xmax=100,ymin=-100,ymax=100): def ci(a1,b1,c1,a2,b2,c2): A=np.array([[a1,b1], [a2,b2]],dtype = float) B=np.array([[-c1], [-c2]],dtype = float) if np.linalg.matrix_rank(A)==2: X=np.linalg.inv(A)@B else: X=np.array([[np.inf], [np.inf]],dtype = float) return X ''' plot_intersection(da,db,dc,0,1,-ymin) plot_intersection(da,db,dc,0,1,-ymax) plot_intersection(da,db,dc,1,0,-xmin) plot_intersection(da,db,dc,1,0,-xmax) ''' #def plot_intersection(a1,b1,c1,a2,b2,c2): #plt.scatter(X[0],X[1],marker='+',color='red',alpha=0.5) X1=ci(da,db,dc,0,1,-ymin) X2=ci(da,db,dc,0,1,-ymax) X3=ci(da,db,dc,1,0,-xmin) X4=ci(da,db,dc,1,0,-xmax) #cherche les 2 points les plus près du centre de la figure C=np.array([[(xmax+xmin)/2], [(ymax+ymin)/2]],dtype = float) #plt.scatter(C[0],C[1],marker='*',color='black',alpha=0.5) # https://numpy.org/doc/stable/reference/generated/numpy.linalg.norm.html XX=np.array([ [X1],[X2],[X3],[X4]]).reshape(4,2) d1=np.linalg.norm(X1-C) d2=np.linalg.norm(X2-C) d3=np.linalg.norm(X3-C) d4=np.linalg.norm(X4-C) listd=np.array([d1,d2,d3,d4],dtype = float) #récupère en 0 et 1 les indices des 2 plus petites distances idx=np.argsort(listd) #print(f"idx:{idx}") x1=XX[idx[0],0] x2=XX[idx[1],0] y1=XX[idx[0],1] y2=XX[idx[1],1] plt.plot([XX[idx[0],0], XX[idx[1],0] ], [XX[idx[0],1], XX[idx[1],1] ], style) ############################################################################################## # Fonction fournie permettant de comparer 2 droites (da,db,dc) et (das,dbs,dcs) #retourne vrai si les 2 droites sont identiques, faux sinon def compare_droite(da,db,dc,das,dbs,dcs): n=math.sqrt(da*da+db*db+dc*dc) #doit être !=0 ns=math.sqrt(das*das+dbs*dbs+dcs*dcs) #doit être !=0 print(f"obtenu: {da},{db},{dc} / souhaité: {das},{dbs},{dcs}") #normalisation da=da/n db=db/n dc=dc/n das=das/ns dbs=dbs/ns dcs=dcs/ns #si les signes de da et das sont différents if((da<0) and (das>0)) or ((da>0) and (das<0)): das=-das dbs=-dbs dcs=-dcs #print(f"après normalisation et même sens: {da},{db},{dc} / souhaité: {das},{dbs},{dcs}") erreur=math.fabs(da-das)+math.fabs(db-dbs)+math.fabs(dc-dcs) if erreur<1e-6: print(f"compare_droite: test réussi") return True else: print(f"compare_droite: test échoué") return False ############################################################################################## def determine_droite(pointsSegmentX, pointsSegmentY): #valeurs par défaut: da,db,dc=1,0,0 if len(pointsSegmentX)==2: print("Cas de l'ajustement de droite sur 2 points") x1=float(pointsSegmentX[0]) y1=float(pointsSegmentY[0]) x2=float(pointsSegmentX[1]) y2=float(pointsSegmentY[1]) if x1==x2 and y1==y2: print("Determine_droite(...) appelée avec 2 points identiques... ") return None #exit(-1) #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- else: print("Cas de l'ajustement de droite sur plus de 2 points") #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #normalisation pour avoir norme de (da,db)=1 n=math.sqrt((da*da)+(db*db)) da=da/n db=db/n dc=dc/n return da,db,dc #################################################################### def test_determine_droite(): print("test_determine_droite()") #test de la fonction determine_droite avec 2 points identiques fournis, elle doit retourner None print("test1") if determine_droite([0,0],[1,1]) is None: print(f"test réussi") else: print(f"test échoué") #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #################################################################### #------------------------------------------- def calcule_distance_droite_a_point(a,b,c,x,y): #à coder à partir de l'explication sur https://homeomath2.imingo.net/distance2.htm #vous pouvez utiliser math.fabs() et math.sqrt() d=0 #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- return d #------------------------------------------- def test_calcule_distance_droite_a_point(): print("test_calcule_distance_droite_a_point") d1=calcule_distance_droite_a_point(1,0,0,3,0) # x=0 / (3,0) print(f"d1 obtenu: {d1} / attendu {3}") d1=calcule_distance_droite_a_point(0,1,0,3,0) # y=0 / (3,0) print(f"d1 obtenu: {d1} / attendu {0}") d1=calcule_distance_droite_a_point(1,1,1,0,0) # x+y+1=0 / (0,0) print(f"d1 obtenu: {d1} / attendu {math.sqrt(2)/2}") #------------------------------------------- def calcule_intersection(a1,b1,c1,a2,b2,c2): #position X=[x,y] de l'intersection des deux droites X=np.array([[0], [0]],dtype = float) #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- return X #------------------------------------------- def plot_intersection(a1,b1,c1,a2,b2,c2): X=calcule_intersection(a1,b1,c1,a2,b2,c2) plt.scatter(X[0],X[1],marker='+',color='red',alpha=0.5) #------------------------------------------- def test_calcule_intersection(): print("test_calcule_intersection()") #test1 a1,b1,c1,a2,b2,c2=0,1,3,4,0,2 affiche_droite(a1,b1,c1,style='r-') affiche_droite(a2,b2,c2,style='r-') X=calcule_intersection(a1,b1,c1,a2,b2,c2) affiche(X) plt.scatter(X[0],X[1],marker='+',color='red',alpha=0.5) if not (X[0]==-0.5 and X[1]==-3): print("test1 échoué") #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- ################################################################ def essai_paire_ransac(scan,indice1,indice2,seuil_distance): #calcul de la droite à partir de 2 points points_segment_test_X=[scan[indice1][0],scan[indice2][0]] points_segment_test_Y=[scan[indice1][1],scan[indice2][1]] da,db,dc=determine_droite(points_segment_test_X, points_segment_test_Y) #collecte les points du scan correspondant à cette droite et les compte points_segment_X=[] points_segment_Y=[] somme_score=0 for s in scan: x=s[0] y=s[1] distance=calcule_distance_droite_a_point(da,db,dc,x,y) if distancescore_maxi: score_maxi=score indices_maxi[0]=indices[0] indices_maxi[1]=indices[1] return indices_maxi[0],indices_maxi[1] ################################################################ def test_ransac(): #liste de points 2D vide scan1=[] #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #définir une droite de votre choix dans la zone affichable (x et y compris entre -100 et +100) #modifier ces valeurs, avec b non nul: a,b,c=0.1,0.8,40 amplitude_perturbation=0 nombre_points_outliers=0 #générer 200 points échantillonnés sur cette droite dans la zone affichable (x et y compris entre -100 et +100) #et les ajouter à la liste avec scan1.append([x,y]) for n in range(-100,100,1): #TODO: générer des points sur la droite x=0 y=0 #ajout d'une perturbation au point x=x+amplitude_perturbation*float(np.random.randn(1)) y=y+amplitude_perturbation*float(np.random.randn(1)) scan1.append([x,y]) #ajout de données aberrantes: les outliers for n in range(0,nombre_points_outliers): #tirage aléatoire d'un point x=-100+200*float(np.random.rand(1)) y=-100+200*float(np.random.rand(1)) scan1.append([x,y]) scan1X=[] scan1Y=[] for s in scan1: scan1X.append(s[0]) scan1Y.append(s[1]) if len(scan1)>=2: plt.scatter(scan1X,scan1Y,marker='+',c="blue",alpha=0.5) i1,i2=ransac(scan1,10) da,db,dc=determine_droite([scan1[i1][0],scan1[i2][0]],[scan1[i1][1],scan1[i2][1]]) affiche_droite(da,db,dc,style='r-') compare_droite(da,db,dc,a,b,c) ################################################################ #Préparation des données pour egomotion ################################################################ #Importation d'un fichier python contenant les données d'un scan: import scan000001 as sc #copie des données importées vers une variable globale utilisable par ce programme scan_polaire=sc.donnees nb_mesures=len(scan_polaire) print("Le scan contient: "+str(nb_mesures)+ " mesures") #conversion des données de coordonnées polaires vers coordonnées cartésiennes scan1=[] for m in scan_polaire: if m[1]!=0: #les données avec mesure de distance à 0 sont des trous x=m[1]*math.cos(m[0]*math.pi/180.) y=m[1]*math.sin(m[0]*math.pi/180.) scan1.append([x,y]) #génère des données que percevrait le capteur après deplacement theta=0.789 dx=841.3 dy=-521.0 #matrice de changement de repère en coordonnées homogènes: M_2_1=np.array( [ [math.cos(theta), -math.sin(theta), dx] , [math.sin(theta), math.cos(theta) , dy] , [0 , 0 , 1] ]) affiche(M_2_1) #application de la transformation au scan1 scan2=[] for m in scan_polaire: if m[1]!=0: #les données avec mesure de distance à 0 sont des trous #TODO: verifier le sens de rotation du capteur (peut être faut il - sur l'angle pour convertir en cartésien) x=m[1]*math.cos(m[0]*math.pi/180.) y=m[1]*math.sin(m[0]*math.pi/180.) p1=np.array( [x , y , 1 ]).reshape(3,1) p2=M_2_1@p1 #affiche(p2) #ajout d'un bruit gaussien d'écart type 3 sur les coordonnées des points A = 3* np.random.randn(2).reshape(2,1) scan2.append([float(p2[0]+A[0]), float(p2[1]+A[1]) ]) #choix des indices de 2 points pour l'estimation des deux droites dans les deux scans #ici les indices sont les mêmes pour les deux scans, i1=295 #point 1 droite 1 i2=405 #point 2 droite 1 i3=103 #point 1 droite 2 i4=455 #point 2 droite 2 ################################################################ def affiche_scan(scan1,scan2=None): global i1,i2,i3,i4 scan1X=[] scan1Y=[] scan2X=[] scan2Y=[] scan1et2X=[] scan1et2Y=[] for s in scan1: scan1X.append(s[0]) scan1Y.append(s[1]) scan1et2X.append(s[0]) scan1et2Y.append(s[1]) if scan2 != None: for s in scan2: scan2X.append(s[0]) scan2Y.append(s[1]) scan1et2X.append(s[0]) scan1et2Y.append(s[1]) #détermination des limites pour l'affichage, dépassement de 10% de chaque coté xmin=min(scan1et2X)-(max(scan1et2X)-min(scan1et2X))*0.1 xmax=max(scan1et2X)+(max(scan1et2X)-min(scan1et2X))*0.1 ymin=min(scan1et2Y)-(max(scan1et2Y)-min(scan1et2Y))*0.1 ymax=max(scan1et2Y)+(max(scan1et2Y)-min(scan1et2Y))*0.1 fig = plt.figure(figsize=(8, 6)) axes = plt.axes() axes.set_xlim([xmin,xmax]) axes.set_ylim([ymin,ymax]) axes.axis('equal') #https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.scatter.html plt.scatter(scan1X,scan1Y,marker='+',c="blue",alpha=0.5) plt.scatter(scan2X,scan2Y,marker='x',c="green",alpha=0.5) #trace les droites detectées dans les 2 scans da11,db11,dc11=determine_droite( [scan1X[i1],scan1X[i2]] , [scan1Y[i1],scan1Y[i2]] ) da12,db12,dc12=determine_droite( [scan1X[i3],scan1X[i4]] , [scan1Y[i3],scan1Y[i4]] ) affiche_droite(da11,db11,dc11,'b-',xmin,xmax,ymin,ymax) affiche_droite(da12,db12,dc12,'b-',xmin,xmax,ymin,ymax) if scan2 is not None: da21,db21,dc21=determine_droite( [scan2X[i1],scan2X[i2]] , [scan2Y[i1],scan2Y[i2]] ) da22,db22,dc22=determine_droite( [scan2X[i3],scan2X[i4]] , [scan2Y[i3],scan2Y[i4]] ) affiche_droite(da21,db21,dc21,'g-',xmin,xmax,ymin,ymax) affiche_droite(da22,db22,dc22,'g-',xmin,xmax,ymin,ymax) pointsSegmentX=[scan1X[i1],scan1X[i2]] pointsSegmentY=[scan1Y[i1],scan1Y[i2]] plt.xlabel('X') plt.ylabel('Y') plt.show() ################################################################ def ego_motion(scan1,scan2): global i1,i2,i3,i4 #paramètres de la transformation à calculer, valeurs par défaut theta, dx, dy=0,0,0 M_2_1=np.array( [ [math.cos(theta), -math.sin(theta), dx] , [math.sin(theta), math.cos(theta) , dy] , [0 , 0 , 1] ]) M_1_2=np.linalg.inv(M_2_1) scan1X=[] scan1Y=[] scan2X=[] scan2Y=[] for s in scan1: scan1X.append(s[0]) scan1Y.append(s[1]) for s in scan2: scan2X.append(s[0]) scan2Y.append(s[1]) da11,db11,dc11=determine_droite( [scan1X[i1],scan1X[i2]] , [scan1Y[i1],scan1Y[i2]] ) da12,db12,dc12=determine_droite( [scan1X[i3],scan1X[i4]] , [scan1Y[i3],scan1Y[i4]] ) da21,db21,dc21=determine_droite( [scan2X[i1],scan2X[i2]] , [scan2Y[i1],scan2Y[i2]] ) da22,db22,dc22=determine_droite( [scan2X[i3],scan2X[i4]] , [scan2Y[i3],scan2Y[i4]] ) #intersection de D1 et D2 dans scan1 I1=calcule_intersection(da11,db11,dc11,da12,db12,dc12) X1=float(I1[0]) Y1=float(I1[1]) #intersection de D1 et D2 dans scan2 I2=calcule_intersection(da21,db21,dc21,da22,db22,dc22) X2=float(I2[0]) Y2=float(I2[1]) #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #affichage des paramètres calculés print(f"dx calculé:{dx} , dy calculé:{dy} , theta calculé:{theta} ") #applique la transformation M_1_2 au scan2 et retourne le scan2 transformé scan2_transforme=[] for s in scan2: scan2X.append(s[0]) scan2Y.append(s[1]) x=s[0] y=s[1] p2=np.array( [x , y , 1 ]).reshape(3,1) p1=M_1_2@p2 scan2_transforme.append([float(p1[0]), float(p1[1]) ]) return scan2_transforme ################################################################ ######################################### def affichage_tests_etudiant(): fig = plt.figure(figsize=(8, 6)) axes = plt.axes() #axes.set_xlim([xmin,xmax]) #axes.set_ylim([ymin,ymax]) axes.axis('equal') #https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.scatter.html #exemple de tracé de droite en rouge: affiche_droite(1,0,1,'c-') #pour effacer la figure, vous pouvez utiliser: #plt.clf() #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #------------------------------------------------------------------- # FIN DU CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- plt.xlabel('X') plt.ylabel('Y') plt.show() exit() #arrete le programme à la fin de la fonction ######################################### affichage_tests_etudiant() #le code suivant n'est executé que si affichage_tests_etudiant() n'est pas exécuté: #modifie scan2 après estimation du changement de pose scan2=ego_motion(scan1,scan2) affiche_scan(scan1,scan2)