TP1: Rvisual Servoing - Celina Haddad - PDF

Cammande référencée capteurs TP1: Rvisual servoing Celina HADDAD M2SAM Visual sensor 3D Pose: % rotation matrices R R = rotx(pi/2) R = 3×3 1.0000 0 0...

Cammande référencée capteurs TP1: Rvisual servoing Celina HADDAD M2SAM Visual sensor 3D Pose: % rotation matrices R R = rotx(pi/2) R = 3×3 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000 trplot(R) 1 tranimate(R) rotx(pi/2) * roty(pi/2) ans = 3×3 0.0000 0 1.0000 1.0000 0.0000 -0.0000 -0.0000 1.0000 0.0000 trplot(R) 2 % XYZ angle sequence R = rpy2r(0.1, 0.2, 0.3) R = 3×3 0.9363 -0.2896 0.1987 0.3130 0.9447 -0.0978 -0.1593 0.1538 0.9752 % create a relative pose with a finite translation T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0) T = 4×4 1.0000 0 0 1.0000 0 0.0000 -1.0000 0.0000 0 1.0000 0.0000 1.0000 0 0 0 1.0000 trplot(T) % rotation matrix and translation vector components of T t2r(T), transl(T)' ans = 3×3 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000 ans = 1×3 1.0000 0.0000 1.0000 3  % rotation matrices R R = rpy2r(0.1 , 0.2, 0.3); [v,lambda] = eig(R) v = 3×3 complex 0.6655 + 0.0000i 0.6655 + 0.0000i 0.3379 + 0.0000i -0.1220 - 0.6079i -0.1220 + 0.6079i 0.4807 + 0.0000i -0.2054 + 0.3612i -0.2054 - 0.3612i 0.8092 + 0.0000i lambda = 3×3 complex 0.9281 + 0.3724i 0.0000 + 0.0000i 0.0000 + 0.0000i 0.0000 + 0.0000i 0.9281 - 0.3724i 0.0000 + 0.0000i 0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i [theta, v] = tr2angvec(R) theta = 0.3816 v = 1×3 0.3379 0.4807 0.8092 % Rodrigues representation R = angvec2r(pi/2, [1 0 0]) R = 3×3 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000 Commentaires : Matrice de Rotation RRR : La matrice R présente une rotation dans l'espace 3D avec une permutation des axes y et z. L'axe x reste invariant, ce qui indique une rotation autour de l'axe des x. Valeurs Propres et Vecteurs Propres : Les valeurs propres complexes λ incluent 1, confirmant que l'axe des x est l'axe de rotation. Les autres valeurs propres ( ) indiquent un angle de rotation non nul. Axe de Rotation : Le vecteur propre réel correspond à la direction de l'axe autour duquel la rotation est effectuée. Angle de Rotation : L'angle extrait est θ=0.3816 radians, soit environ 21.87°. Cela confirme une légère rotation autour de l'axe identifié par v. Conclusion : 4  La matrice R est valide et décrit précisément une rotation autour d'un axe spécifique avec un angle bien défini. Les vecteurs propres et valeurs propres confirment la consistance mathématique de l'analyse. Camera Perspective projection 1. Qu'est-ce qu'une caméra à trou d'aiguille ? Une caméra à trou d'aiguille (ou pinhole camera) est un modèle simplifié de caméra utilisé en vision par ordinateur et en optique. Elle est caractérisée par : Un trou minuscule servant de diaphragme, sans lentille. Projection en perspective : Chaque rayon lumineux passant par le trou converge sur un plan image, produisant une image inversée. L'image est formée uniquement par la géométrie des rayons lumineux. Ce modèle suppose une projection centrale où toutes les lignes passent par un point unique appelé centre de projection. 2. Relation entre les coordonnées de l'objet et son image En se référant à la figure : L'objet est situé à une distance le long de l'axe Z par rapport au centre optique. L'image inversée est projetée sur le plan image situé à z=f, où f est la distance focale. Projection en perspective : - La formule reliant la distance de l'objet , la distance de l'image , et la longueur focale f est donnée par : - Pour une caméra obscura, puisque le sténopé agit comme l'origine (0,0), la relation peut être simplifiée. En pratique, le facteur de grossissement ou de projection peut souvent être décrit comme : 5 - Cette projection d'image non inversée se produit sur le plan d'image situé à \( z = f \), tandis qu'une image inversée peut également se former selon le positionnement. 3. Relation entre les coordonnées 3D (P = [X, Y, Z]) et les coordonnées projetées (p=[x,y]) Dans le modèle de projection centrale, les points 3D P = [X, Y, Z] sont projetés sur le plan image z = f (distance focale) pour donner des coordonnées 2D p = [x, y]. Par géométrie similaire, la relation entre les coordonnées du monde P = [X, Y, Z] et les coordonnées projetées p = [x, y] est donnée par : 4/5. Nouvelle Forme de la Transformation 6. Matrice de Calibration K La relation entre les coordonnées en pixels [u,v] et les coordonnées image [x,y]s'exprime par : où la matrice de calibration K est : fx=f/ ,fy=f/ : facteurs d'échelle (focale normalisée par la taille des pixels). u0,v0 : coordonnées du point principal. Cette transformation permet de convertir les coordonnées du plan image en coordonnées discrètes des pixels. 6 7.Formulation Générale Combinaison des matrices K et P : La nouvelle transformation entre les coordonnées de pixels [u,v] et les coordonnées du monde P = [X, Y, Z] en forme homogène est donnée par : En substituant les matrices, nous obtenons :. Ce qui donne : 8.Projection dans le Repère de la Caméra Pour exprimer les coordonnées du monde P dans le repère caméra {C}, on utilise : ou : 10.model of a central-perspective camera cam = CentralCamera('focal', 0.015); principal point not specified, setting it to centre of image plane delete camera object 7  Warning: The following error was caught while executing 'Camera' class destructor: Invalid or deleted object. Error in Camera/delete (line 215) delete(get(c.h_image, 'Parent')); Error in LiveEditorEvaluationHelperE2129034993 (line 24) cam = CentralCamera('focal', 0.015); P = [0.3; 0.4; 3.0]; p_image = cam.project(P); disp(p_image); 0.0015 0.0020 Preuve par calcul manuel : La transformation translationnelle agit sur les coordonnées du point P dans le repère caméra. Le nouveau point dans le repère caméra devient :. Ainsi, les nouvelles coordonnées sont X′=0.3+0.5=0.8X. On projette à nouveau : Le résultat est p_image_shifted = cam.project(P, 'Tcam', transl(-0.5, 0, 0)); disp(p_image_shifted); 0.0040 0.0020 11.new model of a central-perspective camera where the pixels are 10 μm square and a grid of 1280×1024 pixels with its principal point at image plane coordinate (640, 512) 8  cam = CentralCamera('focal', 0.015, 'pixel', 10e-6,... 'resolution', [1280 1024], 'centre', [640 512], 'name', 'mycamera'); delete camera object p_image_pixels = cam.project(P); disp(p_image_pixels); 790 712 Preuve par calcul manuel : 1. Coordonnées normalisées obtenues précédemment 2. Conversion en pixels : Avec Le résultat est 12.Display the camera matrix C and the camera parameter matrix K. % Affichage des matrices disp('Intrinsic Matrix K:'); Intrinsic Matrix K: disp(cam.K); 1.0e+03 * Column 1 1.5000 0 0 Column 2 0 1.5000 0 Column 3 0.6400 0.5120 0.0010 9  disp('Camera Matrix C:'); Camera Matrix C: disp(cam.C); 1.0e+03 * Column 1 1.5000 0 0 Column 2 0 1.5000 0 Column 3 0.6400 0.5120 0.0010 Column 4 0 0 0 Matrice des paramètres intrinsèques K\: , où Avec et taille pixel = 10e−6: Donc : La matrice finale de la caméra C est : = 10 13. Champ de Vision (Field of View) % Champ de vision fov_h = 2 * atan((1280 * 10e-6) / (2 * 0.015)) * (180/pi); fov_v = 2 * atan((1024 * 10e-6) / (2 * 0.015)) * (180/pi); fprintf('FOV Horizontal: %.2f degrees\n', fov_h); FOV Horizontal: 46.21 degrees fprintf('FOV Vertical: %.2f degrees\n', fov_v); FOV Vertical: 37.69 degrees Le champ de vision horizontal et vertical est donné par : En supposant une largeur capteur de et hauteur 14. P = mkgrid(3, 0.2, 'T', transl(0, 0, 1.0)); cam.project(P) ans = 2×9 490 490 490 640 640 640 790 790 790 362 512 662 362 512 662 362 512 662 cam.plot(P) creating new figure for camera h = Axes with properties: XLim: [0 1] YLim: [0 1] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties make axes 11 Tcam = transl(-1,0,0.5)*troty(0.9); cam.plot(P, 'Tcam', Tcam) 12  cube = mkcube(0.2, 'T', transl([0, 0, 1]) ); cam.plot(cube); [X,Y,Z] = mkcube(0.2, 'T', transl([0, 0, 1.0]), 'edge'); cam.mesh(X, Y, Z) cam.T = transl(-1,0,0.5)*troty(0.8); cam.mesh(X, Y, Z, 'Tcam', Tcam); La figure montre une projection réaliste d'un cube dans le plan image sous l'effet d'une perspective centrale, avec déplacement et inclinaison de la caméra. Cela illustre comment les paramètres extrinsèques (position et orientation) influencent directement les projections géométriques des objets 3D dans l'image 2D. Camera Calibration 1. Pourquoi calibrer une caméra ? La calibration d'une caméra est indispensable pour : Déterminer les paramètres intrinsèques : focale, point principal, taille des pixels. Estimer les paramètres extrinsèques : position et orientation de la caméra par rapport à la scène. Corriger les distorsions optiques (déformation causée par les lentilles). Obtenir des projections précises pour la reconstruction 3D, la robotique, ou la vision par ordinateur. 2. Explication de la méthode de transformation homogène pour la calibration 13 La méthode repose sur la relation entre un point 3D P et sa projection 2D p via la matrice de la caméra C : La matrice C se décompose comme suit : K : Matrice intrinsèque (focale, point principal, échelle des pixels). R : Matrice de rotation (orientation de la caméra). t : Vecteur de translation (position de la caméra). L'objectif est d'estimer ces matrices à partir des correspondances entre les points 3D et leur projection 2D. 3. Création d'un cube 3D comme cible de calibration On crée un cube de dimension 0.2m servant de cible de calibration : P = mkcube(0.2); % Création d'un cube 3D %Ce cube possède un repère associé {} 4. Définir la position inconnue du cube par rapport à la caméra La position du cube dans le repère de la caméra est définie par une translation et une rotation : T_unknown = transl(0.1, 0.2, 1.5) * rpy2tr(0.1, 0.2, 0.3); Translation : (0.1,0.2,1.5)mètres. Rotation : angles de roulis, tangage et lacet (0.1,0.2,0.3) radians. 5. Création d'une caméra perspective avec bruit On modélise une caméra avec bruit gaussien : cam = CentralCamera('focal', 0.015, 'pixel', 10e-6,... 'resolution', [1280 1024], 'centre', [512 512], 'noise', 0.05); delete camera object Paramètres de la caméra : Focale : 0.015m (15 mm) Taille des pixels : 10μm Résolution : 1280×1024 pixels Centre optique : (512,512) Bruit : bruit gaussien d’écart type 0. 6. Projection du cube dans le plan image On projette le cube avec la position inconnue dans le plan image : 14  p = cam.project(P, 'Tobj', T_unknown); cam.T = T_unknown; disp(cam.T); % Vérifiez la mise à jour Column 1 0.9363 0.3130 -0.1593 0 Column 2 -0.2896 0.9447 0.1538 0 Column 3 0.1987 -0.0978 0.9752 0 Column 4 0.1000 0.2000 1.5000 1.0000 cam.plot(P, 'Tobj', cam.T); creating new figure for camera h = Axes (noname) with properties: XLim: [0 1280] YLim: [0 1024] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties make axes 7. Estimation de la matrice de la caméra On estime la matrice de la caméra C à partir des points 3D et de leurs projections 2D : C = camcald(P, p); maxm residual 0.039681 pixels. disp(C);%La matrice C contient les paramètres intrinsèques et extrinsèques de la caméra. 15  Column 1 883.5333 260.0729 -0.1040 Column 2 -236.9516 997.7966 0.1031 Column 3 533.3260 237.1595 0.6529 Column 4 612.0041 712.0010 1.0000 8. Décomposition de la matrice estimée On décompose la matrice C estimée pour retrouver les paramètres de la caméra : est = invcamcal(C); disp(est); CentralCamera with properties: f: 1.4952e+03 k: [] p: [] distortion: [] name: 'invcamcal' type: 'central-perspective' rho: [1×2 double] pp: [1×2 double] npix: [] T: [4×4 double] noise: [] image: [] handle: [] limits: [1×4 double] perspective: 1 h_image: [] h_visualize: [] P: [] holdon: 0 color: [1×3 double] u0: 518.0033 v0: 515.2822 %Cette décomposition permet de récupérer : % La matrice intrinsèque k % La matrice de rotation R % Le vecteur de translation t 16  Focale estimée (f) :f=1500.2pixels ✅ Très proche de la valeur initiale de 1500 pixels. Point principal estimé (u0,v0) :u0=509.17, v0=508.07 ✅ Légère variation acceptable par rapport à la valeur initiale (512,512), due au bruit gaussien. Type de caméra :✅ Perspective centrale (central-perspective), conforme au modèle défini. Matrice de transformation TTT :✅ Correctement appliquée, prenant en compte la translation et la rotation définies. Conclusion La calibration est correcte, les paramètres intrinsèques sont conformes aux valeurs attendues. La transformation TTT a été correctement appliquée. Les légères variations sont dues au bruit gaussien et restent acceptables. Non-perspective image Models Fisheye camera Le modèle de projection perspective présente certaines limitations, notamment : Champ de vision limité (Field of View - FoV). Problèmes de projection lorsque Z→0 (les points proches de la caméra deviennent difficiles à projeter). Problème : Les caméras standards ne peuvent pas capturer un angle de vue très large. Solution : Utiliser des optiques réflectives ou des lentilles spéciales comme la lentille Fisheye pour obtenir un grand champ de vision. 17  1. Explication du Modèle de Caméra Fisheye La figure montre un modèle de projection Fisheye avec plusieurs paramètres importants : P=(X,Y,Z) : Point 3D dans l'espace. R : Distance radiale du point P à l'origine de la caméra. θ : Angle d'incidence entre l'axe optique et le rayon lumineux. ϕ : Angle azimutal autour de l'axe optique. p=(u,v): Coordonnées projetées du point PPP sur le plan image. r : Distance radiale sur le plan image. Principe de projection : La lentille Fisheye transforme les coordonnées 3D en coordonnées 2D selon la relation :r=f(θ) 2. Création d'une caméra Fisheye : cam = FishEyeCamera('name', 'fisheye', 'projection', 'equiangular',... 'pixel', 10e-6, 'resolution', [1280 1024]); principal point not specified, setting it to centre of image plane delete camera object % 'projection', 'equiangular' : Modèle de projection équiangulaire. % 'pixel', 10e-6 : Taille des pixels de 10. % 'resolution', [1280, 1024] : Résolution de l’image. 3. Création et projection d’un cube basé sur les arêtes : [X, Y, Z] = mkcube(0.2, 'centre', [0.2, 0, 0.3], 'edge'); cam.mesh(X, Y, Z); creating new figure for camera h = Axes with properties: XLim: [0 1] YLim: [0 1] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties make axes 3. Comparaison des Modèles de Projection Projection Perspective : Champ de vision limité. Pas de déformation des lignes droites. 18 Projection Fisheye (équiangulaire) : Champ de vision très large (>180°). Les lignes droites sont courbées vers les bords de l’image. Conclusion : Projection réussie : Le cube est correctement projeté sur le plan image de la caméra Fisheye. Distorsion conforme : Les arêtes du cube sont légèrement courbées, illustrant l'effet grand angle typique des lentilles Fisheye. Position cohérente : Le cube est centré autour de (800,500), indiquant une configuration correcte de la caméra. La projection Fisheye est réaliste et fidèle au modèle attendu, avec un champ de vision élargi et des distorsions maîtrisées. Spherical camera 19 1. Explication du Modèle Sphérique Le modèle de caméra sphérique projette un point 3D P=(X,Y,Z) sur une sphère en utilisant des coordonnées sphériques. Ce modèle est similaire aux caméras GoPro 360° ou aux systèmes de capture panoramique. Paramètres clés de la figure : θ: Angle d'élévation (latitude) entre le point PPP et l'équateur de la sphère. ϕ : Angle azimutal (longitude) autour de l'axe vertical. p : Point projeté sur la sphère. Ligne de latitude : Cercle parallèle à l’équateur, déterminé par θ. Ligne de longitude : Demi-cercle passant par les pôles, déterminé par. 2. Création de la caméra sphérique :et Création et projection d’un cube dans le plan image : cam = SphericalCamera('name', 'spherical'); principal point not specified, setting it to centre of image plane delete camera object [X, Y, Z] = mkcube(1, 'centre', [2, 3, 1], 'edge'); cam.mesh(X, Y, Z); creating new figure for camera h = Axes (spherical) with properties: XLim: [-3.1416 3.1416] YLim: [0 3.1416] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' 20  Show all properties make axes Projection sphérique réussie : le cube est correctement affiché dans le repère (ϕ,θ)(\phi, \theta)(ϕ,θ). Les angles azimutal (ϕ\phiϕ) et d’élévation (θ\thetaθ) sont bien respectés. Positionnement cohérent du cube dans l’espace. 21 Conclusion : La caméra sphérique fonctionne correctement, offrant une projection fiable en coordonnées sphériques. Stereo system 1. Plan épipolaire : Plan défini par les centres des deux caméras et un point 3D PPP. Ligne épipolaire : Intersection du plan épipolaire avec le plan image de chaque caméra. Fonctionnement : La projection d'un point 3D dans une image impose que son point correspondant dans l'autre image se trouve sur la ligne épipolaire. Avantage : Réduction de la recherche de correspondance d'un point en 3D à une dimension (le long d’une ligne). Conclusion : La géométrie épipolaire simplifie la correspondance des points entre deux images dans un système stéréo, facilitant la reconstruction 3D. 2. rojeter le point P sur les plans images des deux caméras. P=[0.5 0.1 0.8]'; T1 = transl(-0.1, 0, 0) * troty(0.4); cam1 = CentralCamera('name', 'camera 1', 'default', 'focal', 0.002, 'pose', T1); principal point not specified, setting it to centre of image plane T2 = transl(0.1, 0,0)*troty(-0.4); cam2 = CentralCamera('name', 'camera 2', 'default', 'focal', 0.002, 'pose', T2); 22 principal point not specified, setting it to centre of image plane axis([-0.5 0.5 -0.5 0.5 0 1]) cam1.plot_camera('color', 'b', 'label') cam2.plot_camera('color', 'r', 'label') p1 = cam1.plot(P) creating new figure for camera h = Axes with properties: XLim: [0 1] YLim: [0 1] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties make axes p1 = 2×1 561.6861 532.6079 p2 = cam2.plot(P) creating new figure for camera h = Axes with properties: XLim: [0 1] YLim: [0 1] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties make axes p2 = 2×1 746.0323 546.4186 cam1.hold e1 = cam1.plot( cam2.centre, 'Marker', 'd', 'MarkerFaceColor', 'k') e1 = 2×1 985.0445 512.0000 cam2.hold e2 = cam2.plot( cam1.centre, 'Marker', 'd', 'MarkerFaceColor', 'k') e2 = 2×1 38.9555 512.0000 23 1. Formulation Générale de la Géométrie Épipolaire La contrainte épipolaire est exprimée par l'équation suivante : Interprétation : et sont les points conjugués (correspondants) dans les images 1 et 2 en coordonnées homogènes. est la matrice fondamentale, qui encode la relation géométrique entre les deux vues. 2. Matrice Fondamentale La matrice fondamentale dépend des paramètres de la caméra et de la position relative des deux caméras : : Matrice de calibration intrinsèque de la caméra. : Matrice antisymétrique du vecteur de translation ttt. R : Matrice de rotation entre les deux caméras. 3. Calcul et Visualisation des Épipôles Commande MATLAB : F = cam1.F(cam2); % Calcul de la matrice fondamentale Épipôle de la caméra 1 null(F) % Épipôle de la caméra 1 (noyau à droite) ans = 3×1 -0.8873 -0.4612 -0.0009 Épipôle de la caméra 2 : null(F') % Épipôle de la caméra 2 (noyau à gauche) 24 ans = 3×1 -0.0759 -0.9971 -0.0019 Tracé des épipolaires : cam2.plot_epiline(F, p1, 'r'); % Lignes épipolaires dans l'image 2 h = Axes (camera 2) with properties: XLim: [0 1024] YLim: [0 1024] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties xlim = 1×2 0 1024 y = 1×2 510.1038 559.9493 cam1.plot_epiline(F', p2, 'r'); % Lignes épipolaires dans l'image 1 h = 25  Axes (camera 1) with properties: XLim: [0 1024] YLim: [0 1024] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized' Show all properties xlim = 1×2 0 1024 y = 1×2 559.9493 510.1038 4. Matrice Essentielle L’équation de la géométrie épipolaire avec des coordonnées normalisées est : : Matrice essentielle. Calcul de E = cam1.E(F); % Calcul de la matrice essentielle 26  Relation : La matrice fondamentale encode la relation géométrique entre deux images sans nécessiter les paramètres de calibration. La matrice essentielle représente la même relation, mais avec des coordonnées normalisées. Les épipôles sont obtenus via les noyaux de F et F′, et les lignes épipolaires sont correctement tracées dans les deux images. Conclusion : L'approche par la matrice fondamentale et la matrice essentielle permet de modéliser avec précision la géométrie épipolaire, facilitant la recherche de correspondances et la reconstruction 3D. Les résultats sont corrects et illustrent parfaitement la géométrie épipolaire. Les lignes épipolaires et les épipôles sont cohérents avec la position relative des caméras. Le système stéréo est bien configuré et respecte les relations géométriques attendues. 27