TODO suivant: Linux+git + validation et commit à chaque exercice ===Infos prof=== F5 debug caster en float X1,Y1 X2,Y2 question: est ce qu'il faut une note de TP? clic droit sur variable, "evaluate in debug console", puis dans la fenêtre "DEBUG Console" taper ce que l'on souhaite ===notes d'installation=== en E002: "C:\Program Files\Microsoft VS Code\Visual Studio Code.cmd" python 3.9.7 accessible dans le PATH conventions de nommages à utiliser: https://realpython.com/python-pep8/ et https://peps.python.org/pep-0008/#function-and-variable-names et https://www.techversantinfotech.com/python-naming-conventions-points-you-should-know/ $S=\begin{bmatrix} 0.00 & 3.00 & 6.00 & 9.00\\ 12.00 & 15.00 & 18.00 & 21.00\\ 24.00 & 27.00 & 30.00 & 33.00 \end{bmatrix}$ =====TP OML AII LIDAR===== ====Contexte===== Dans ce TP de 4h, nous allons illustrer l'usage du calcul matriciel pour traiter des données issues d'un télémètre laser (LIDAR) à balayage. Ce type de capteur réalise des mesures de distance grâce à la durée nécessaire pour que la lumière effectue le trajet aller/retour du capteur à l'obstacle le plus proche. Le capteur de distance est lui même mis en rotation autour d'un axe pour réaliser un balayage angulaire afin d'effecteur des mesures dans différentes directions et former ainsi un nuage de points 2D. Le capteur est visible sur la figure suivante: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/rplidar_schema.png}} L'image suivante montre le capteur placé dans un environnement d'intérieur de bâtiment ainsi que les trajets optiques correspondant aux mesures dans différentes directions: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/scan1.jpg}} Chaque mesure de distance dans la scène est associée à une mesure angulaire, les deux infomations formant des coordonnées polaires. L'image suivante représente l'environnement tel qu'il est perçu par le capteur lors d'une rotation d'un tour, sous forme d'un nuage de points 2D affichés en coordonnées cartésiennes, centrés sur le capteur: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/scan2.jpg}} Durant ce TP, vous devrez: - prendre en main une librairie de calcul matriciel appelée numpy - détecter des segments de droites dans les données issues du télémètre - déterminer le déplacement d'un capteur entre deux captures différentes effectuées par le capteur Cette figure représente les données d'un scan ainsi que 2 droites détectées: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/scan_simple.png}} Cette figure représente les données de deux scans ainsi que 2 droites détectées dans chaque scan: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/scan_decale.png}} Cette figure représente les données des mêmes scans après recalage grâce à la mise en correspondances des 2 paires de droites: {{https://bvdp.inetdoc.net/files/iut/tp_oml1/images/scan_recale.png}} ====Documentation de numpy==== Ouvrir dans un autre onglet la documentation suivante: https://bvdp.inetdoc.net/files/iut/tp_oml1/numpy-user-1.23.0.pdf ====Démarrage du projet=== ===Installation des paquets PIP=== Si nécessaire, installer les paquets pip suivants: Touche Windows+R cmd pip install numpy pip install matplotlib ===Rapatriement du projet et installation des extensions Python pour Visual Studio Code=== Créer un dossier tpomllidar dans votre dossier R:\etu... Télécharger les 2 fichiers squelettes python suivants dans votre dossier R:\etu...\tpomllidar\ (en faisant clic droit sur le nom du fichier et "enregistrer le lien sous"...): donnees=[[0.12085 , 1129 ] ,[0.76355 , 1101 ] ,[1.54358 , 1092 ] ,[1.82922 , 0 ] ,[2.83997 , 1063 ] ,[3.48267 , 1051 ] ,[4.11987 , 1044 ] ,[4.77905 , 1063 ] ,[5.29541 , 1081 ] ,[5.93262 , 1078 ] ,[6.5918 , 1064 ] ,[7.35535 , 1057 ] ,[7.88818 , 1045 ] ,[8.65173 , 1035 ] ,[9.29443 , 1029 ] ,[9.94812 , 1023 ] ,[10.5908 , 1001 ] ,[11.2335 , 990 ] ,[12.0135 , 981 ] ,[12.5299 , 974 ] ,[13.3099 , 969 ] ,[13.9526 , 965 ] ,[14.5898 , 958 ] ,[15.249 , 951 ] ,[15.8862 , 948 ] ,[16.5289 , 940 ] ,[17.1826 , 936 ] ,[17.8253 , 929 ] ,[18.479 , 939 ] ,[19.1217 , 941 ] ,[19.7809 , 936 ] ,[20.4181 , 931 ] ,[21.0773 , 930 ] ,[21.7145 , 955 ] ,[22.2473 , 981 ] ,[22.89 , 1015 ] ,[23.5437 , 1049 ] ,[24.0765 , 1119 ] ,[24.7137 , 1072 ] ,[25.3729 , 1068 ] ,[26.0156 , 1069 ] ,[26.7957 , 1064 ] ,[27.4329 , 1061 ] ,[27.9657 , 1059 ] ,[28.6249 , 1057 ] ,[29.5148 , 946 ] ,[30.1685 , 992 ] ,[30.6848 , 991 ] ,[31.5912 , 885 ] ,[32.1075 , 883 ] ,[32.7612 , 904 ] ,[33.4039 , 973 ] ,[33.8104 , 1064 ] ,[34.4641 , 1082 ] ,[35.1068 , 1081 ] ,[35.7605 , 1079 ] ,[36.4032 , 1080 ] ,[37.0624 , 1076 ] ,[37.6996 , 1076 ] ,[38.3588 , 1078 ] ,[39.0125 , 1075 ] ,[39.6552 , 1075 ] ,[40.4352 , 1076 ] ,[41.0779 , 1073 ] ,[41.8579 , 878 ] ,[42.6215 , 0 ] ,[42.6215 , 879 ] ,[43.7915 , 893 ] ,[44.4507 , 888 ] ,[45.2142 , 874 ] ,[45.8734 , 875 ] ,[46.3898 , 881 ] ,[47.1698 , 875 ] ,[47.8125 , 877 ] ,[48.4442 , 0 ] ,[48.4662 , 879 ] ,[49.0924 , 0 ] ,[49.7406 , 0 ] ,[50.9326 , 888 ] ,[51.5918 , 899 ] ,[52.229 , 926 ] ,[52.8882 , 938 ] ,[53.5309 , 903 ] ,[54.3109 , 905 ] ,[54.8273 , 907 ] ,[55.481 , 908 ] ,[56.1237 , 917 ] ,[56.217 , 0 ] ,[56.8652 , 0 ] ,[57.5134 , 0 ] ,[58.1561 , 0 ] ,[58.8043 , 0 ] ,[59.4525 , 0 ] ,[60.1007 , 0 ] ,[60.7489 , 0 ] ,[61.3971 , 0 ] ,[62.0453 , 0 ] ,[62.6935 , 0 ] ,[63.3362 , 0 ] ,[63.9844 , 0 ] ,[64.6326 , 0 ] ,[65.2808 , 0 ] ,[65.929 , 0 ] ,[66.5771 , 0 ] ,[67.2253 , 0 ] ,[67.868 , 0 ] ,[68.5162 , 0 ] ,[69.1644 , 0 ] ,[69.8126 , 0 ] ,[70.4608 , 0 ] ,[71.109 , 0 ] ,[71.7572 , 0 ] ,[72.4054 , 0 ] ,[73.0481 , 0 ] ,[73.6963 , 0 ] ,[74.3445 , 0 ] ,[74.9927 , 0 ] ,[75.6409 , 0 ] ,[76.2891 , 0 ] ,[76.9373 , 0 ] ,[77.5854 , 0 ] ,[78.2281 , 0 ] ,[78.8763 , 0 ] ,[79.5245 , 0 ] ,[80.1727 , 0 ] ,[80.8209 , 0 ] ,[81.4691 , 0 ] ,[82.1173 , 0 ] ,[82.7655 , 0 ] ,[83.4082 , 0 ] ,[84.0564 , 0 ] ,[84.7046 , 0 ] ,[85.3528 , 0 ] ,[86.001 , 0 ] ,[86.6492 , 0 ] ,[87.2974 , 0 ] ,[87.9401 , 0 ] ,[88.5883 , 0 ] ,[89.2365 , 0 ] ,[89.8846 , 0 ] ,[90.5328 , 0 ] ,[91.181 , 0 ] ,[91.8292 , 0 ] ,[92.4774 , 0 ] ,[93.1201 , 0 ] ,[93.7683 , 0 ] ,[94.4165 , 0 ] ,[95.0647 , 0 ] ,[95.7129 , 0 ] ,[96.3611 , 0 ] ,[97.0093 , 0 ] ,[97.6575 , 0 ] ,[98.3002 , 0 ] ,[98.9484 , 0 ] ,[99.5966 , 0 ] ,[100.245 , 0 ] ,[100.871 , 1580 ] ,[101.514 , 1609 ] ,[102.03 , 1652 ] ,[102.794 , 1668 ] ,[103.326 , 1664 ] ,[103.964 , 1656 ] ,[104.606 , 1688 ] ,[105.249 , 1714 ] ,[105.903 , 1747 ] ,[106.545 , 1779 ] ,[107.183 , 1828 ] ,[107.842 , 1888 ] ,[108.479 , 1877 ] ,[109.308 , 0 ] ,[109.638 , 1976 ] ,[110.605 , 0 ] ,[110.808 , 2370 ] ,[111.451 , 2371 ] ,[112.104 , 2407 ] ,[112.747 , 2408 ] ,[113.39 , 2471 ] ,[114.027 , 2577 ] ,[114.686 , 2707 ] ,[115.323 , 2716 ] ,[115.966 , 2737 ] ,[116.62 , 2959 ] ,[117.136 , 3033 ] ,[117.779 , 3163 ] ,[118.422 , 3334 ] ,[119.075 , 3541 ] ,[119.718 , 3568 ] ,[120.355 , 3555 ] ,[121.014 , 3591 ] ,[121.652 , 3579 ] ,[122.294 , 3565 ] ,[122.937 , 3560 ] ,[123.574 , 3558 ] ,[124.233 , 3553 ] ,[124.871 , 3684 ] ,[125.513 , 3929 ] ,[126.156 , 3949 ] ,[126.793 , 3828 ] ,[127.452 , 3549 ] ,[128.09 , 3521 ] ,[128.732 , 3525 ] ,[129.375 , 3516 ] ,[130.029 , 3536 ] ,[130.671 , 3350 ] ,[131.309 , 3096 ] ,[131.951 , 3080 ] ,[132.589 , 3056 ] ,[133.374 , 2325 ] ,[134.138 , 2329 ] ,[134.78 , 2332 ] ,[135.417 , 2202 ] ,[136.077 , 2199 ] ,[136.714 , 2204 ] ,[137.357 , 2203 ] ,[137.999 , 2206 ] ,[138.636 , 2214 ] ,[139.296 , 2217 ] ,[139.933 , 2218 ] ,[140.449 , 2320 ] ,[141.092 , 2369 ] ,[141.746 , 2392 ] ,[142.388 , 2419 ] ,[143.031 , 2439 ] ,[143.685 , 2474 ] ,[144.327 , 2508 ] ,[144.981 , 2527 ] ,[145.624 , 2575 ] ,[146.865 , 0 ] ,[146.92 , 2679 ] ,[148.156 , 0 ] ,[148.804 , 0 ] ,[149.453 , 0 ] ,[149.639 , 2378 ] ,[150.293 , 2249 ] ,[151.397 , 0 ] ,[152.045 , 0 ] ,[152.693 , 0 ] ,[153.336 , 0 ] ,[153.984 , 0 ] ,[154.292 , 1897 ] ,[154.825 , 1907 ] ,[155.929 , 0 ] ,[156.121 , 1929 ] ,[156.764 , 1937 ] ,[157.418 , 1945 ] ,[158.307 , 1636 ] ,[158.967 , 1641 ] ,[159.609 , 1645 ] ,[160.01 , 1990 ] ,[160.653 , 2012 ] ,[161.312 , 2023 ] ,[161.949 , 2045 ] ,[162.609 , 2056 ] ,[163.499 , 1617 ] ,[164.015 , 1619 ] ,[164.542 , 2094 ] ,[165.185 , 2113 ] ,[165.828 , 2096 ] ,[166.608 , 1805 ] ,[167.245 , 1796 ] ,[167.778 , 1992 ] ,[168.42 , 1970 ] ,[169.184 , 1940 ] ,[169.717 , 1924 ] ,[170.48 , 1888 ] ,[171.123 , 1869 ] ,[171.777 , 1846 ] ,[172.419 , 1822 ] ,[173.073 , 1795 ] ,[173.716 , 1779 ] ,[174.359 , 1760 ] ,[175.012 , 1733 ] ,[175.655 , 1714 ] ,[176.292 , 1698 ] ,[176.951 , 1681 ] ,[177.715 , 1659 ] ,[178.374 , 1646 ] ,[179.011 , 1622 ] ,[179.654 , 1615 ] ,[180.308 , 1599 ] ,[180.95 , 1585 ] ,[181.593 , 1570 ] ,[182.247 , 1615 ] ,[182.637 , 2060 ] ,[183.296 , 2079 ] ,[183.933 , 2107 ] ,[184.576 , 2137 ] ,[185.356 , 1716 ] ,[185.999 , 1745 ] ,[186.652 , 1763 ] ,[187.295 , 1797 ] ,[187.949 , 1829 ] ,[188.591 , 1855 ] ,[189.124 , 1939 ] ,[189.761 , 2010 ] ,[190.421 , 2096 ] ,[191.058 , 2089 ] ,[191.717 , 2148 ] ,[192.354 , 2224 ] ,[193.013 , 2207 ] ,[193.656 , 2192 ] ,[194.293 , 2240 ] ,[195.342 , 3797 ] ,[195.425 , 0 ] ,[195.996 , 3190 ] ,[196.639 , 3126 ] ,[197.292 , 3076 ] ,[197.935 , 3039 ] ,[198.589 , 3002 ] ,[199.232 , 2954 ] ,[199.885 , 2924 ] ,[200.528 , 2887 ] ,[201.187 , 2843 ] ,[201.951 , 2803 ] ,[202.604 , 2778 ] ,[203.247 , 2751 ] ,[203.906 , 2720 ] ,[204.543 , 2675 ] ,[205.186 , 2656 ] ,[205.84 , 2621 ] ,[206.483 , 2618 ] ,[207.015 , 3059 ] ,[207.653 , 3055 ] ,[208.312 , 3059 ] ,[208.949 , 3055 ] ,[209.592 , 2924 ] ,[210.245 , 3041 ] ,[210.888 , 3020 ] ,[211.542 , 3016 ] ,[212.184 , 3009 ] ,[212.844 , 3015 ] ,[213.481 , 2930 ] ,[214.261 , 2751 ] ,[214.904 , 2744 ] ,[215.546 , 2744 ] ,[216.2 , 2725 ] ,[216.843 , 2742 ] ,[217.496 , 2743 ] ,[218.139 , 2745 ] ,[218.793 , 2741 ] ,[219.435 , 2744 ] ,[220.078 , 2626 ] ,[220.732 , 2782 ] ,[221.902 , 2906 ] ,[221.973 , 0 ] ,[222.545 , 2847 ] ,[223.325 , 2677 ] ,[223.967 , 2628 ] ,[224.621 , 2753 ] ,[225.137 , 2984 ] ,[225.797 , 3013 ] ,[226.434 , 3007 ] ,[227.093 , 3027 ] ,[227.73 , 3016 ] ,[228.389 , 3027 ] ,[229.026 , 3022 ] ,[229.686 , 3050 ] ,[230.323 , 3047 ] ,[230.982 , 3048 ] ,[231.625 , 3083 ] ,[232.278 , 2929 ] ,[233.042 , 2749 ] ,[233.701 , 2646 ] ,[234.218 , 3624 ] ,[234.871 , 3610 ] ,[235.53 , 3605 ] ,[236.168 , 3606 ] ,[236.827 , 3582 ] ,[237.464 , 3587 ] ,[238.123 , 3575 ] ,[238.76 , 3586 ] ,[239.42 , 3608 ] ,[240.062 , 3625 ] ,[240.716 , 3647 ] ,[241.359 , 3657 ] ,[242.012 , 3679 ] ,[242.655 , 3704 ] ,[243.309 , 3707 ] ,[243.951 , 3722 ] ,[244.605 , 3740 ] ,[245.264 , 3758 ] ,[245.901 , 3768 ] ,[246.561 , 3780 ] ,[247.198 , 3736 ] ,[247.857 , 3688 ] ,[248.5 , 3627 ] ,[249.153 , 3587 ] ,[249.796 , 3535 ] ,[250.45 , 3481 ] ,[251.109 , 3444 ] ,[251.746 , 3384 ] ,[252.405 , 3318 ] ,[253.043 , 3276 ] ,[253.702 , 3245 ] ,[254.339 , 3191 ] ,[254.998 , 3164 ] ,[255.652 , 3128 ] ,[256.295 , 3086 ] ,[256.948 , 3052 ] ,[258.228 , 0 ] ,[258.245 , 2990 ] ,[258.887 , 2948 ] ,[259.547 , 2912 ] ,[260.184 , 2887 ] ,[260.964 , 2864 ] ,[261.623 , 2784 ] ,[262.266 , 2758 ] ,[262.919 , 2741 ] ,[263.562 , 2691 ] ,[264.216 , 2668 ] ,[264.858 , 2639 ] ,[265.512 , 2605 ] ,[266.171 , 2587 ] ,[266.808 , 2563 ] ,[267.451 , 2522 ] ,[268.088 , 2487 ] ,[268.748 , 2465 ] ,[269.39 , 2437 ] ,[270.027 , 2429 ] ,[270.687 , 2397 ] ,[271.324 , 2369 ] ,[271.967 , 2343 ] ,[272.62 , 2315 ] ,[273.263 , 2295 ] ,[274.026 , 2278 ] ,[274.559 , 2265 ] ,[275.323 , 2244 ] ,[275.966 , 2231 ] ,[276.625 , 2211 ] ,[277.262 , 2190 ] ,[277.905 , 2181 ] ,[278.542 , 2170 ] ,[279.201 , 2151 ] ,[279.844 , 2134 ] ,[280.481 , 2120 ] ,[281.14 , 2100 ] ,[281.777 , 2088 ] ,[282.42 , 2077 ] ,[283.074 , 2062 ] ,[283.716 , 2051 ] ,[284.359 , 2034 ] ,[285.013 , 2028 ] ,[285.656 , 2024 ] ,[286.293 , 2005 ] ,[286.952 , 1997 ] ,[287.589 , 1994 ] ,[288.232 , 1978 ] ,[288.885 , 1964 ] ,[289.528 , 1964 ] ,[290.187 , 1950 ] ,[290.825 , 1946 ] ,[291.605 , 1932 ] ,[292.121 , 1927 ] ,[292.78 , 1921 ] ,[293.544 , 1914 ] ,[294.203 , 1906 ] ,[294.714 , 1894 ] ,[295.373 , 1890 ] ,[296.016 , 1888 ] ,[296.796 , 1883 ] ,[297.433 , 1880 ] ,[298.076 , 1868 ] ,[298.729 , 1867 ] ,[299.372 , 1861 ] ,[299.905 , 1851 ] ,[300.542 , 1851 ] ,[301.328 , 1850 ] ,[301.965 , 1846 ] ,[302.624 , 1842 ] ,[303.261 , 1840 ] ,[303.92 , 1835 ] ,[304.557 , 1830 ] ,[305.217 , 1830 ] ,[305.859 , 1827 ] ,[306.513 , 1826 ] ,[307.156 , 1825 ] ,[307.809 , 1825 ] ,[308.452 , 1823 ] ,[309.106 , 1823 ] ,[309.749 , 1821 ] ,[310.402 , 1823 ] ,[311.045 , 1821 ] ,[311.699 , 1821 ] ,[312.358 , 1823 ] ,[312.995 , 1823 ] ,[313.654 , 1825 ] ,[314.297 , 1822 ] ,[314.951 , 1824 ] ,[315.604 , 1832 ] ,[316.247 , 1830 ] ,[316.906 , 1828 ] ,[317.543 , 1837 ] ,[318.203 , 1836 ] ,[318.856 , 1837 ] ,[319.499 , 1842 ] ,[320.153 , 1843 ] ,[320.795 , 1844 ] ,[321.449 , 1849 ] ,[321.982 , 1851 ] ,[322.745 , 1856 ] ,[323.405 , 1863 ] ,[323.921 , 1867 ] ,[324.701 , 1792 ] ,[325.355 , 1718 ] ,[325.997 , 1657 ] ,[326.777 , 1580 ] ,[327.42 , 1559 ] ,[328.074 , 1594 ] ,[328.733 , 1615 ] ,[329.37 , 1639 ] ,[330.013 , 1635 ] ,[330.667 , 1649 ] ,[331.309 , 1649 ] ,[331.968 , 1640 ] ,[332.479 , 1658 ] ,[333.265 , 1687 ] ,[333.902 , 1642 ] ,[334.561 , 1479 ] ,[335.198 , 1473 ] ,[335.984 , 1461 ] ,[336.621 , 1448 ] ,[337.154 , 1450 ] ,[337.917 , 1439 ] ,[338.45 , 1468 ] ,[339.093 , 1497 ] ,[339.73 , 1535 ] ,[340.389 , 1488 ] ,[341.027 , 1468 ] ,[341.812 , 1445 ] ,[342.449 , 1449 ] ,[342.982 , 1460 ] ,[343.746 , 1403 ] ,[344.405 , 1401 ] ,[345.042 , 1407 ] ,[345.701 , 1380 ] ,[346.218 , 1430 ] ,[346.998 , 1425 ] ,[347.64 , 1421 ] ,[348.168 , 1444 ] ,[348.81 , 1530 ] ,[349.59 , 1288 ] ,[350.233 , 1291 ] ,[350.87 , 1364 ] ,[351.53 , 1399 ] ,[352.167 , 1439 ] ,[352.766 , 1310 ] ,[353.403 , 1283 ] ,[354.062 , 1295 ] ,[354.825 , 1239 ] ,[355.468 , 1169 ] ,[356.122 , 1196 ] ,[356.765 , 1193 ] ,[357.418 , 1195 ] ,[358.061 , 1171 ] ,[358.698 , 1199 ] ,[359.357 , 1159 ] ] #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) ====Ouverture du projet==== Lancer Visual Studio Code Cliquer sur Fichier->open Folder et choisir le dossier "R:\etu\....\tpomllidar" Cliquer sur "Yes I trust the Authors" Cliquer sur le fichier tpoml1.py et presser sur F5 pour lancer Si la question "You don't have an extension for debugging python..." apparait, cliquer sur "Find Python Extension" Dans la colonne de gauche, cliquer sur "install" à coté de "Python Intellisense..." puis en bas à droite sur "Install" =====Gestion des droites===== Pour le premier TP, vous devrez appeler vos fonctions de test à la suite à l'endroit indiqué dans la fonction **affichage_tests_etudiant()**. ====Choix de paramétrisation des droites==== Afin de manipuler les droites dans le programme, nous n'utiliserons pas les équations de droites affines sous la forme $y=a.x+b$ car cette forme ne permet pas de représenter à la fois des droites horizontales et verticales. Nous utiliserons ici la forme $a.x + b.y + c = 0$. Avec cette paramétrisation: * le vecteur directeur est réglé par les paramètres $a$ et $b$ ( $\left( \begin{array}{c}a \\ b \end{array} \right)$ est le vecteur normal à la droite) * la distance à l'origine est réglée par $c$ (mais pas forcement égale à) L'équation peut être multipliée par tout réel non nul sans changer la droite qu'elle représente: $\left( \begin{array}{c}a \\ b \\ c\end{array} \right) \equiv k.\left( \begin{array}{c}a \\ b \\ c\end{array} \right) \equiv \left( \begin{array}{c}k.a \\ k.b \\ k.c\end{array} \right) \forall k\neq 0$ Dans le cas ou $\left | \begin{array}{c}a \\ b\end{array} \right | = 1$ alors $\left\Vert c \right\Vert$ est égal à la distance de la droite à l'origine. ====Prise en main de la paramétrisation des droites et affichage==== Dans le squelette fourni se trouve une fonction **affiche_droite(da,db,dc,style='r-',xmin=-100,xmax=100,ymin=-100,ymax=100)** permettant d'afficher des droites dans une fenêtre. Compléter la fonction **affichage_tests_etudiant()** à l'endroit indiqué afin d'afficher des droites dont vous aurez préalablement choisi les paramètres, l'affichage se faisant dans un carré de coté 200 centré sur $(0,0)$. Le style pourra être réglé à 'b-','m-','y-','g-','c-','k-' pour choisir la couleur et différentier les droites. Vous devrez appeler toutes les différentes fonctions de test depuis **affichage_tests_etudiant()**. Pour effacer l'affichage graphique après un test, vous pourrez utiliser: **plt.clf()** ===Comparaison des droites=== Lire le code de la fonction **compare_droite(da,db,dc,das,dbs,dcs)** afin d'en comprendre le fonctionnement et interroger l'enseignant en cas de besoin. ====Ajustement d'une droite sur 2 points==== Nous nous intéressons maintenant à la fonction **determine_droite(pointsSegmentX, pointsSegmentY)** prenant en paramètres: * pointsSegmentX: une liste de coordoonées X des points * pointsSegmentY: une liste de coordoonées Y des points Cette fonction retourne **None** lorsque les 2 points fournis sont confondus. Dans le cas contraire, elle devra retourner les paramètres da,db,dc de la droite ajustée. La fonction **test_determine_droite()** va nous servir à tester la fonction **determine_droite(pointsSegmentX, pointsSegmentY)** . Elle contient déjà un exemple de test fournissant 2 points confondus et vérifie que le résultat produit est correct. Appelez **test_determine_droite()** dans **affichage_tests_etudiant()** et vérifier que le test se déroule bien. Ajouter ce test dans **test_determine_droite()** : print("test2") plt.scatter([0,1],[-1,0],marker='+',color='red',alpha=0.5) da,db,dc=determine_droite([0,1],[-1,0]) compare_droite(da,db,dc, -1,1,1) affiche_droite(da,db,dc,'r-') et observer le résultat du test. Vous devez maintenant compléter **determine_droite(pointsSegmentX, pointsSegmentY):** pour qu'elle puisse passer ce test. Vous pourrez dans un premier temps considérer que le paramètre $c$ de la droite est égal à 1 (pour rappel, l'équation de droite est définie à un facteur d'échelle non nul près), il y aura donc 2 inconnues $a$ et $b$ à déterminer. Pour cela vous devrez utiliser la librairie numpy afin de résoudre un système de 2 équations à 2 inconnues de la forme $A.X=B$ que vous construirez à l'aide des coordonnées des points fournis. Vous pourrez résoudre ce système en calculant $X=A^{-1}.B$ . Vous utiliserez la fonction **zeros** de numpy pour initialiser les matrices $A$ et $B$ avant de les remplir avec les bonnes valeurs. De plus, vous penserez à déposer dans les paramètres de retour de la fonctions les valeurs extraites de $X$, en ayant pris soin de les caster en type float. Vérifier le bon fonctionnement du test. Ajouter ce test dans **test_determine_droite()** : print("test3") plt.scatter([-1,-2],[-1,-2],marker='+',color='blue',alpha=0.5) da,db,dc=determine_droite([-1,-2],[-1,-2]) compare_droite(da,db,dc, 1,-1,0) affiche_droite(da,db,dc,'b-') et observer le résultat du test. Mettre en relation la propriété de la droite et le problème causé sur le système d'équations à résoudre. Compléter **determine_droite(pointsSegmentX, pointsSegmentY):** pour qu'elle puisse passer ce test. Pour cela vous devrez vérifier le rang de la matrice $A$ du système d'équations à l'aide de: rang_a=np.linalg.matrix_rank(A) Le traitement précédent fonctionne pour des matrices $A$ de rang 2, il faut donc conditionner son exécution au fait que le rang de $A$ soit de 2, et sinon exécuter un traitement différent adapté au cas particulier du rang de $A$ égal à 1 qui signifie que les lignes de $A$ ne sont pas linéairement indépendantes, autrement dit ici que l'une et l'autre sont égales à un facteur d'échelle près. Réfléchir à ce que cela implique pour la droite et proposer un traitement (très simple) pour calculer les paramètres $a$, $b$ et $c$ de la droite dans ce cas particulier. Vérifier le bon fonctionnement du test après avoir intégré vos modifications. Ajouter ce test dans **test_determine_droite()** : print("test4") plt.scatter([0,0],[0,1],marker='+',color='green',alpha=0.5) da,db,dc=determine_droite([0,0],[0,1]) compare_droite(da,db,dc, -1,0,0) affiche_droite(da,db,dc,'g-') et observer le résultat du test. Mettre en relation la propriété de la droite et le problème causé sur le système d'équations à résoudre. Au besoin, modifier votre fonction **determine_droite(...)** pour qu'elle passe ce test. Ajouter 2 tests supplémentaires de votre choix dans la fonction **test_determine_droite()** ====Calcul de la distance d'un point à une droite==== Implémenter la fonction **calcule_distance_droite_a_point(a,b,c,x,y)** en vous inspirant du résultat de https://homeomath2.imingo.net/distance2.htm et en utilisant les fonctions **math.fabs()** et **math.sqrt()**. Utilisez la fonction **test_calcule_distance_droite_a_point()** fournie pour tester votre fonction **calcule_distance_droite_a_point(a,b,c,x,y)**. ====Calcul de l'intersection de deux droites==== En vous inspirant de ce que vous venez de faire pour calculer la droite passant par deux points, compléter la fonction **calcule_intersection(a1,b1,c1,a2,b2,c2)** pour calculer le point à l'intersection de deux droites. Dans le cas particulier de droites parallèles, la fonction devra retourner un vecteur dont les deux composantes seront égales à l'infini à l'aide de **np.inf**. Exécuter le programme de test de la fonction **test_calcule_intersection()** et vérifier le bon fonctionnement. Vous pourrez utiliser la fonction **affiche(...)** fournie pour afficher le résultat de **calcule_intersection(...)** car il s'agit d'un **nd.array**. Compléter le programme de test pour gérer un cas pour lequel le rang de la matrice du système d'équations sera égal à 1 et tester. ====Ajustement d'une droite sur plus de 2 points==== Nous allons maintenant enrichir la fonction ** determine_droite(pointsSegmentX, pointsSegmentY)** pour qu'elle soit capable de calculer la droite passant //au mieux// par plus de 2 points. Cette droite sera obtenue au sens des moindres carrés, ce qui signifie qu'elle minimisera la somme des distances au carrés aux différents points sur lesquels elle sera ajustée. Ainsi la valeur obtenue pour les paramètres $a$, $b$ et $c$ optimum sont obtenus pour minimiser la somme des carrés des distances des points $P(x_i,y_i)$ à la droite $D(a,b,c)$: $(\hat{a}, \hat{b}, \hat{c},) = argmin_{(a,b,c)}{\sum_{i=1}^{n}{distance(P(x_i,y_i), D(a,b,c))^2}}$ Comme pour le cas à 2 points, vous pourrez dans un premier temps considérer que le paramètre $c$ de la droite est égal à 1 (pour rappel, l'équation de droite est définie à un facteur d'échelle non nul près), il y aura donc 2 inconnues $a$ et $b$ à déterminer. Nous allons donc construire un système d'équations linéaires comme pour le cas à 2 points, mais nous allons y intégrer plus d'équations indépendantes que d'inconnues. Ce système $A.X=B$ n'admettra donc pas de solution au sens strict, mais en multipliant le système à gauche par $A^{T}$, le système obtenu $A^{T}.A.X=A^{T}.B$ peut être résolu avec $X= {(A^{T}.A)}^{-1}.A^{T}.B$ . La matrice ${(A^{T}.A)}^{-1}.A^{T}$ est appelée pseudo-inverse de $A$ . Pour construire les matrices $A$ et $B$ à partir des listes de points de taille variable, vous pourrez utiliser le code suivant: A=np.column_stack((pointsSegmentX, pointsSegmentY))) B=-np.ones((A.shape[0],1)) Pour calculer la solution du système, consulter la documentation de numpy. Ajouter le test suivant dans **test_determine_droite()** et observer le résultat: #test6 plt.scatter([10,20,30],[10,11,9],marker='+',color='green',alpha=0.5) da,db,dc=determine_droite([10,20,30],[10,11,9]) affiche_droite(da,db,dc,'g-') ====Détection d'une droite dans un nuage de points==== Le programme fourni contient 2 fonctions **essai_paire_ransac(scan,indice1,indice2,seuil_distance)** et **ransac(scan,seuil_distance,nombre_tirages_aleatoires=100)** . Lire le code et essayer de comprendre le fonctionnement de ces fonctions. Vous allez ensuite compléter la fonction **test_ransac()** pour générer des données et afficher les résultats. Dans un premier temps, calculer les coordonnées x et y des points sur la droite paramétrée par $a$, $b$ et $c$ dans la boucle qui fait varier $n$ de -100 à +100. Appeler le programme de test **test_ransac()** dans **affichage_tests_etudiant()** et observer le résultat. La droite calculée doit passer par les points et "compare_droite: test réussi" doit s'afficher. Dans un second temps, régler **amplitude_perturbation** à 6 et relancer un test. Interpréter le résultat. Finalement, régler **nombre_points_outliers** à 500 et relancer un test. Interpréter le résultat. =====Egomotion: détermination de transformation rigide===== ====Analyse==== Nous cherchons maintenant à déterminer la transformation rigide entre 2 nuages de points Pour cela, dans la partie du code **Préparation des données pour le TP**, nous: - chargeons les données d'un scan vers une variable **scan_polaire** - convertissons les données de coordonnées polaires vers coordonnées cartésiennes vers une variable **scan1** - générons une transformation connue appliquée aux points de **scan1** pour obtenir des données modifiées dans une variable **scan2**, auquel nous ajoutons des petites perturbations - déterminons (manuellement ici) 2 paires de points dans chaque scan, chaque paire servant à déterminer une droite Les paramètres de la transformation rigides sont: * $\theta$ angle de rotation autour de l'axe vertical, réglé via la variable **theta** réglée dans test à 0.789 radians * $d_x$ translation selon $x$ dans le plan horizontal, réglé via la variable **dx** réglée dans test à 841.3cm * $d_y$ translation selon $y$ dans le plan horizontal, réglé via la variable **dy** réglée dans test à -521.0cm Nous utilisons ces paramètres dans la matrice de changement de repère suivante $^2_{}M_{1}$ stockée dans la variable **M_2_1** telle que: $^2_{}M_{1}=\begin{bmatrix} ^2_{}R_{1} & ^2_{}T_{1}\\ 0 & 1 \end{bmatrix} $ avec $^2_{}T_{1}=\begin{bmatrix}d_x\\d_y\end{bmatrix}$ et $^2_{}R_{1} =\begin{bmatrix} cos(\theta) & -sin(\theta)\\sin(\theta) & cos(\theta) \end{bmatrix}$ La fonction **affiche_scan(scan1,scan2=None)** fournie permet l'affichage en bleu du scan1 et en vert du scan2 ainsi que des droites correspondantes (calculées par votre fonction). Commenter à la fin du fichier tpoml1.py l'appel de la fonction **affichage_tests_etudiant()**. Le programme va dorénavant appeler les fonctions **ego_motion(scan1,scan2)** et **affiche_scan(scan1,scan2)**. Exécutez le programme avant de le modifier afin de voir les 2 scans non recalés (la fonction **ego_motion(...)** fournie applique une transformation sans effet sur le scan2 tant que vous ne l'avez pas modifiée). La ligne **scan2=ego_motion(scan1,scan2)** permet de procéder à l'appel de la fonction **ego_motion(scan1,scan2)** qui va devoir modifier le scan2 pour lui appliquer la transformation $^1_{}M_{2}$ inverse de $^2_{}M_{1}$. Pour cela, vous allez devoir utiliser les données disponibles suivantes: * **X1** et **Y1** : coordonnées de l'intersection des 2 droites dans le scan1 * **X2** et **Y2** : coordonnées de l'intersection des 2 droites dans le scan2 * **da11** et **db11** : composantes du vecteurs directeur d'une des droites dans le scan1 * **da21** et **db21** : composantes du vecteurs directeur de la même droite dans le scan2 Écrire comment la matrice $^2_{}M_{1}$ s'applique respectivement aux points et aux directions (vecteurs). Nous allons tenter d'écrire sous forme matricielle $A$.$X$=$B$ le système d'équations en regroupant dans $X$ les inconnues à déterminer et dans $A$ et $B$ les données disponibles. Comme le terme $\theta$ n'intervient pas en facteur mais à l'intérieur de fonctions trigonométriques, il n'est pas possible de mettre directement  $\theta$ dans $X$. Au lieu de cela, nous allons procéder à un changement de variables et déterminer $c_{\theta}=cos(\theta)$ et $s_{\theta}=sin(\theta)$. Grâce à ce changement vous devriez être capable d'écrire le système sous forme matricielle et de déterminer $X$. Pour calculer $\theta$ à partir de $c_{\theta}$ et $s_{\theta}$ vous devrez: * vous assurer que $c_{\theta}$ et $s_{\theta}$ sont bien les cosinus et sinus d'un angle en vérifiant que $c_{\theta}^2+s_{\theta}^2=1$ * puis utiliser la fonction **angle=math.atan2(Y,X)** ====Codage==== Compléter la fonction **ego_motion(scan1,scan2)** à l'endroit indiqué pour réaliser le calcul de la transformation $^1_{}M_{2}$. Pour cela, coder les calculs que vous venez de réaliser, extraire les différents paramètres de $X$ et reconstruire la matrice **M_2_1** et l'inverser pour obtenir **M_1_2**. Dans le programme fourni, **M_1_2** est appliqué au scan2 et si votre programme est correct, vous devriez voir les 2 scans se superposer en le relançant. {{https://bvdp.inetdoc.net/files/iut/tp_gps/BONUS.jpg}} Vous pouvez calculer la transformation $^2_{}M_{1}$ en écrivant un système comportant plus d'équations (6) que d'inconnues (4) utilisant les directions des deux autres droites et en résolvant le système obtenu au sens des moindres carrés. =====Solution===== #B. Vandeportaele 2022 #Squelette de programme pour TP OML LIDAR #IUT GEII TOULOUSE #Début commentaire prof #Ne pas mettre de balises solution car je n'arrive pas à les remplacer par # CODE A COMPLETER PAR LES ETUDIANTS en préservant les espaces devant... #Début solution #c'est la solution #Fin solution #Début solution #c'est la solution #Fin solution #Début solution #c'est la solution #Fin solution #venv avec visual studio code: https://code.visualstudio.com/docs/python/environments #(Ctrl+Shift+P), start typing the Python: Create Environment puis clicker sur Venv #puis cliquer en bas à droite sur ouvrir un dossier et choisir le dossier #Note: The command will also install necessary packages outlined in a requirements/dependencies file, such as requirements.txt, pyproject.toml, or environment.yml, located in the project folder # macOS/Linux # You may need to run sudo apt-get install python3-venv first # python3 -m venv .venv # Windows # You can also use py -3 -m venv .venv # python -m venv .venv #cliquer sur oui en bas à droite pour selectionner cet environnement virtuel #Tip: When you're ready to deploy the application to other computers, you can create a requirements.txt file with the command pip freeze > requirements.txt (pip3 on macOS/Linux). The requirements file describes the packages you've installed in your virtual environment. With only this file, you or other developers can restore those packages using pip install -r requirements.txt (or, again, pip3 on macOS/Linux). By using a requirements file, you need not commit the virtual environment itself to source control. #taper dans le terminal: # ca marque! Defaulting to user installation because normal site-packages is not writeable # c'est un problème de droits: https://linuxhint.com/default-user-installation/ # Ctrl+Shift+P), start typing the Python: select interpreter et choisir Python 3.10.6 'env':venv # https://stackoverflow.com/questions/54106071/how-can-i-set-up-a-virtual-environment-for-python-in-visual-studio-code # Ctrl + Shift + P and Terminal: Create New Integrated Terminal # from the terminal # Windows: .\.venv\Scripts\activate # Linux: .\.venv\bin\activate # du coup je fait dans un vrai terminal: # pour activer l'environnement virtuel depuis en dehors de vsc # python3 -m venv env # source env/bin/activate # pip install array_to_latex # pip install numpy # pip install matplotlib # je sauve les dépendances: # pip freeze > requirements.txt #exit(0) ########################################################## # Pour exécuter sous Linux, # CTRL+SHIFT+P, clic sur "Python select interpreter" puis choisir Python 3.10.6 'env':venv # copier dans le terminal: # source env/bin/activate # F5, puis cliquers sur "Fichier Python Deboguer le fichier Python actuellement actif" #Fin commentaire prof #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 #Début commentaire prof #https://stackoverflow.com/questions/18425225/getting-the-name-of-a-variable-as-a-string ''' x, y, z = 1, 2, 3 def retrieve_name(var): callers_local_vars = inspect.currentframe().f_back.f_locals.items() return [var_name for var_name, var_val in callers_local_vars if var_val is var] print(retrieve_name(y)) import inspect x,y,z = 1,2,3 def retrieve_name(var): callers_local_vars = inspect.currentframe().f_back.f_locals.items() return [var_name for var_name, var_val in callers_local_vars if var_val is var] def mod_retrieve_name(var): 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] #def foo(bar): # print( retrieve_name(bar) ) # print mod_retrieve_name(bar) #foo(x) ''' #Fin commentaire prof 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) ############################################################################################## #Début commentaire prof def TP1(): #Démonstration de création de matrice/vecteur et affichage #déclarations de vecteurs lignes contenant des entiers: a = np.array([1, 2, 3, 4]) #affichage des attributs print(f"a: {a}") print(f"a.shape: { a.shape}") print(f"a.ndim: { a.ndim}") print(f"a.dtype: { a.dtype}") #affichage à l'aide de la fonction affiche fournie: affiche(a) #déclarations de vecteurs lignes contenant des flottants: b = np.array([1, 2.5, 3, 4]) affiche(b) #déclarations de vecteur colonne: c = np.array([[1], [2.5], [3], [4]]) affiche(c) #transpose ne change pas shape.... cbis=b.transpose() affiche(cbis) #il faut plutôt utiliser reshape cter=b.reshape(1, 4) affiche(cter) #déclarations de vecteur ligne: d=np.array([[4], [0], [1], [0.5]]).reshape(1, 4) affiche(d) #exit(0) #################################### #initialisation Z=np.zeros((3, 1), dtype=np.int32) affiche(Z) D=np.ones((2, 3), dtype=np.int16) affiche(D) E=np.empty((4, 2), dtype=np.float64) affiche(E) R=np.arange(0, 2, 0.3) affiche(R) S=np.arange(0, 36, 3).reshape(3,4) affiche(S, "avant modification") #accès à un element pour en changer la valeur S[2,3]=6 affiche(S, "après modification") #numpy to latex: https://mathdatasimplified.com/2021/06/23/array_to_latex-turn-a-numpy-array-into-latex/ import array_to_latex as a2l latex=a2l.to_ltx(S) print(latex) #exit() #################################### #produit scalaire comme cas particulier du produit de matrices # #on peut leur fournir ce code comme départ def produit_scalaire(A,B): c=0. if A.shape[0] == 1 and B.shape[1] ==1: for elt in range(len(B)): c += A[0,elt] * B[elt,0] return c else: print("Désolé, impossible de multiplier A et B.") return None #################################### #A=np.arange(3).reshape(1, 3) #B=np.arange(3).reshape(3, 1) #déclarations de vecteur ligne: A=np.array([[1], [5], [0], [-1]]).reshape(1, 4) #déclarations de vecteur colonne: B = np.array([[1], [-3], [4], [-2]]) AB=produit_scalaire(A,B) #affiche le résultat print(f"A.B:{AB}") BA=produit_scalaire(B,A) #affiche le résultat print(f"B.A:{BA}") #équivalent avec opérateur produit de la librairie numpy print(f"A.B:{A@B}") #équivalent avec opérateur produit de la librairie numpy print(f"A.B:{B@A}") #################################### #trouvé sur https://geekflare.com/multiply-matrices-in-python/ def multiplie_matrice(A,B): #global C if A.shape[1] == B.shape[0]: C = np.zeros((A.shape[0],B.shape[1]),dtype = float) rows=A.shape[0] cols=B.shape[1] for row in range(rows): for col in range(cols): for elt in range(len(B)): C[row, col] += A[row, elt] * B[elt, col] return C else: print("Désolé, impossible de multiplier A et B.") return None #################################### BA=multiplie_matrice(B,A) #affiche le résultat print(f"B.A:{BA}") #exit() def test_multiplie_matrice(): print("test_multiplie_matrice()") A=np.arange(6).reshape(3, 2) B=np.arange(0,16,2).reshape(2, 4) C=multiplie_matrice(A,B) affiche(A) affiche(B) affiche(C) #produit par numpy: C2=A@B #comparaison print("Test de comparaison: " +str(np.allclose(C,C2))) test_multiplie_matrice() #exit() #################################### #faire aussi l'addition? ou la multiplication par un scalaire en exercice à la maison? def additionne_matrice(A,B): #global C if A.shape == B.shape: C = np.zeros(A.shape,dtype = float) rows=A.shape[0] cols=A.shape[1] for row in range(rows): for col in range(cols): C[row, col] = A[row, col] + B[row, col] return C else: return "Sorry, cannot add A and B." #################################### def test_additionne_matrice(): print("test_additionne_matrice()") A=np.arange(12).reshape(3, 4) B=np.arange(0,36,3).reshape(3, 4) C=additionne_matrice(A,B) affiche(A) affiche(B) affiche(C) #produit par numpy: C2=A+B #comparaison print("Test de comparaison: " +str(np.allclose(C,C2))) test_additionne_matrice() ############################################################################################## #np.random.seed(27) #A = np.random.random(1,10,size = (3,3)) #B = np.random.random(1,10,size = (3,2)) A=np.array([[1,2,3], [4,5,6]]) B=np.array([[1,2,3], [4,5,6]]).transpose() print(f"Matrix A: {A.shape}\n {A}\n") print(f"Matrix B: {B.shape}\n {B}\n") C = np.zeros((A.shape[0],B.shape[1]),dtype = float) C=multiplie_matrice(A,B) print(f"Matrix C:\n {C}\n") #produit par numpy: C2=A@B #comparaison print(str(np.allclose(C,C2))) ############################################################################################## #TP1() ############################################################################################## ############################################################################################## ############################################################################################## ############################################################################################## ############################################################################################## ############################################################################################## ############################################################################################## #Fin commentaire prof ############################################################################################## # 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): #Début commentaire prof #en fait ci est def calcule_intersection(a1,b1,c1,a2,b2,c2) mais cachée pour que les étudiants puisse utiliser #affiche_droite avant d'avoir codé leur calcule_intersection(...) #Fin commentaire prof 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 #------------------------------------------------------------------- #Début commentaire prof #il faut que ces matrices stockent des flottants -->mettre des coordonnées non entières dans le test unitaires #le mieux ce serait de faire la SVD.... qui retournerait un vecteur solution normalisé, et minimise une erreur géométrique dans le cas moindre carré... #a = np.arange(4, dtype=np.float64).reshape(2, 2) A = np.zeros(4, dtype=np.float64).reshape(2, 2) B = np.zeros(2, dtype=np.float64)[:,np.newaxis] #arange(2).reshape(2, 1) A[0][0]=x1; A[0][1]=y1; A[1][0]=x2; A[1][1]=y2; B[0]=-1 B[1]=-1 rang_A=np.linalg.matrix_rank(A) if rang_A==2: #les coordonnées du premier points ne sont pas un multiple des coordonnées du second: #droite ne passant par l'origine, on peut régler arbitrairement c=1 #il faut résoudre le systeme a.x=b Ainv=np.linalg.inv(A) #print("ainv: "+str(ainv)) #3 facons d'écrire le produit de matrices: #print("ainv@a: "+str(ainv@a)) #print("ainv.dot(a): "+str(ainv.dot(a))) #print("np.dot(ainv,a): "+str(np.dot(ainv,a))) #resolution du système d'équations RES=Ainv@B residus=(A@RES)-B #print(f"residus: {residus}") da=float(RES[0]) db=float(RES[1]) dc=float(1) else: #rang_A==1: #droite passant par l'origine, on ne peut pas régler arbitrairement c=1, car c=0 #il faut résoudre le systeme a.x=0 #1 seul des 2 points + l'origine suffit pour résoudre ce systeme, car les 2 lignes ne sont pas linéairement indépendantes if x1!=0 or y1!=0: da=float(-y1) db=float(x1) dc=float(0) else: da=float(-y2) db=float(x2) dc=float(0) #vérification: erreur=math.fabs(da*x1+db*y1+dc)+math.fabs(da*x2+db*y2+dc) #print(f"erreur:{erreur}") #Fin commentaire prof else: print("Cas de l'ajustement de droite sur plus de 2 points") #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #Début commentaire prof #print(f"pointsSegmentX: {pointsSegmentX}") #print(f"pointsSegmenty: {pointsSegmentY}") #A=np.column_stack(([1, 2, 3], [4, 5, 6])) A=np.column_stack((pointsSegmentX, pointsSegmentY)) #, dtype=float) #print(f"A: {A}") B=-np.ones((A.shape[0],1)) #print(f"B: {B}") #TODO: il faudra gérer le cas des droites passant par l'origine c=0 RES= (np.linalg.inv(A.transpose() @ A) @ A.transpose() ) @ B #print(f"res: {res}") da=float(RES[0]) db=float(RES[1]) dc=float(1) #Fin commentaire prof #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 #------------------------------------------------------------------- #Début commentaire prof print("test2") plt.scatter([0,1],[-1,0],marker='+',color='red',alpha=0.5) da,db,dc=determine_droite([0,1],[-1,0]) compare_droite(da,db,dc, -1,1,1) affiche_droite(da,db,dc,'r-') #droites passant par l'origine, problème avec la paramètrisation c=1 print("test3") plt.scatter([-1,-2],[-1,-2],marker='+',color='blue',alpha=0.5) da,db,dc=determine_droite([-1,-2],[-1,-2]) compare_droite(da,db,dc, 1,-1,0) affiche_droite(da,db,dc,'b-') #droites passant par l'origine, dont le premier point est à l'origine, problème avec la paramètrisation c=1 print("test4") plt.scatter([0,0],[0,1],marker='+',color='green',alpha=0.5) da,db,dc=determine_droite([0,0],[0,1]) compare_droite(da,db,dc, -1,0,0) affiche_droite(da,db,dc,'g-') print("test5") plt.scatter([0,0],[1,0],marker='+',color='green',alpha=0.5) da,db,dc=determine_droite([0,0],[1,0]) compare_droite(da,db,dc, -1,0,0) affiche_droite(da,db,dc,'g-') print("test6") plt.scatter([1,1],[0,1],marker='+',color='black',alpha=0.5) da,db,dc=determine_droite([1,1],[0,1]) compare_droite(da,db,dc, -1,0,1) affiche_droite(da,db,dc,'k-') da,db,dc=determine_droite([2,1],[2,1]) compare_droite(da,db,dc, -1,1,0) plt.scatter([0,1],[-1,0],marker='+',color='red',alpha=0.5) da,db,dc=determine_droite([0,1],[-1,0]) compare_droite(da,db,dc, -1,1,1) #test6 plt.scatter([10,20,30],[10,11,9],marker='+',color='green',alpha=0.5) da,db,dc=determine_droite([10,20,30],[10,11,9]) affiche_droite(da,db,dc,'g-') #Fin commentaire prof #################################################################### #------------------------------------------- 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 #------------------------------------------------------------------- #Début commentaire prof #idéalement on pourrait nomaliser préalablement (a,b) pour avoir la distance par simple produit scalaire.... d=math.fabs((a*x)+(b*y)+c)/math.sqrt((a*a)+(b*b)) #Fin commentaire prof 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 #------------------------------------------------------------------- #Début commentaire prof A=np.array([[a1,b1], [a2,b2]],dtype = float) B=np.array([[-c1], [-c2]],dtype = float) #vérifier que le système est inversible if np.linalg.matrix_rank(A)==2: #OUI: droites non // X=np.linalg.inv(A)@B else: #NON: droites // et intersection à l'infini X=np.array([[np.inf], [np.inf]],dtype = float) #Fin commentaire prof 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é") #Début commentaire prof # autre moyen de comparer if not (np.allclose(X,np.array([[-0.5], [-3]],dtype = float))): print("test1 échoué") #Fin commentaire prof #------------------------------------------------------------------- # CODE A COMPLETER PAR LES ETUDIANTS #------------------------------------------------------------------- #Début commentaire prof #test2: 2 droites // -> intersection à infini a1,b1,c1,a2,b2,c2=0,1,8,0,5,2 affiche_droite(a1,b1,c1,style='b-') affiche_droite(a2,b2,c2,style='b-') X=calcule_intersection(a1,b1,c1,a2,b2,c2) affiche(X) if not (X[0]==np.inf and X[1]==np.inf): print("test2 échoué") plt.scatter(X[0],X[1],marker='+',color='blue',alpha=0.5) #Fin commentaire prof ################################################################ 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] #Début commentaire prof #if score=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]] #Début commentaire prof RANSAC_actif=False if RANSAC_actif: #indice de 2 points pour estimation de la droite i1=295 i2=405 #indices trouvés par ransac avec seuilDistance=140.... #i1=103 #i2=455 seuilDistance=120 i1,i2=ransac(scan1,seuilDistance) #exit() #Le bon trouvé à la main score_indices_manuels=essai_paire_ransac(scan1,295,405,seuilDistance) print(f"score_indices_manuels: {score_indices_manuels}") #celui trouvé par ransac score_indices_ransac=essai_paire_ransac(scan1,i1,i2,seuilDistance) print(f"score_indices_ransac: {score_indices_ransac}") #plt.plot([x1,x2],[y1,y2],'g--') pointsSegmentX=[] pointsSegmentY=[] #TODO: comparer avec les points gardés par: essai_paire_ransac nbpts=0 #https://stackoverflow.com/questions/1663807/how-do-i-iterate-through-two-lists-in-parallel for x,y in zip(scan1X,scan1Y): d=calcule_distance_droite_a_point(da,db,dc,x,y) if d 1e-10: print(f"erreur_theta={ erreur_theta}") #version qui utilise les vecteurs directeurs des 2 droites pour estimer la rotation A=np.array( [ [ X1 , -Y1 , 1 , 0], [ Y1 , X1 , 0 , 1], [ da11 , -db11 , 0 , 0], [ db11 , da11 , 0 , 0], [ da12 , -db12 , 0 , 0], [ db12 , da12 , 0 , 0]], dtype=np.float64) B=np.array( [ [ X2 ], [ Y2 ], [ da21], [ db21], [ da22], [ db22]], dtype=np.float64) X= (np.linalg.inv(A.transpose() @ A) @ A.transpose() ) @ B affiche(A) affiche(B) affiche(X) theta=math.atan2(X[1],X[0]) dx=float(X[2]) dy=float(X[3]) 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) M_1_2=np.linalg.inv(M_2_1) affiche(M_1_2) #Fin commentaire prof #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 #Début commentaire prof #plt.scatter(scan1X,scan1Y,marker='+',c="blue",alpha=0.5) #plt.scatter(scan2X,scan2Y,marker='x',c="green",alpha=0.5) #plt.scatter(pointsSegmentX,pointsSegmentY,marker='.',c="red",alpha=0.5) #raffine l'estimation de le droite avec tous les points collectés: #da,db,dc=determine_droite(pointsSegmentX, pointsSegmentY) #Fin commentaire prof #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 #------------------------------------------------------------------- #Début commentaire prof affiche_droite(0,1,1,'b-') affiche_droite(0,1,10,'m-') affiche_droite(0,1,20,'y-') affiche_droite(0,1,30,'g-') affiche_droite(0,1,40,'c-') affiche_droite(0,1,50,'k-') #plt.clf() test_determine_droite() test_calcule_distance_droite_a_point() #plt.clf() test_calcule_intersection() test_ransac() #Fin commentaire prof #------------------------------------------------------------------- # 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)