Laboratoire 4.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Laboratoire 4 - Oak-D Lite\n",
    "\n",
    "Au cours de ce laboratoire, nous allons étudier le capteur Oak-D Lite, qui permet \n",
    "d'obtenir des données de profondeur. Connectons-nous d'abord au robot.\n",
    "\n",
    "**Lancez ce laboratoire avec Jupyter Lab:** Vous pouvez l'ouvrir en cliquant sur l'hyperlien dans le coin supérieur droit.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "%matplotlib ipympl\n",
    "\n",
    "import math\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import cv2\n",
    "\n",
    "from robmob import robot\n",
    "from robmob.point_cloud import PointCloud\n",
    "from robmob.rover.sensors import OakLiteCamera\n",
    "\n",
    "robot_ip = 'localhost'\n",
    "robot = robot.Robot(robot_ip)\n",
    "robot.connect()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 1 - Visualisation\n",
    "\n",
    "Ajoutons deux capteur au robot, le capteur Oak-D Lite retourne les images couleur et la coordonnée en `z` de chaque pixel.\n",
    "\n",
    "> **NOTE** Le système de coordonnées utilisé est $z$ devant, $x$ à droite et $y$ vers le bas"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "oak_sensor = OakLiteCamera(use_depth=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "La fonction suivante affiche les données retournées par la Oak-D Lite. Dans l'image du haut, la valeur du pixel est directement la distance en millimètres. Vous pouvez afficher cette distance en mettant la souris sur le pixel."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def show_oak_data(depth_data, rgb_data=None):\n",
    "    if rgb_data is not None:\n",
    "        fig, (ax1, ax2) = plt.subplots(2, 1)\n",
    "        im1 = ax1.imshow(depth_data, aspect='equal')\n",
    "        im2 = ax2.imshow(rgb_data, aspect='equal')\n",
    "\n",
    "        cbar_ax = fig.add_axes([0.85, 0.15, 0.05, 0.7])\n",
    "        fig.colorbar(im1, cax=cbar_ax)\n",
    "\n",
    "        plt.show()\n",
    "    else:\n",
    "        plt.imshow(depth_data, aspect='equal')\n",
    "        plt.colorbar()\n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Placer le robot environ 1 mètre devant un plan possèdant beaucoup de caractéristiques visuelles. Ceci peut-être un cahier, un échiquier, une structure en LEGO, etc. Appelons cette fonction avec les dernières données du capteur:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_data = oak_sensor.peek_depth()\n",
    "rgb_data = oak_sensor.peek_rgb()\n",
    "\n",
    "show_oak_data(depth_data, rgb_data)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "> **NOTE** N'oubliez pas de fermer le graphique interactif avec le bouton bleu.\n",
    "\n",
    "## Partie 2 - Conversion en nuage de points\n",
    "\n",
    "Dans cette partie, nous allons convertir des données de profondeur en $z$ en un nuage de points $x$ $y$ $z$.\n",
    "\n",
    "\n",
    "Nous avons déjà toutes les informations nécessaires. Le field-of-view de la Oak-D Lite se trouve dans les constantes ```OakLiteCamera.FOV_X``` et  ```OakLiteCamera.FOV_Y```. De même, le point principal se trouve dans les constantes ```OakLiteCamera.CENTER_X``` et  ```OakLiteCamera.CENTER_Y```. Ces informations nous permettent déjà de trouver les angles en $x$ et $y$ de chaque pixel par rapport à l'axe optique.\n",
    "\n",
    "\n",
    "La distance $z$ est connue, c'est ce qui est retourné directement par la Oak-D Lite. À partir des valeurs de `theta` et de $z$, on peut trouver les distances $dx$ et $dy$ qui représentent la composantes $x$ et $y$ du vecteur qui part de la caméra vers le point représenté à un pixel donné. Puis, avec ces distances, on peut trouver $x$ et $y$.\n",
    "\n",
    "> **NOTE** Avec numpy, si on a une matrice **a** de taille 3x3 et un vecteur ligne **b** de taille 1x3, `a * b` représente le fait de multiplier le vecteur `b` element-wise sur chaque ligne. On parle ici de broadcasting en vocabulaire numpy. Cela évite les boucles for, qui sont plus lente que des opérations numpy.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def to_points(depth_image, color_image=None, keep_invalid_points=False):\n",
    "    height, width = depth_image.shape\n",
    "\n",
    "    # Angle en x et y de chaque pixel par rapport à l'axe optique\n",
    "    theta_x = np.arctan((np.arange(width) - OakLiteCamera.CENTER_X_DEPTH) / OakLiteCamera.FOCAL_LENGTH)\n",
    "    theta_y = np.arctan((np.arange(height) - OakLiteCamera.CENTER_Y_DEPTH) / OakLiteCamera.FOCAL_LENGTH)\n",
    "    theta_x = np.expand_dims(theta_x, axis=0)  # Vecteur ligne pour que le broadcasting fonctionne sur deepth_image\n",
    "    theta_y = np.expand_dims(theta_y, axis=1)  # Vecteur colonne pour que le broadcasting fonctionne sur deepth_image\n",
    "\n",
    "    # Calculer la position de chaque point\n",
    "    x =  #TODO\n",
    "    y =  #TODO\n",
    "    z =  #TODO\n",
    "\n",
    "    # Met les matrices en colonne pour pouvoir les empiler\n",
    "    x_column = np.reshape(x, (-1, 1))\n",
    "    y_column = np.reshape(y, (-1, 1))\n",
    "    z_column = np.reshape(z, (-1, 1))\n",
    "\n",
    "    points = np.hstack((x_column, y_column, z_column))\n",
    "\n",
    "    points_to_keep_mask = np.ones(points.shape[0], dtype=bool)\n",
    "    if not keep_invalid_points:\n",
    "        MAX_DISTANCE = 1500\n",
    "        points_to_keep_mask = ~(abs(points) < 1e-6)[:, 2] & ~(abs(points) > MAX_DISTANCE)[:, 2]\n",
    "\n",
    "    # TODO ne conserver que les points où points_to_keep_mask est vrai\n",
    "    points =  #TODO\n",
    "\n",
    "    if color_image is not None:\n",
    "        color_image = cv2.resize(color_image, (width, height), interpolation=cv2.INTER_AREA)\n",
    "        colors = np.asarray(color_image).reshape((-1, 3))\n",
    "        colors = colors[points_to_keep_mask]\n",
    "        return points, colors\n",
    "    else:\n",
    "        return points"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Appelez la fonction que vous venez de définir avec une depth image et l'image rgb correspondante. Les array numpy retournées ont un point par ligne, respectivement x, y, z et r, g, b. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "points, colors = to_points(depth_data, rgb_data)\n",
    "print(points.shape, colors.shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pour visualizer le nuage de points en 3D, il suffit de créer un objet PointCloud et d'appeler la fonction save(). Cela va enregistrer sur le disque le nuage de points en format binaire pour un visualisateur web.\n",
    "\n",
    "> **Attention!** La fonction save() écrase le nuage de points précédemment sauvegardé\n",
    "\n",
    "Une fois la fonction save appelée, il suffit de cliquer sur le lien suivant pour ouvrir le visualisateur.\n",
    "\n",
    "### [POINT CLOUD VIEWER](point_cloud/viewer.html)\n",
    "\n",
    "> **NOTE** Si vous sauvegardez un nouveau nuage de points, il suffit de rafraîchir la page du visualisateur pour l'afficher.\n",
    "\n",
    "**Lancez un nouveau terminal et exécutez la commande suivante pour lancer un serveur web:**\n",
    "\n",
    "```bash\n",
    "ssh -L 8000:localhost:8000 norlab@192.168.0.101\n",
    "\n",
    "cd point_cloud/\n",
    "python3 -m http.server\n",
    "# Ouvrir http://127.0.0.1:8000/ dans un navigateur internet (ou http://0.0.0.0:8000/)\n",
    "# Cliquer sur viewer.html\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "PointCloud(points, colors).save()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 3 - Bruit de capteur\n",
    "### Selon z (profondeur)\n",
    "Les données obtenues avec la Oak-D Lite, comme avec tous autres capteurs, sont bruitées. De plus, les\n",
    "profondeurs seront quantifiées (ne prendront pas des valeurs continues mais vont varier par des sauts\n",
    "fixes), avec un pas variant avec la profondeur. Afin de mieux voir ces effets, placez la Oak-D Lite sur une\n",
    "boite à environ un mètre du mur tel qu’illustré ci-dessous. Assurez-vous, autant que possible, que le\n",
    "montage soit parallèle au mur.\n",
    "\n",
    "![Montage d'obsevation du bruit](./img/kinect-noise.png)\n",
    "\n",
    "Le code suivant affiche la valeur de $z$ au centre de l'image et sur les côtés. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_data = oak_sensor.peek_depth()\n",
    "\n",
    "y = math.floor(depth_data.shape[0] / 2)\n",
    "x_center = math.floor(depth_data.shape[1] / 2)\n",
    "x_left = math.floor(depth_data.shape[1] / 5)\n",
    "x_right = depth_data.shape[1] - x_left\n",
    "\n",
    "z_center = depth_data[y, x_center]\n",
    "z_left = depth_data[y, x_left]\n",
    "z_right = depth_data[y, x_right]\n",
    "\n",
    "print(\"Valeurs de z:\")\n",
    "print(\"Gauche: %1.8f mm | Centre: %1.8f mm | Droite: %1.8f mm\" % (z_left, z_center, z_right))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Répétez la démarche avec la Oak-D Lite à 0.5 mètre du mur, 1m, 3m et 5m. Est-ce que la précision sur les\n",
    "mesures/grandeurs des pas est la même pour de plus grandes distances? D’après vos observations, est-\n",
    "ce que la taille des sauts est \n",
    "- a) fixe?\n",
    "- b) croit linéairement avec $z$?\n",
    "- c) croit en $z^2$ ?\n",
    "\n",
    "### Selon x\n",
    "Le bruit dans une image produite\n",
    "par une caméra se présente sous\n",
    "plusieurs formes (e.g. distorsion\n",
    "radiale). Étant donné que le calcul\n",
    "de la position en x et en y pour\n",
    "chaque pixel dépend de la valeur\n",
    "bruitée en z et de la position du\n",
    "pixel dans l’image (elle aussi\n",
    "bruitée), il serait intéressant\n",
    "d’observer la précision des valeurs\n",
    "obtenues dans le nuage de point.\n",
    "\n",
    "\n",
    "La fonction suivant prend en argument un nuage de points une coordonnée en pixel $i$ (ligne) $j$ (colonne) et retourne la coordonnée $xyz$ correspondante. Vous pouvez trouver les valeurs de $i$ et $j$ grâce à la visualisation plus haut."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def point_at(points, i, j):\n",
    "    return points[i * 1920 + j]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Placez le coté d’une boite au centre\n",
    "optique horizontal de la caméra et\n",
    "utilisez des règles (voir la figure ci-\n",
    "dessous) pour mesurer la position\n",
    "relative réelle d’un repère (e.g. le\n",
    "coin inférieur droit de la boite).\n",
    "\n",
    "![Montage d'obsevation du bruit 2](./img/kinect-noise-2.png)\n",
    "\n",
    "En utilisant les fonctions ```point_at``` et ```to_points```, vérifier si la position mesurée selon x correspond à la valeur dans le nuage de\n",
    "point. Observez quelques mesures à cette position et répétez le processus pour une plus grande distance\n",
    "en x et en z. Est-ce que la précision est similaire dans les deux cas ?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_data = oak_sensor.peek_depth()\n",
    "rgb_data = oak_sensor.peek_rgb()\n",
    "\n",
    "show_oak_data(depth_data, rgb_data)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "i, j = 77, 424  #TODO\n",
    "points = to_points(depth_data, keep_invalid_points=True)  # keep_invalid_points is used keep 640x480 points\n",
    "# and be able index the points in i, j\n",
    "\n",
    "print(\"x, y, z: \" + str(point_at(points, i, j)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Partie 4 - Détection de plans avec RANSAC\n",
    "L’extraction de surfaces planes dans des nuages de points est une tâche clef dans le traitement et\n",
    "l’analyse de ces derniers. Dans un environnement intérieur, ces surfaces planes correspondront à des\n",
    "murs, au plancher, et à toutes autres surfaces planes présentes dans le champ visuel de la Oak-D Lite.\n",
    "Comme plusieurs équipes utiliseront la Oak-D Lite pour naviguer, une des premières tâches consiste à\n",
    "identifier le plancher dans un nuage de point. Cette méthode vous sera donc très utile pour vos projets!\n",
    "\n",
    "\n",
    "Dans la première moitié du cours, nous avons vu l’algorithme RANSAC. L’extraction de surface plane\n",
    "est une application intéressante de RANSAC, car vous cherchez une hypothèse (un plan en 3D) dans un\n",
    "ensemble de donné bruités. La méthode consiste à piger, au hasard, 3 points dans le nuage. Ces trois\n",
    "points définissent, de façon unique, un plan en 3D. S’il y a peu d’inliers à ce plan, cette hypothèse n’est\n",
    "donc pas valide. Si, par contre, beaucoup de points sont des inliers, alors il y a de fortes chances qu’ils\n",
    "constituent un plan.\n",
    "\n",
    "\n",
    "Pour comprendre la fonction suivante, veillez vous référez à [ce site](http://mathworld.wolfram.com/Point-PlaneDistance.html) qui décrit comment trouver la distance entre un plan et un point. Prenez également note qu' il est possible de calculer la distance de tous les points tout en même temps. En effet, dans le lien, on fait le produit scalaire en deux vecteur pour avoir la distance. Cette opération peut également se représenter comme un produit matriciel dans le cas de plusieurs points (la fonction ```numpy.dot```). \n",
    "\n",
    "\n",
    "Le code suivant retourne un masque de booléens représentant les points qui font partie du plan principal. Il est possible par la suite de retrouver ces points avec l'opérateur crochet \n",
    "```\n",
    "points[mask]\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def find_plane_ransac(points, n_trials=1000, epsilon=1e-2):\n",
    "    best_inliers, best_n_inliers = None, 0\n",
    "    for _ in range(n_trials):\n",
    "        #Select three points - the minimum number of points that define a plane\n",
    "        plane_points = points[np.random.choice(points.shape[0], 3, replace=False)]\n",
    "\n",
    "        #Define the plane by a unit vector\n",
    "        # TODO trouver le vecteur normal au plan defini par les trois points\n",
    "        normal_vector = np.cross(TODO, TODO)\n",
    "        normal_unit_vector =  # TODO normaliser le vecteur normal\n",
    "\n",
    "        #Compute distances between the points and the plane\n",
    "        distances =  # TODO - distances should be an array of (n_points, 1)\n",
    "\n",
    "        #Find inliers\n",
    "        inliers = abs(distances) < epsilon\n",
    "        n_inliers = np.count_nonzero(inliers)\n",
    "\n",
    "        if n_inliers > best_n_inliers:\n",
    "            best_inliers, best_n_inliers = inliers, n_inliers\n",
    "\n",
    "    return best_inliers, best_n_inliers"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extraction du plan principal\n",
    "\n",
    "Premièrement, allons chercher des données de la Oak-D Lite:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_data = oak_sensor.peek_depth()\n",
    "rgb_data = oak_sensor.peek_rgb()\n",
    "points, colors = to_points(depth_data, rgb_data)\n",
    "\n",
    "show_oak_data(depth_data, rgb_data)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Le défi est de trouver une bonne valeur de epsilon qui n'est ni trop permissive ni trop restrictive. Modifier la valeur d'epsilon dans le code suivant jusqu'à ce que vous soyez satisfaits du résultats dans le visualisateur de nuages de points."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "inliers, n_inliers = find_plane_ransac(points, 100, epsilon=2e-1)  #TODO change epsilon (in millimeters)\n",
    "print(\"Found %d inliers\" % n_inliers)\n",
    "\n",
    "points_of_plane = points[inliers]\n",
    "#Choose red color for the points of the principal plane\n",
    "points_of_plane_colors = np.tile(np.array([255, 0, 0], dtype=np.uint8), (n_inliers, 1))\n",
    "\n",
    "other_points = points[~inliers]  # ~inliers is the opposite boolean mask\n",
    "other_points_colors = colors[~inliers]\n",
    "\n",
    "# put all points in the same array\n",
    "all_points = np.vstack((points_of_plane, other_points))\n",
    "all_colors = np.vstack((points_of_plane_colors, other_points_colors))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "PointCloud(all_points, all_colors).save()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Extraction de $n$ plans\n",
    "\n",
    "Le code suivant extrait $n$ plans du nuage de points sans remise et les colore d'une couleur aléatoire."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def color_n_planes(points, n, ransac_params={}):\n",
    "    colored_points, colors = None, None\n",
    "    remaining_points = points\n",
    "\n",
    "    for i in range(n):\n",
    "        inliers, n_inliers = find_plane_ransac(remaining_points, **ransac_params)\n",
    "        points_of_plane = remaining_points[inliers]\n",
    "        remaining_points = remaining_points[~inliers]\n",
    "\n",
    "        random_color = np.random.randint(100, 200, size=[3], dtype=np.uint8)\n",
    "        new_colors = np.tile(random_color, (n_inliers, 1))\n",
    "\n",
    "        if colored_points is not None:\n",
    "            colored_points = np.vstack((colored_points, points_of_plane))\n",
    "            colors = np.vstack((colors, new_colors))\n",
    "        else:\n",
    "            colored_points = points_of_plane\n",
    "            colors = new_colors\n",
    "\n",
    "        if remaining_points.shape[0] < 3:\n",
    "            break\n",
    "\n",
    "    return colored_points, colors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Modifiez les valeurs de $epsilon$ et $n\\_trials$ et observez le résultat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_data = oak_sensor.peek_depth()\n",
    "rgb_data = oak_sensor.peek_rgb()\n",
    "points, colors = to_points(depth_data, rgb_data)\n",
    "\n",
    "show_oak_data(depth_data, rgb_data)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pc, cc = color_n_planes(points, 4, ransac_params={'n_trials': 50, 'epsilon': 2e-2})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "PointCloud(pc, cc).save()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}