Laboratoire 3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Laboratoire 3 - Caméra\n",
    "Matériel fourni: règle en aluminium de 1 m, règle en plastique de 30 cm, rapporteur d'angle, repère vertical.\n",
    "\n",
    "## Partie 0 - Interagir avec la caméra\n",
    "\n",
    "D'abord, exécutons un peu de code de configuration, puis connectons-nous au robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%matplotlib nbagg\n",
    "\n",
    "import numpy as np\n",
    "import numpy.linalg as lin\n",
    "\n",
    "import matplotlib as mpl\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "mpl.use('nbagg')\n",
    "\n",
    "from matplotlib import animation\n",
    "mpl.rc('animation', html='html5') #display animated plots inline\n",
    "\n",
    "from robmob.robot import Robot\n",
    "from robmob.sensors import KinectRGBSensor\n",
    "from robmob.visualization import Visualizer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ip_robot = '192.168.0.111'\n",
    "robot = Robot(ip_robot)\n",
    "robot.connect()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Le code suivant sert à créer ajouter la Kinect aux capteurs du robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "kinect = KinectRGBSensor()\n",
    "robot.add_sensor(kinect)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> **PROTIP** Aussitôt que vous exécutez la ligne `robot.add_sensor(kinect)`, le flux de la caméra est transmi \n",
    "> par le réseau du robot jusqu'à votre ordinateur. Comme ces données causent un bon traffic sur le routeur, \n",
    "> tentez de ne pas vous connecter à ce flux de données plusieurs fois en parallèle!\n",
    "\n",
    "> **ATTENTION** Certaines Kinect sont branchées dans un hub USB au lieu d'être branchées directement dans l'ordinateur de bord. Si vous n'arrivez pas recevoir des images de la Kinect, mais que vous arrivez à vous connecter à d'autres capteurs du robot, tentez de brancher la Kinect directement dans l'ordinateur de bord."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Acquisition d'images\n",
    "\n",
    "Pour afficher la dernière image capturée par la Kinect, on peut utiliser `peek_data`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "kinect.peek_data()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Même si ce n'est pas tout à fait nécessaire dans le cadre de ce laboratoire, on peut aussi afficher les images de la kinect en continu avec le code suivant."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "from IPython.display import display,HTML,clear_output\n",
    "import time\n",
    "\n",
    "N_OF_FRAMES_TO_DISPLAY = 100\n",
    "\n",
    "for i in range(N_OF_FRAMES_TO_DISPLAY):\n",
    "    clear_output(wait=True)\n",
    "    display(kinect.peek_data())\n",
    "    time.sleep(0.09999)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 1 - Calibration de la longueur focale\n",
    "\n",
    "L'objectif de cette partie de de déterminer la longueur focale de la caméra du robot. Nous utiliserons la caméra RGB de la Kinect, en ignorant pour l'instant la partie infra-rouge des données.\n",
    "\n",
    "> **NOTE** La caméra RGB de la Kinect est la lentille du centre. Mettez cette caméra au dessus du 0 de la règle d'un mètre.\n",
    "\n",
    "Pour commencer, placez la règle de 1 m sur une table, puis placez le robot à une des extrémités de la règle. À l'autre extrémité, placez la règle de plastique de 30 cm à la même hauteur que la caméra, de sorte que la Kinect observe directement la règle de plastique. La caméra devrait être à environ 50 cm de la règle de plastique. Vous pouvez utiliser une feuille de papier derrière la règle de plastique pour rendre les mesures plus faciles à prendre. Votre assemblage devrait ressembler à l'image ci-bas.\n",
    "\n",
    "![Assemblage pour la calibration](img/assemblage_calibration.jpg)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "La commande ci-bas va afficher une image prise par la kinect dans une console interactive. Quand vous passez votre curseur de souris dans la console interactive, on devrait vous indiquer à quelle position, en pixel, se trouve votre curseur. Prenez en note la position de deux pixels qui sont à une distance connue. Par exemple, vous pourriez prendre en note la position de deux pixels qui sont à 5 cm de distance sur la règle. Pour vous faciliter la tâche, n'hésitez pas à utiliser le bouton *zoom to rectangle* de la console interactive, qui vous permettera de zoomer sur la règle et de voir les chiffres plus facilement.\n",
    "\n",
    "> **ATTENTION** Dans le présent *jupyter notebook*, il est important de fermer les figures (en appuyant sur le bouton bleu) quand vous avez terminé des les consulter. Jupyter ne semble pas capable d'afficher deux figures en même temps."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.imshow(kinect.peek_data())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Utilisez le théorème de Thalès pour déduire la longueur focale de la caméra de la kinect. En guise de rappel, la longueur focale est donnée par \n",
    "\n",
    "$$ f = \\Delta L_{caméra} \\frac{A_z}{\\Delta L_{réel}} $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "first_pixel_x = 285\n",
    "second_pixel_x = 335\n",
    "\n",
    "delta_l_camera = second_pixel_x - first_pixel_x\n",
    "delta_l_real_object = 5 # Distance réelle entre les deux pixels sélectionnés\n",
    "distance_to_ruler = 50\n",
    "\n",
    "f = delta_l_camera * distance_to_ruler / delta_l_real_object\n",
    "print('Longueur focale: {}'.format(f))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "Pour valider vos calculs, déplacez la règle de plastique à une distance différente de la caméra. Toujours en utilisant le théorème de Thalès, estimez la distance $A_z$ en utilisant la longueur focale que vous avez trouvé. Votre estimation de $A_z$ et sa valeur réelle devraient être similaire."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 2 - Mesure d'angles\n",
    "\n",
    "Comme mentionné en classe, la caméra est un rapporteur d'angle. Cette partie décrit comment faire des mesures d'angles avec la caméra du robot.\n",
    "\n",
    "Placez deux objets dans le champ de vision de la caméra. En vous servant des règles et d'un rapporteur d'angle, mesurez l'angle approximatif entre ces deux objets, du point de vue de la caméra.\n",
    "\n",
    "À l'aide de la commande suivante, trouvez les coordonnées en $x$ du centre de chacun des objets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.imshow(kinect.peek_data())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "p1 = 0\n",
    "p2 = 0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Calculez l'angle $\\theta$ entre les deux objets. Pour ce faire, vous devez considérer la distance en x (sur le plan image) entre le centre de l'objet et le centre optique de la caméra. Le centre optique passe au milieu de l'image, la colonne 320 dans notre cas. En guise de rappel, voici un schéma qui pourrait vous aider à calculer les angles nécessaires.\n",
    "\n",
    "![](img/calcul_angle.png)\n",
    "\n",
    "> **PROTIP** La fonction `arctan2` de numpy pourrait vous être utile! Comme nous avons renommé `numpy` pour `np` dans le haut du document, vous pouvez l'appeler en faisant `np.arctan2()`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "lx1 = 0 # TODO\n",
    "theta1 = 0 # TODO\n",
    "\n",
    "lx2 = 0 # TODO\n",
    "theta2 = 0 # TODO\n",
    "\n",
    "\n",
    "angle_between_objects = 0 # TODO\n",
    "print(angle_between_objects)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 3 - Localisation en deux dimensions\n",
    "\n",
    "Placez cette fois trois objets sur le plancher, en vous servant des tuiles comme système cartésien. Le point de repère `p2` sera la position (0,0) de votre référentiel monde. Ce système de coordonnées aura comme unité de longueur une *tuile*, soit environ 30 cm. Placez deux autres objets aux intersections de tuiles, de sorte à avoir le montage ci-bas.\n",
    "\n",
    "![img](img/montage_crayon.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Avec le code suivant, capturez une image, puis notez la position en $x$ de chaque objet. Calculez l'angle $\\alpha$ entre les objets 1 et 2 puis l'angle $\\beta$ entre les objets 2 et 3."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.imshow(kinect.peek_data())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# TODO - mX représente la coordonnée en pixel de pX dans l'image\n",
    "m1 = 0.\n",
    "m2 = 0.\n",
    "m3 = 0.\n",
    "\n",
    "def distance(p1, p2):\n",
    "    return np.sqrt((p2[0,0] - p1[0,0])**2 + (p2[1,0] - p1[1,0])**2)\n",
    "\n",
    "def alpha_beta_from_three_coordinates(f, c1, c2, c3):\n",
    "    \"\"\"\n",
    "    Retourne l'angle entre l'objet 1 et 2 (alpha), puis l'angle entre l'objet 2 et 3 (beta). \n",
    "    Les arguments c1, c2, c3 sont la position en x de chaque objet dans l'image. f est la\n",
    "    longueur focale.\n",
    "    \"\"\"\n",
    "    position_of_optical_axis = 320\n",
    "    \n",
    "    positions = np.array([c1,c2,c3])\n",
    "    thetas = np.degrees(np.arctan((positions - position_of_optical_axis) / f))\n",
    "    \n",
    "    return (thetas[1] - thetas[0], thetas[2] - thetas[1])\n",
    "\n",
    "def circle_from_pts_and_angle(pts, angle):\n",
    "    \"\"\"\n",
    "    Construit un cercle à partir de deux points de ce cercle et de l'angle\n",
    "    entre ces deux points vu par un objet qui est aussi sur le cercle. pts doit\n",
    "    être un tuple de points. Le point le plus à gauche doit toujours être donné\n",
    "    en premier.\n",
    "    \"\"\"\n",
    "    (p1, p2) = pts\n",
    "    \n",
    "    q = distance(p1,p2)\n",
    "    m = (p1 - p2) / 2. + p2                      # Point milieu entre les deux points connus\n",
    "    v = np.array([[0, -1], [1, 0]]).dot(m - p2)  # Vecteur perpendiculaire à la droite reliant p1 et p2\n",
    "    \n",
    "    l = (q / 2) / np.tan(np.radians(angle))      # Distance entre le points milieu et le centre du cercle\n",
    "    \n",
    "    v = (v / lin.norm(v)) * l                    # Ajustement de la longueur du vecteur\n",
    "    \n",
    "    c = m + v                                        # Centre du cercle\n",
    "    r = np.fabs((q / 2.) / np.sin(np.radians(angle)))# Rayon du cercle\n",
    "\n",
    "    return (c.transpose()[0], r)\n",
    "\n",
    "p1 = np.array([[-1.],[1.]])\n",
    "p2 = np.array([[0.], [0.]])\n",
    "p3 = np.array([[1.], [0.]])\n",
    "\n",
    "(alpha, beta) = alpha_beta_from_three_coordinates(f, m1, m2, m3)\n",
    "\n",
    "(c1, r1) = circle_from_pts_and_angle((p1,p2), alpha)\n",
    "(c2, r2) = circle_from_pts_and_angle((p2,p3), beta)\n",
    "\n",
    "print('Premier cercle centré en {} avec un rayon de {}'.format(c1, r1))\n",
    "print('Second cercle centré en {} avec un rayon de {}'.format(c2, r2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 4 - Effet du bruit sur la localisation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Les fonctions qui suivent servent à montrer l'effet du bruit des mesures sur la position estimée du robot. Ici nos mesures sont les positions en x des repères dans l'image. La boucle qui suit va ajouter un bruit aléatoire (selon une distribution normale) sur vos mesures. Ensuite, elle recalcule la position du robot comme dans la partie précédente. Finalement, on trace un graphique de toutes les positions estimées."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "import numpy.random\n",
    "from robmob.geometry import circle_intersection\n",
    "\n",
    "\n",
    "measures = np.array([m1, m2, m3])\n",
    "\n",
    "MEAN_NOISE = 0.0\n",
    "NOISE_STD_DEV = 5.0\n",
    "\n",
    "\n",
    "# Construire une liste d'hypothèses sur la position du robot.\n",
    "intersection_list = np.empty((0,2))\n",
    "for i in range(100):\n",
    "    # On ajoute un bruit gaussien aux mesures\n",
    "    noisy_measures = measures + np.random.normal(MEAN_NOISE, NOISE_STD_DEV, (3))\n",
    "    \n",
    "    (alpha, beta) = alpha_beta_from_three_coordinates(f, *noisy_measures)\n",
    "    \n",
    "    noisy_c1 = circle_from_pts_and_angle((p1, p2), alpha)\n",
    "    noisy_c2 = circle_from_pts_and_angle((p2, p3), beta)\n",
    "    \n",
    "    \n",
    "    intersections = circle_intersection((noisy_c1[0][0], noisy_c1[0][1], noisy_c1[1]), \n",
    "                                        (noisy_c2[0][0], noisy_c2[0][1], noisy_c2[1]))\n",
    "    \n",
    "    new_rows = np.array([intersections[0], intersections[1]])\n",
    "    intersection_list = np.concatenate([intersection_list, new_rows], axis=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(1,1,1)\n",
    "\n",
    "circle1 = plt.Circle(c1, r1, edgecolor='r', facecolor='none', linewidth=2.0)\n",
    "circle2 = plt.Circle(c2, r2, edgecolor='b', facecolor='none', linewidth=2.0)\n",
    "\n",
    "ax.add_patch(circle1)\n",
    "ax.add_patch(circle2)\n",
    "\n",
    "ax.scatter(intersection_list[:,0], intersection_list[:,1])\n",
    "\n",
    "ax.set_xticks(np.arange(-10,10,1))\n",
    "ax.set_yticks(np.arange(-10,10,1))\n",
    "ax.set_xlim([-5,5])\n",
    "ax.set_ylim([-5,5])\n",
    "ax.set_aspect('equal', 'datalim')\n",
    "ax.grid()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}