calibration-d3.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 273,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 274,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Intrinsèques:\n",
      " [[  2.94904239e+03   0.00000000e+00   6.36206498e+02]\n",
      " [  0.00000000e+00   2.92091347e+03   3.60524151e+02]\n",
      " [  0.00000000e+00   0.00000000e+00   1.00000000e+00]]\n",
      "Extrinsèques:\n",
      " [[ -6.47344803e-03  -9.94407868e-01   1.05409139e-01  -9.00050268e+01]\n",
      " [  9.99973754e-01  -6.78032996e-03  -2.55324480e-03  -4.99835037e+01]\n",
      " [  3.25367546e-03   1.05389844e-01   9.94425661e-01   5.49953560e+02]]\n",
      "Simplifiée:\n",
      " [[ -1.70204632e+01  -2.86550125e+03   9.43516086e+02   8.44553886e+04]\n",
      " [  2.92200983e+03   1.81908270e+01   3.51056660e+02   5.22740510e+04]\n",
      " [  3.25367546e-03   1.05389844e-01   9.94425661e-01   5.49953560e+02]]\n"
     ]
    }
   ],
   "source": [
    "k = np.array([[2949.0423906949222, 0, 636.2064976797665],\n",
    "              [0, 2920.913467538854, 360.5241507687829],\n",
    "              [0, 0, 1]])\n",
    "e = np.array([[-0.006473448029271454, -0.9944078679641121, 0.10540913908044718, -90.00502680390417],\n",
    "              [0.9999737536888679, -0.006780329961701132, -0.002553244799415662, -49.98350367635707],\n",
    "              [0.003253675461321659, 0.10538984418187167, 0.9944256605394457, 549.9535595133096]])\n",
    "m = k.dot(e)\n",
    "print(\"Intrinsèques:\\n\", k)\n",
    "print(\"Extrinsèques:\\n\", e)\n",
    "print(\"Simplifiée:\\n\", m)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 275,
   "metadata": {},
   "outputs": [],
   "source": [
    "def position_from_uvh(u, v, h):\n",
    "    \"\"\"Gets (x, y, z) world position from (u, v) image position and height h from the origin plane.\"\"\"\n",
    "    x = ( (-m[0,3] + u*m[2,3])*(m[1,1] - v*m[2,1]) - (m[1,3] - v*m[2,3])*(-m[0,1] + u*m[2,1]) - h*( (m[0,1] - u*m[2,1])*(m[1,2] - v*m[2,2]) - (m[0,2] - u*m[2,2])*(m[1,1] - v*m[2,1]) ) ) /\\\n",
    "        ( (m[0,0] - u*m[2,0])*(m[1,1] - v*m[2,1]) + (m[0,1] - u*m[2,1])*(-m[1,0] + v*m[2,0]) )\n",
    "    y = ( (-m[0,3] + u*m[2,3])*(-m[1,0] + v*m[2,0]) - (m[1,3] - v*m[2,3])*(m[0,0] - u*m[2,0]) - h*( (m[0,2] - u*m[2,2])*(m[1,0] - v*m[2,0]) - (m[0,0] - u*m[2,0])*(m[1,2] - v*m[2,2]) ) ) /\\\n",
    "        ( (m[0,0] - u*m[2,0])*(m[1,1] - v*m[2,1]) + (m[0,1] - u*m[2,1])*(-m[1,0] + v*m[2,0]) )\n",
    "    z = h\n",
    "    return x, y, z\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 276,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(1.7515108694569148, -5.6168897381779033, 30)\n",
      "(-0.95919571071517085, 2.787563954861763, 0)\n"
     ]
    }
   ],
   "source": [
    "# Un rayon proche de l'origine monde, 30 de hauteur (unité non spécifiée) vs déposé sur de la table\n",
    "# (Imaginez les rayons projetés depuis la caméra, coupés par un plan qui s'élève de h depuis la table)\n",
    "print(position_from_uvh(139, 90, 30))\n",
    "print(position_from_uvh(139, 90, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 277,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(49.179426714923018, -93.97544697520101, 30)\n",
      "(49.272185000342375, -90.793869016263926, 0)\n"
     ]
    }
   ],
   "source": [
    "# Un rayon proche du centre de l'image\n",
    "print(position_from_uvh(636, 360, 30))\n",
    "print(position_from_uvh(636, 360, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 278,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(106.77314781696403, -190.58661496410599, 30)\n",
      "(110.27027187958375, -193.11572469305176, 0)\n"
     ]
    }
   ],
   "source": [
    "# Un rayon proche d'un coin de l'image\n",
    "print(position_from_uvh(1200, 700, 30))\n",
    "print(position_from_uvh(1200, 700, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 279,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "origine dans l'image:\n",
      " [ 153.56821881   95.05175493]\n",
      "position x,y,z (erreur de reprojection):\n",
      " (0.08857242946605226, -0.27496256301012018, 1)\n"
     ]
    }
   ],
   "source": [
    "# Calculer la position d'un point x,y,z dans l'image:\n",
    "uvs = m.dot([0, 0, 0, 1])\n",
    "origin_cam = uvs[:2] / uvs[2]\n",
    "print(\"origine dans l'image:\\n\", origin_cam)\n",
    "print(\"position x,y,z (erreur de reprojection):\\n\", position_from_uvh(origin_cam[0], origin_cam[1], 1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 280,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 37.77535826   0.74212577   3.14365882]\n"
     ]
    }
   ],
   "source": [
    "# Calcul d'un rayon projecteur\n",
    "a = np.linalg.lstsq(m, [153, 296, 1])[0]\n",
    "print(a[:3] / a[3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:Anaconda3]",
   "language": "python",
   "name": "conda-env-Anaconda3-py"
  },
  "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.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}