{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import numpy as np\n",
    "from PIL import Image, ImageFile\n",
    "from skimage import io\n",
    "import matplotlib.pyplot as plt\n",
    "import tifffile\n",
    "from scipy import misc\n",
    "from tifffile import TiffFile\n",
    "from tifffile.tifffile import TIFF_DECOMPESSORS\n",
    "from visualization import ids_to_greyscale, load_depth, show_bounding_boxes, load_stencil_ids, load_stencil_flags, \\\n",
    "get_bounding_boxes, show_loaded_bounding_boxes, get_detections\n",
    "from math import tan, atan, radians, degrees"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2018-03-07--18-50-24--246\n"
     ]
    }
   ],
   "source": [
    "import visualization\n",
    "visualization.multi_page = False\n",
    "visualization.ini_file = 'gta-postprocessing.ini'\n",
    "\n",
    "conn = visualization.get_connection()\n",
    "cur = conn.cursor()\n",
    "cur.execute(\"\"\"SELECT snapshot_id, imagepath, cam_near_clip, cam_far_clip, timestamp, view_matrix, proj_matrix, world_matrix, \\\n",
    "    width, height, camera_fov, ui_width, ui_height,  \\\n",
    "    ARRAY[st_x(camera_relative_rotation), st_y(camera_relative_rotation), st_z(camera_relative_rotation)] as relative_cam_rot, \\\n",
    "    ARRAY[st_x(camera_pos), st_y(camera_pos), st_z(camera_pos)] as camera_pos \\\n",
    "    FROM snapshots \\\n",
    "    ORDER BY snapshot_id DESC \\\n",
    "    LIMIT 1 \\\n",
    "    OFFSET 3 \\\n",
    "    \"\"\")\n",
    "\n",
    "results = [dict(res) for res in cur]\n",
    "for i, res in enumerate(results):\n",
    "    res['view_matrix'] = np.array(res['view_matrix'])\n",
    "    res['proj_matrix'] = np.array(res['proj_matrix'])\n",
    "    res['world_matrix'] = np.array(res['world_matrix'])\n",
    "    res['camera_pos'] = np.array(res['camera_pos'])\n",
    "    results[i] = res\n",
    "\n",
    "res = results[0]\n",
    "name = res['imagepath']\n",
    "print(name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('off')\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "detections = get_detections(name)\n",
    "print('total detections: {}'.format(len(detections)))\n",
    "detections = [d for d in detections if np.linalg.norm(res['camera_pos'] - d['pos']) < 16]\n",
    "print('near detections: {}'.format(len(detections)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "for d in detections:\n",
    "    print('pos: {}, type: {}, class: {}, handle: {}, dist. {}'.format(d['pos'], d['type'], d['class'], d['handle'], np.linalg.norm(res['camera_pos'] - d['pos'])))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "l_car = detections[0]\n",
    "r_car = detections[3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'l_car' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-3-1d7cf65a37ef>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m      7\u001b[0m \u001b[0mview_matrix\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mres\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'view_matrix'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[0mproj_matrix\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mres\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'proj_matrix'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 9\u001b[1;33m \u001b[0ml_vec_pos\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0ml_car\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'pos'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     10\u001b[0m \u001b[0mr_vec_pos\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mr_car\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;34m'pos'\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     11\u001b[0m \u001b[0ml_vec\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0marray\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m[\u001b[0m\u001b[0ml_vec_pos\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0ml_vec_pos\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0ml_vec_pos\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mNameError\u001b[0m: name 'l_car' is not defined"
     ]
    }
   ],
   "source": [
    "width = res['width']\n",
    "height = res['height']\n",
    "size = (height, width)\n",
    "near_clip = res['cam_near_clip']\n",
    "far_clip = res['cam_far_clip']\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "proj_matrix = res['proj_matrix']\n",
    "l_vec_pos = l_car['pos']\n",
    "r_vec_pos = r_car['pos']\n",
    "l_vec = np.array([l_vec_pos[0], l_vec_pos[1], l_vec_pos[2], 1])\n",
    "r_vec = np.array([r_vec_pos[0], r_vec_pos[1], r_vec_pos[2], 1])\n",
    "\n",
    "print('original points')\n",
    "print(l_vec)\n",
    "print(r_vec)\n",
    "\n",
    "l_vec_camera = view_matrix @ l_vec\n",
    "r_vec_camera = view_matrix @ r_vec\n",
    "l_vec_camera /= l_vec_camera[3]\n",
    "r_vec_camera /= r_vec_camera[3]\n",
    "\n",
    "print('camera points')\n",
    "print(l_vec_camera)\n",
    "print(r_vec_camera)\n",
    "\n",
    "l_vec_ndc = proj_matrix @ l_vec_camera\n",
    "r_vec_ndc = proj_matrix @ r_vec_camera\n",
    "l_vec_ndc /= l_vec_ndc[3]\n",
    "r_vec_ndc /= r_vec_ndc[3]\n",
    "\n",
    "print('projected points')\n",
    "print(l_vec_ndc)\n",
    "print(r_vec_ndc)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def pixel_to_ndc(pixel, size):\n",
    "    p_y, p_x = pixel\n",
    "    s_y, s_x = size\n",
    "    return ((2/s_y)*p_y - 1, (2/s_x)*p_x - 1)\n",
    "\n",
    "def normalized_to_pixel(pixel, size):\n",
    "    p_y, p_x = pixel\n",
    "    s_y, s_x = size\n",
    "    return ((s_y/2)*p_y + (s_y/2), (s_x/2)*p_x + (s_x/2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "l_vec_point = normalized_to_pixel((l_vec_ndc[1], l_vec_ndc[0]), size)\n",
    "r_vec_point = normalized_to_pixel((r_vec_ndc[1], r_vec_ndc[0]), size)\n",
    "\n",
    "print('projected points')\n",
    "print(l_vec_point)\n",
    "print(r_vec_point)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('off')\n",
    "plt.plot([l_vec_point[1], r_vec_point[1]], [l_vec_point[0], r_vec_point[0]], 'o')\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### mapping all 4 cars"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "vecs_pos = [d['pos'] for d in detections]\n",
    "vecs = np.array([[vec[0], vec[1], vec[2], 1] for vec in vecs_pos]).T\n",
    "\n",
    "width = res['width']\n",
    "height = res['height']\n",
    "size = (height, width)\n",
    "near_clip = res['cam_near_clip']\n",
    "far_clip = res['cam_far_clip']\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "proj_matrix = res['proj_matrix']\n",
    "\n",
    "print('original points')\n",
    "print(vecs)\n",
    "\n",
    "vecs_camera = view_matrix @ vecs\n",
    "vecs_camera /= vecs_camera[3, :]\n",
    "\n",
    "print('camera points')\n",
    "print(vecs_camera)\n",
    "\n",
    "vecs_ndc = proj_matrix @ vecs_camera\n",
    "vecs_ndc /= vecs_ndc[3, :]\n",
    "\n",
    "print('projected points')\n",
    "vecs_ndc[1, :] *= -1 \n",
    "vecs_ndc[0, :] *= -1 \n",
    "print(vecs_ndc)\n",
    "\n",
    "points = []\n",
    "for vec in vecs_ndc.T:\n",
    "    points.append(normalized_to_pixel((vec[1], vec[0]), size))\n",
    "points = np.array(points)\n",
    "points = points.astype(int)\n",
    "    \n",
    "print('pixel points')\n",
    "print(points)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('off')\n",
    "plt.plot(points[:, 1], points[:, 0], 'o')\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "depth = load_depth(name)\n",
    "print(\"depth points\")\n",
    "print(\"expected:\")\n",
    "print(vecs_ndc[2, :])\n",
    "print(\"real:\")\n",
    "print(depth[(points[:, 0], points[:, 1])])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "depth = load_depth(name)\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('on')\n",
    "plt.imshow(depth, cmap='gray')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[  1.21006652e+00   4.02395929e-11   5.23221372e-09   2.33362242e-06]\n",
      " [  3.86116095e-10   2.14450701e+00   5.74842176e-10  -2.87629736e-06]\n",
      " [  8.21432196e-13  -2.34242218e-14   1.49965293e-04   1.50022490e+00]\n",
      " [  9.30889209e-18  -8.67361738e-19  -1.00000000e+00   1.42108547e-14]]\n",
      "[[  1.21006660e+00   0.00000000e+00  -0.00000000e+00   0.00000000e+00]\n",
      " [  0.00000000e+00   2.14450692e+00  -0.00000000e+00   0.00000000e+00]\n",
      " [  0.00000000e+00   0.00000000e+00   1.49977503e-04   1.50022497e+00]\n",
      " [  0.00000000e+00   0.00000000e+00  -1.00000000e+00   0.00000000e+00]]\n",
      "[[  7.78383344e-08  -4.02395929e-11  -5.23221372e-09  -2.33362242e-06]\n",
      " [ -3.86116095e-10  -8.44995416e-08  -5.74842176e-10   2.87629736e-06]\n",
      " [ -8.21432196e-13   2.34242218e-14   1.22102182e-08   6.30693917e-08]\n",
      " [ -9.30889209e-18   8.67361738e-19   0.00000000e+00  -1.42108547e-14]]\n",
      "[[False False False  True]\n",
      " [False False False  True]\n",
      " [False False False False]\n",
      " [False False False False]]\n"
     ]
    }
   ],
   "source": [
    "def construct_proj_matrix():\n",
    "    fov = 50.0\n",
    "    # for z coord\n",
    "    f = 1.5  # the near clip, but f in the book\n",
    "    n = 10003  # the far clip, somewhere in the ned of the world\n",
    "    # x coord\n",
    "    H = 1080\n",
    "    W = 1914\n",
    "    r = W*n*tan(radians(fov)/2)/H\n",
    "    l = -r\n",
    "    # y coord   \n",
    "    t = n*tan(radians(fov)/2)\n",
    "    b = -t\n",
    "    #x00 = 2*n/(r-l)\n",
    "    x00 = H/(tan(radians(fov)/2)*W)\n",
    "    #x11 = 2*n/(t-b)\n",
    "    x11 = 1/tan(radians(fov)/2)\n",
    "    return np.array([        \n",
    "        [x00, 0, -(r+l)/(r-l), 0],\n",
    "        [0, x11, -(t+b)/(t-b), 0],\n",
    "        [0, 0, -f/(f-n), -f*n/(f-n)],\n",
    "        [0, 0, -1, 0],\n",
    "    ])\n",
    "\n",
    "constructed = construct_proj_matrix()\n",
    "print(proj_matrix)\n",
    "print(constructed)\n",
    "print(constructed - proj_matrix)\n",
    "print(np.abs(constructed - proj_matrix) > 1e-6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "n: -10003.8140266\n",
      "f: 1.5004499186\n"
     ]
    }
   ],
   "source": [
    "x00 = proj_matrix[0, 0]\n",
    "x11 = proj_matrix[1, 1]\n",
    "x22 = proj_matrix[2, 2]\n",
    "x23 = proj_matrix[2, 3]\n",
    "\n",
    "n = -x23/x22\n",
    "f = -x23/(x22 - 1)\n",
    "\n",
    "print('n:', n)\n",
    "print('f:', f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "fov:  50.0\n",
      "t:  0.6994614872324979\n",
      "fov_h:  88.61111111111111\n",
      "r:  1.46407266619936\n",
      "W:  1914\n",
      "H:  1080\n",
      "near clip:  1.5\n",
      "x00_c:  1.0245393105342955\n",
      "x00_c2:  1.2100666009144845\n",
      "x11_c:  2.1445069205095586\n",
      "x00:  1.21006652308\n",
      "x11:  2.14450700501\n",
      "fov_c_v:  49.999998270568014\n",
      "fov_c_h:  88.61110804617331\n",
      "fov_c_h2:  79.14071211128197\n",
      "for height 1080, width is 682.3289390709872\n",
      "for height 1080, width is 4724.048641926896\n",
      "ui_width 1280\n",
      "width 1914\n",
      "aspect ration from matrix: 1.77222240605\n",
      "H/W: 0.5642633228840125\n",
      "aspect ration from db: 1.7722222222222221\n",
      "aspect ration from ui_db: 1.7777777777777777\n",
      "width by db ratio: 1914.00019854\n"
     ]
    }
   ],
   "source": [
    "fov = res['camera_fov']\n",
    "print('fov: ', fov)\n",
    "tan(radians(fov))\n",
    "n = 1.5\n",
    "t = n*tan(radians(fov)/2)\n",
    "print('t: ', t)\n",
    "W = res['width']\n",
    "H = res['height']\n",
    "fov_h = fov*W/H\n",
    "print('fov_h: ', fov_h)\n",
    "r = n*tan(radians(fov_h)/2)\n",
    "print('r: ', r)\n",
    "print('W: ', W)\n",
    "print('H: ', H)\n",
    "print('near clip: ', near_clip)\n",
    "print('x00_c: ', 1/tan(radians(fov_h)/2))\n",
    "print('x00_c2: ', H/(W*tan(radians(fov)/2)))\n",
    "print('x11_c: ', 1/tan(radians(fov)/2))\n",
    "print('x00: ', x00)\n",
    "print('x11: ', x11)\n",
    "\n",
    "fov_c_v = degrees(atan(1/x11))*2\n",
    "print('fov_c_v: ', fov_c_v)\n",
    "fov_c_h = fov_c_v*W/H\n",
    "print('fov_c_h: ', fov_c_h)\n",
    "fov_c_h2 = degrees(atan(1/x00))*2\n",
    "print('fov_c_h2: ', fov_c_h2)\n",
    "\n",
    "H=res['height']\n",
    "#H=res['ui_height']\n",
    "print('for height {}, width is {}'.format(H, (fov_c_v*H)/fov_c_h2))\n",
    "print('for height {}, width is {}'.format(H, (tan(radians(fov_c_h2))*H)/tan(radians(fov_c_v))))\n",
    "print('ui_width', res['ui_width'])\n",
    "print('width', res['width'])\n",
    "\n",
    "print('aspect ration from matrix:', x11 / x00)\n",
    "print('H/W:', H / W)\n",
    "print('aspect ration from db:', res['width'] / res['height'])\n",
    "print('aspect ration from ui_db:', res['ui_width'] / res['ui_height'])\n",
    "print('width by db ratio:', (x11 / x00)*res['height'])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
