{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "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 *\n",
    "from gta_math import *\n",
    "import pickle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import visualization\n",
    "visualization.multi_page = False\n",
    "visualization.ini_file = 'gta-postprocessing.local.ini'\n",
    "\n",
    "scene_condition = \"\"\"\n",
    "    ( \\\n",
    "        SELECT scene_id \\\n",
    "        FROM snapshots \\\n",
    "        WHERE run_id = 6 \\\n",
    "        ORDER BY timestamp DESC \\\n",
    "        OFFSET 80 \\\n",
    "        LIMIT 1 \\\n",
    "    ) \\\n",
    "    \"\"\"\n",
    "\n",
    "scene_condition = \"\"\"\n",
    "    '1ca7f4b8-ef30-4a9f-8657-bc977dd04a89' \\\n",
    "    \"\"\"\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, \\\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",
    "    ARRAY[st_x(camera_rot), st_y(camera_rot), st_z(camera_rot)] as camera_rot, \\\n",
    "    ARRAY[st_x(camera_direction), st_y(camera_direction), st_z(camera_direction)] as camera_direction \\\n",
    "    FROM snapshots \\\n",
    "    WHERE scene_id = {} \\\n",
    "    ORDER BY snapshot_id ASC \\\n",
    "    \"\"\".format(scene_condition))\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'], dtype=np.float64)\n",
    "    res['world_matrix'] = np.array(res['world_matrix'], dtype=np.float64)\n",
    "    res['proj_matrix'] = np.array(res['proj_matrix'], dtype=np.float64)\n",
    "    res['relative_cam_rot'] = np.array(res['relative_cam_rot'], dtype=np.float64)\n",
    "    res['camera_pos'] = np.array(res['camera_pos'], dtype=np.float64)\n",
    "    res['camera_rot'] = np.array(res['camera_rot'], dtype=np.float64)\n",
    "    res['camera_direction'] = np.array(res['camera_direction'], dtype=np.float64)\n",
    "    results[i] = res\n",
    "\n",
    "names = [i['imagepath'] for i in results]\n",
    "print(names[0])\n",
    "\n",
    "for res in results:\n",
    "    res['detections'] = get_detections(res['imagepath'])\n",
    "\n",
    "with open('db_data.rick', mode='wb+') as file:\n",
    "    pickle.dump(results, file)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### loads pickled data instead of database"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "with open('db_data.rick', mode='rb') as file:\n",
    "    results = pickle.load(file)\n",
    "    print('I\\'m pickle Rick!!!')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### obtaining detections for this scene"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "detections = {}\n",
    "for res in results:\n",
    "    cur.execute(\"\"\"SELECT detection_id, type, class, bbox, \\\n",
    "        snapshot_id, handle, ARRAY[st_x(pos), st_y(pos), st_z(pos)] AS pos \\\n",
    "        FROM detections \\\n",
    "        WHERE snapshot_id = {} \\\n",
    "        AND type = 'car' \\\n",
    "        AND NOT bbox @> point '(Infinity, Infinity)' \\\n",
    "        \"\"\".format(res['snapshot_id']))\n",
    "    detections[res['snapshot_id']] = [dict(i) for i in cur]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "for res in results:  \n",
    "    print('detections for image {}:'.format(res['snapshot_id']))\n",
    "    for det in detections[res['snapshot_id']]:\n",
    "        print(det)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### showing rgb images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "for name in names:\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": "markdown",
   "metadata": {},
   "source": [
    "### showing depths"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "for name in names:\n",
    "    depth = load_depth(name)\n",
    "    fig = plt.figure(figsize=(12,12))\n",
    "    plt.axis('off')\n",
    "    plt.imshow(depth, cmap='gray')\n",
    "    plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### showing stencil ids"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "for name in names:\n",
    "    depth = load_stencil_ids(name)\n",
    "    fig = plt.figure(figsize=(12,12))\n",
    "    plt.axis('off')\n",
    "    plt.imshow(depth, cmap='gray')\n",
    "    plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### tinkering with depth, showing its values\n",
    "\n",
    "Depth is in NDC, openGL's Normalized Device Coordinates. \n",
    "\n",
    "According to [this thread](https://www.opengl.org/discussion_boards/showthread.php/170718-Normalized-Device-Coordinates) \n",
    "it maps from $[near clip (n_c), far clip(f_c)]$ to $[-1, 1]$.\n",
    "\n",
    "So the formula for calculating NDC from world coordinates (WC) is simple linear transformation:\n",
    "\n",
    "$ NDC = k \\cdot WC + q $\n",
    "\n",
    "$ -1 = k \\cdot n_c + q $\n",
    "\n",
    "$ 1 = k \\cdot f_c + q $\n",
    "\n",
    "After solving these equations, we obtain transformation parameters\n",
    "\n",
    "$ q = -1 - k \\cdot n_c $\n",
    "\n",
    "$ q = 1 - k \\cdot f_c $\n",
    "\n",
    "$ -1 - k \\cdot n_c = 1 - k \\cdot f_c $\n",
    "\n",
    "$ - k \\cdot n_c = 2 - k \\cdot f_c $\n",
    "\n",
    "$ -2 = k (n_c - f_c) $\n",
    "\n",
    "$ k = \\frac{-2}{n_c - f_c} $\n",
    "\n",
    "$ q = 1 - k \\cdot f_c $\n",
    "\n",
    "$ q = 1 - \\frac{-2}{n_c - f_c} \\cdot f_c $\n",
    "\n",
    "$ q = 1 + \\frac{2 f_c}{n_c - f_c} $\n",
    "\n",
    "So the resulting linear transformations is as follows\n",
    "\n",
    "$ NDC = \\frac{-2}{n_c - f_c} \\cdot WC + \\frac{2 f_c}{n_c - f_c} $\n",
    "\n",
    "And the reverse transformation, from NDC back to WC, shall be obtained by the same approach\n",
    "\n",
    "$ WC = k \\cdot NDC + q $\n",
    "\n",
    "$ n_c = k \\cdot -1 + q $\n",
    "\n",
    "$ f_c = k \\cdot 1 + q $\n",
    "\n",
    "$ n_c = -k + q $\n",
    "\n",
    "$ f_c = k + q $\n",
    "\n",
    "After solving these equations, we obtain transformation parameters\n",
    "\n",
    "$ q = n_c + k $\n",
    "\n",
    "$ q = f_c - k $\n",
    "\n",
    "$ n_c + k = f_c - k $\n",
    "\n",
    "$ n_c + 2k = f_c $\n",
    "\n",
    "$ k = \\frac{f_c - n_c}{2} $\n",
    "\n",
    "$ q = f_c - k $\n",
    "\n",
    "$ q = f_c - \\frac{f_c - n_c}{2} $\n",
    "\n",
    "$ q = f_c + \\frac{-f_c + n_c}{2} $\n",
    "\n",
    "$ q = \\frac{f_c + n_c}{2} $\n",
    "\n",
    "And resulting transformation is\n",
    "\n",
    "$ WC = \\frac{f_c - n_c}{2} \\cdot NDC + \\frac{f_c + n_c}{2} $\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For mapping from and to $[0, 1]$\n",
    "\n",
    "So the formula for calculating NDC from world coordinates (WC) is simple linear transformation:\n",
    "\n",
    "$ NDC = k \\cdot WC + q $\n",
    "\n",
    "$ 0 = k \\cdot n_c + q $\n",
    "\n",
    "$ 1 = k \\cdot f_c + q $\n",
    "\n",
    "After solving these equations, we obtain transformation parameters\n",
    "\n",
    "$ q = 0 - k \\cdot n_c $\n",
    "\n",
    "$ q = 1 - k \\cdot f_c $\n",
    "\n",
    "$ 0 - k \\cdot n_c = 1 - k \\cdot f_c $\n",
    "\n",
    "$ - k \\cdot n_c = 1 - k \\cdot f_c $\n",
    "\n",
    "$ -1 = k (n_c - f_c) $\n",
    "\n",
    "$ k = \\frac{-1}{n_c - f_c} $\n",
    "\n",
    "$ q = - k \\cdot f_c $\n",
    "\n",
    "$ q = - \\frac{-1}{n_c - f_c} \\cdot f_c $\n",
    "\n",
    "$ q = \\frac{f_c}{n_c - f_c} $\n",
    "\n",
    "So the resulting linear transformations is as follows\n",
    "\n",
    "$ NDC = \\frac{-1}{n_c - f_c} \\cdot WC + \\frac{f_c}{n_c - f_c} $\n",
    "\n",
    "And the reverse transformation, from NDC back to WC, shall be obtained by the same approach\n",
    "\n",
    "$ WC = k \\cdot NDC + q $\n",
    "\n",
    "$ n_c = k \\cdot 0 + q $\n",
    "\n",
    "$ f_c = k \\cdot 1 + q $\n",
    "\n",
    "$ n_c = q $\n",
    "\n",
    "$ f_c = k + q $\n",
    "\n",
    "After solving these equations, we obtain transformation parameters\n",
    "\n",
    "$ q = n_c $\n",
    "\n",
    "$ q = f_c - k $\n",
    "\n",
    "$ n_c = f_c - k $\n",
    "\n",
    "$ n_c + k = f_c $\n",
    "\n",
    "$ k = f_c - n_c $\n",
    "\n",
    "$ q = f_c - k $\n",
    "\n",
    "$ q = f_c - (f_c - n_c) $\n",
    "\n",
    "$ q = n_c $\n",
    "\n",
    "And resulting transformation is\n",
    "\n",
    "$ WC = (f_c - n_c) \\cdot NDC + n_c $\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "for res in results:    \n",
    "    name = res['imagepath']\n",
    "    depth = load_depth(name)\n",
    "    near_clip = res['cam_near_clip']\n",
    "    far_clip = res['cam_far_clip']\n",
    "    print('camera rot: {}'.format(res['relative_cam_rot']))\n",
    "    print('image name: {}'.format(res['imagepath']))\n",
    "    print('created: {}'.format(res['timestamp']))\n",
    "    print('depth min: {}'.format(np.min(depth)))\n",
    "    print('depth max: {}'.format(np.max(depth)))\n",
    "    print('near clip: {}'.format(near_clip))\n",
    "    print('far clip: {}'.format(far_clip))\n",
    "    \n",
    "    k = (far_clip - near_clip) / 2\n",
    "    q = (far_clip + near_clip) / 2\n",
    "    new_depth = depth * k + q\n",
    "    print('new depth min: {}'.format(np.min(new_depth)))\n",
    "    print('new depth max: {}'.format(np.max(new_depth)))\n",
    "    \n",
    "    # fig = plt.figure(figsize=(12,12))\n",
    "    # plt.axis('off')\n",
    "    # plt.imshow(new_depth, cmap='gray')\n",
    "    # plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### showing bounding boxes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "name = names[3]\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",
    "size = (im.size[1], im.size[0])\n",
    "show_bounding_boxes(name, size, plt.gca())\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### showing bounding boxes for all cameras"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "for name in names:\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",
    "    size = (im.size[1], im.size[0])\n",
    "    show_bounding_boxes(name, size, plt.gca())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "name = names[2]\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",
    "size = (im.size[1], im.size[0])\n",
    "detections = get_bounding_boxes(name)\n",
    "detections = [d for d in detections if (d['bbox'][0, 0] - d['bbox'][1, 0]) > 0.2]\n",
    "show_loaded_bounding_boxes(detections, size, plt.gca())\n",
    "for det in detections:\n",
    "    print('pos: {}, type: {}, class: {}, handle: {}'.format(det['pos'], det['type'], det['class'], det['handle']))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print('columns: '+', '.join(detections[0].keys()))\n",
    "for det in detections:\n",
    "    bbox = det['bbox']\n",
    "    print(bbox)\n",
    "    print(det['bbox'][0, 0] - det['bbox'][1, 0])\n",
    "    print(det['pos'])\n",
    "\n",
    "#detections = get_detections(name) ## this loads them from db, do not run\n",
    "detections = res['detections']\n",
    "# car 0 je to černé\n",
    "car_0 = [d for d in detections if d['handle'] == 53250][0]\n",
    "# car 1 je to bílé\n",
    "car_1 = [d for d in detections if d['handle'] == 52994][0]\n",
    "\n",
    "print('car_0 pos: '+ str(car_0['pos']))\n",
    "print('car_1 pos: '+ str(car_1['pos']))\n",
    "print('cars distance: '+ str(np.linalg.norm(car_0['pos'][0:2] - car_1['pos'][0:2])))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### playing with depth and bounding boxes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "name = names[3]\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",
    "size = (im.size[1], im.size[0])\n",
    "detections = get_bounding_boxes(name)\n",
    "print('detections before filtering: ' + str(len(detections)))\n",
    "# sizes = [str(d['bbox'][0, 0] - d['bbox'][1, 0]) for d in detections]\n",
    "# print('sizes: '+', '.join(sizes))\n",
    "orig_detections = detections\n",
    "detections = [d for d in detections if (d['bbox'][0, 0] - d['bbox'][1, 0]) > 0.2]\n",
    "show_loaded_bounding_boxes(detections, size, plt.gca())\n",
    "print('detections after filtering: ' + str(len(detections)))\n",
    "\n",
    "depth = load_depth(name)\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('on')\n",
    "# plt.gca().grid(color='r', linestyle='-', linewidth=1)\n",
    "point_0 = (400, 700)\n",
    "point_1 = (400, 900)\n",
    "plt.plot([point_0[1], point_1[1]], [point_0[0], point_1[0]], 'o')\n",
    "plt.imshow(depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "print('raw point 0: {}'.format(depth[point_0]))\n",
    "print('raw point 1: {}'.format(depth[point_1]))\n",
    "print('raw depth distance: '+ str(depth[point_1] - depth[point_0]))\n",
    "\n",
    "k = (far_clip - near_clip) / 2\n",
    "q = (far_clip + near_clip) / 2\n",
    "new_depth = depth * k + q\n",
    "print('-1,1 depth distance: {}'.format(new_depth[point_1] - new_depth[point_0]))\n",
    "\n",
    "k = far_clip - near_clip\n",
    "q = near_clip\n",
    "new_depth = depth * k + q\n",
    "print('0, 1 point 0: {}'.format(new_depth[point_0]))\n",
    "print('0, 1 point 1: {}'.format(new_depth[point_1]))\n",
    "print('0, 1 depth distance: {}'.format(new_depth[point_1] - new_depth[point_0]))\n",
    "\n",
    "c = 1\n",
    "new_depth = (np.exp(depth * np.log(far_clip * c + 1)) - 1)/c\n",
    "print('exp depth distance: {}'.format(new_depth[point_1] - new_depth[point_0]))\n",
    "new_depth = 1 / new_depth\n",
    "print('exp invered point 0: {}'.format(new_depth[point_0]))\n",
    "print('exp invered point 1: {}'.format(new_depth[point_1]))\n",
    "print('exp invered depth distance: {}'.format(new_depth[point_1] - new_depth[point_0]))\n",
    "\n",
    "res = results[3]\n",
    "print('keys: '+', '.join(res.keys()))\n",
    "print('view matrix:')\n",
    "print(np.array(res['view_matrix']))\n",
    "print('proj matrix:')\n",
    "print(np.array(res['proj_matrix']))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "depth[899, 800]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "depth[100, 1400]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(np.min(depth))\n",
    "print(np.max(depth))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(np.min(new_depth))\n",
    "print(np.max(new_depth))\n",
    "print(depth.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    " vec = (700/depth.shape[1], 500/depth.shape[0], depth[500, 700])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "submatrix = res['view_matrix'][0:3, 0:3]\n",
    "print(submatrix)\n",
    "np.linalg.det(submatrix)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "proj_matrix = res['proj_matrix']\n",
    "vec = np.linalg.inv(proj_matrix) @ np.array([[1], [1], [0.002], [1]])\n",
    "print(vec / vec[3])\n",
    "print(res['cam_near_clip'])\n",
    "print(res['cam_far_clip'])\n",
    "\n",
    "print('proj_matrix')\n",
    "print(proj_matrix)\n",
    "print('inverted proj_matrix')\n",
    "print(np.linalg.inv(proj_matrix))\n",
    "\n",
    "print('rounding small values to zero')\n",
    "proj_matrix[np.abs(proj_matrix) < 1e-7] = 0\n",
    "\n",
    "print('proj_matrix')\n",
    "print(proj_matrix)\n",
    "print('inverted proj_matrix')\n",
    "print(np.linalg.inv(proj_matrix))\n",
    "\n",
    "\n",
    "el1 = (- far_clip - near_clip)/(2*(near_clip*far_clip))\n",
    "el2 = (far_clip + near_clip)/(2*(near_clip*far_clip))\n",
    "print(el1)\n",
    "print(el2)\n",
    "\n",
    "matrix = np.zeros((4, 4))\n",
    "matrix[2, 3] = -1\n",
    "matrix[3, 2] = el1\n",
    "matrix[3, 3] = el2\n",
    "print(matrix)\n",
    "vec = matrix @ np.array([[1], [1], [0], [1]])\n",
    "print(vec / vec[3])\n",
    "print(proj_matrix[0,0])\n",
    "print(1/ np.linalg.inv(proj_matrix)[0,0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "vec = np.array([1, 1, 1, 1]) @ np.linalg.inv(np.array(res['proj_matrix']))\n",
    "vec / vec[3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(res['cam_near_clip'])\n",
    "print(res['cam_far_clip'])\n",
    "\n",
    "print('transforming near clip')\n",
    "proj_matrix = res['proj_matrix']\n",
    "vec = proj_matrix @ np.array([[1], [1], [-res['cam_near_clip']], [1]])\n",
    "print(vec)\n",
    "print(vec / vec[3])\n",
    "\n",
    "print('transforming far clip')\n",
    "proj_matrix = res['proj_matrix']\n",
    "vec = proj_matrix @ np.array([[1], [1], [-res['cam_far_clip']], [1]])\n",
    "print(vec)\n",
    "print(vec / vec[3])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### preparing correct form of coordinates for inverse projection"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "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",
    "# last column form\n",
    "ndc_0 = pixel_to_normalized(point_0, size)\n",
    "ndc_1 = pixel_to_normalized(point_1, size)\n",
    "vec_0 = [ndc_0[1], ndc_0[0], -depth[point_0], 1]\n",
    "vec_1 = [ndc_1[1], ndc_1[0], -depth[point_1], 1]\n",
    "vec_0 = np.array(vec_0)[:, np.newaxis]\n",
    "vec_1 = np.array(vec_1)[:, np.newaxis]\n",
    "\n",
    "print('constructed points')\n",
    "print(vec_0)\n",
    "print(vec_1)\n",
    "\n",
    "vec_0_p = np.linalg.inv(proj_matrix) @ vec_0\n",
    "vec_1_p = np.linalg.inv(proj_matrix) @ vec_1\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('normalized')\n",
    "vec_0_p /= vec_0_p[3]\n",
    "vec_1_p /= vec_1_p[3]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "result = np.linalg.inv(proj_matrix) @ np.array([[1], [1], [0.172532], [1]])\n",
    "result /= result[3]\n",
    "print(result)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### transformation for two points at once"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "point_0 = (400, 700)\n",
    "point_1 = (400, 900)\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",
    "# last column form\n",
    "ndc_0 = pixel_to_normalized(point_0, size)\n",
    "ndc_1 = pixel_to_normalized(point_1, size)\n",
    "vec_0 = [ndc_0[1], ndc_0[0], -depth[point_0], 1]\n",
    "vec_1 = [ndc_1[1], ndc_1[0], -depth[point_1], 1]\n",
    "vec_0 = np.array(vec_0)[:, np.newaxis]\n",
    "vec_1 = np.array(vec_1)[:, np.newaxis]\n",
    "\n",
    "print('constructed points')\n",
    "print(vec_0)\n",
    "print(vec_1)\n",
    "\n",
    "vecs = np.concatenate((vec_0, vec_1), axis=1)\n",
    "print('merged')\n",
    "print(vecs)\n",
    "\n",
    "proj_matrix = res['proj_matrix']\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('normalized')\n",
    "vecs_p /= vecs_p[3, :]\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "vecs_p = np.linalg.inv(view_matrix) @ vecs_p\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by view')\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### showing depth data histogram"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "depth_to_hist = depth.flatten()\n",
    "print(depth_to_hist.shape)\n",
    "#plt.hist()\n",
    "plt.hist(np.random.choice(depth_to_hist, 400000), bins=400)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### calculating zero and non-zero values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print('depth shape:')\n",
    "print(depth.shape)\n",
    "print('depth size:')\n",
    "print(depth.size)\n",
    "print('zero values:')\n",
    "print(depth.size - np.count_nonzero(depth))\n",
    "print('non-zero values:')\n",
    "print(np.count_nonzero(depth))\n",
    "\n",
    "print(depth[0:10, 0:10])\n",
    "\n",
    "print(depth[0, 2] == 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### all pixels to pointcloud"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# all pixels to vectors, or some subset\n",
    "range_y = range(height)\n",
    "range_x = range(width)\n",
    "\n",
    "range_y = range(200, 800)\n",
    "range_x = range(400, 700)\n",
    "\n",
    "'''\n",
    "vecs = np.zeros((4, len(range_y) * len(range_x)))\n",
    "\n",
    "print(vecs.shape)\n",
    "i = 0\n",
    "#for y in range(height):\n",
    "#    for x in range(width):\n",
    "for y in range_y:\n",
    "    for x in range_x:\n",
    "        ndc = pixel_to_normalized((y, x), size)\n",
    "        vec = [ndc[1], ndc[0], -depth[(y, x)], 1]\n",
    "        vec = np.array(vec)\n",
    "        vecs[:, i] = vec\n",
    "        i += 1\n",
    "'''\n",
    "\n",
    "# only non-zero pixels to pointcloud\n",
    "vecs = np.zeros((4, np.count_nonzero(depth)))\n",
    "i = 0\n",
    "#for y in range(height):\n",
    "#    for x in range(width):\n",
    "\n",
    "for y, x in np.transpose(np.nonzero(depth)):\n",
    "    ndc = pixel_to_normalized((y, x), size)\n",
    "    vec = [ndc[1], ndc[0], -depth[(y, x)], 1]\n",
    "    vec = np.array(vec)\n",
    "    vecs[:, i] = vec\n",
    "    i += 1\n",
    "\n",
    "\n",
    "print(\"prepared, total points: \"+str(vecs.shape[1]))\n",
    "\n",
    "# projection itself\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print(\"projected\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "# transformation to pointcloud form\n",
    "xs = vecs_p[0, ::50]\n",
    "ys = vecs_p[1, ::50]\n",
    "zs = vecs_p[2, ::50]\n",
    "\n",
    "# visualization\n",
    "from mpl_toolkits.mplot3d import axes3d, Axes3D #<-- Note the capitalization! \n",
    "fig = plt.figure()\n",
    "\n",
    "ax = Axes3D(fig) #<-- Note the difference from your original code...\n",
    "#ax = fig.add_subplot(111, projection='3d')\n",
    "ax.scatter(xs, ys, zs, c='b', marker='o')\n",
    "\n",
    "ax.set_xlabel('X Label')\n",
    "ax.set_ylabel('Y Label')\n",
    "ax.set_zlabel('Z Label')\n",
    "\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### trying to project cars positions in world coordinates to NDC"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(\"cars positions: world\")\n",
    "car_0_pos = car_0['pos']\n",
    "car_1_pos = car_1['pos']\n",
    "car_0_pos = np.append(car_0_pos, 1)\n",
    "car_1_pos = np.append(car_1_pos, 1)\n",
    "print(car_0_pos)\n",
    "print(car_1_pos)\n",
    "\n",
    "print(\"cars positions: viewed\")\n",
    "view_matrix = res['view_matrix']\n",
    "car_0_pos = view_matrix @ car_0_pos\n",
    "car_1_pos = view_matrix @ car_1_pos\n",
    "car_0_pos /= car_0_pos[3]\n",
    "car_1_pos /= car_1_pos[3]\n",
    "print(car_0_pos)\n",
    "print(car_1_pos)\n",
    "\n",
    "print(\"cars positions: projected\")\n",
    "proj_matrix = res['proj_matrix']\n",
    "car_0_pos = proj_matrix @ car_0_pos\n",
    "car_1_pos = proj_matrix @ car_1_pos\n",
    "car_0_pos /= car_0_pos[3]\n",
    "car_1_pos /= car_1_pos[3]\n",
    "print(car_0_pos)\n",
    "print(car_1_pos)\n",
    "\n",
    "print(\"cars bboxes\")\n",
    "car_0_bbox = car_0['bbox']\n",
    "car_1_bbox = car_1['bbox']\n",
    "print(car_0_bbox)\n",
    "print(car_1_bbox)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "np.append(car_0_pos, 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# projecting multiple points at once and plotting them"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### preparing points"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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",
    "points = np.array([\n",
    "    (400, 700),\n",
    "    (400, 800),\n",
    "    (400, 900),\n",
    "    (500, 650),\n",
    "    (600, 650),\n",
    "    (700, 650),\n",
    "    (500, 800),\n",
    "    (600, 800),\n",
    "    (700, 800),\n",
    "])\n",
    "\n",
    "colors = [\n",
    "    'r',\n",
    "    'r',\n",
    "    'b',\n",
    "    'b',\n",
    "    'b',\n",
    "    'b',\n",
    "    'b',\n",
    "    'b',\n",
    "    'b',\n",
    "]\n",
    "\n",
    "y_range = range(0, 900)\n",
    "x_range = range(0, 1600)\n",
    "points = np.transpose([np.tile(y_range, len(x_range)), np.repeat(x_range, len(y_range))])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### plotting them on the depth image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "\n",
    "import matplotlib as mpl\n",
    "mpl.rcParams['savefig.dpi'] = 80\n",
    "mpl.rcParams['figure.dpi'] = 80\n",
    "\n",
    "name = names[3]\n",
    "depth = load_depth(name)\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('on')\n",
    "# plt.gca().grid(color='r', linestyle='-', linewidth=1)\n",
    "#for point, color in zip(points, colors):\n",
    "#    plt.plot(point[1], point[0], 'o', color=color)\n",
    "#plt.plot(points[:, 1], points[:, 0], 'o', color=colors)\n",
    "plt.plot(points[:, 1], points[:, 0], 'o')\n",
    "plt.imshow(depth, cmap='gray')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### transforming and projecting them"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# when cutting too far points, I project far clip to depth and then use its value to treshold:\n",
    "print('transforming far clip')\n",
    "proj_matrix = res['proj_matrix']\n",
    "max_depth = res['cam_far_clip']\n",
    "#max_depth = 60 # just for testing\n",
    "vec = proj_matrix @ np.array([[1], [1], [-max_depth], [1]])\n",
    "#print(vec)\n",
    "vec /= vec[3]\n",
    "treshold = vec[2]\n",
    "\n",
    "# vecs = np.zeros((4, points.shape[0]))\n",
    "vecs = np.zeros((4, len(np.where(depth[points[:, 0], points[:, 1]] > treshold)[0])))  # this one is used when ommiting 0 depth (point behind the far clip)\n",
    "print(\"vecs.shape\")\n",
    "print(vecs.shape)\n",
    "i = 0\n",
    "arr = points\n",
    "for y, x in arr:\n",
    "    if depth[(y, x)] <= treshold:\n",
    "        continue\n",
    "    ndc = pixel_to_normalized((y, x), size)\n",
    "    vec = [ndc[1], ndc[0], -depth[(y, x)], 1]\n",
    "    vec = np.array(vec)\n",
    "    vecs[:, i] = vec\n",
    "    i += 1\n",
    "\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('normalized')\n",
    "vecs_p /= vecs_p[3, :]\n",
    "print(vecs_p)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### plotting them projected"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "# transformation to pointcloud form\n",
    "xs = vecs_p[0, :]\n",
    "ys = vecs_p[1, :]\n",
    "zs = vecs_p[2, :]\n",
    "\n",
    "# visualization\n",
    "from mpl_toolkits.mplot3d import axes3d, Axes3D #<-- Note the capitalization! ,\n",
    "fig = plt.figure()\n",
    "\n",
    "ax = Axes3D(fig)\n",
    "#ax = fig.add_subplot(111, projection='3d')\n",
    "#for i in range(vecs_p.shape[1]):\n",
    "#    ax.scatter(xs[i], ys[i], zs[i], c=colors[i], marker='o')\n",
    "ax.scatter(xs, ys, zs, c='b', marker='o')\n",
    "\n",
    "ax.set_xlabel('X Label')\n",
    "ax.set_ylabel('Y Label')\n",
    "ax.set_zlabel('Z Label')\n",
    "\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### pickle them for plotting them later and elsewhere"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import pickle\n",
    "with open('points.rick', mode='wb+') as file:\n",
    "    struct = {\n",
    "        'points': points, \n",
    "        'vecs_p': vecs_p,\n",
    "        'colors': colors,\n",
    "        'name': name\n",
    "    }\n",
    "    pickle.dump(struct, file)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "a = np.asarray(vecs_p[0:3, :].T)\n",
    "np.savetxt(\"points.csv\", a, delimiter=\",\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### examining same points in two images"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "point_0 = (400, 700)\n",
    "point_1 = (400, 900)\n",
    "\n",
    "name = names[3]\n",
    "res = results[3]\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([point_0[1], point_1[1]], [point_0[0], point_1[0]], 'o')\n",
    "plt.imshow(im)\n",
    "\n",
    "depth = load_depth(name)\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('on')\n",
    "plt.plot([point_0[1], point_1[1]], [point_0[0], point_1[0]], 'o')\n",
    "# plt.gca().grid(color='r', linestyle='-', linewidth=1)\n",
    "plt.imshow(depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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",
    "# last column form\n",
    "ndc_0 = pixel_to_normalized(point_0, size)\n",
    "ndc_1 = pixel_to_normalized(point_1, size)\n",
    "vec_0 = [ndc_0[1], ndc_0[0], -depth[point_0], 1]\n",
    "vec_1 = [ndc_1[1], ndc_1[0], -depth[point_1], 1]\n",
    "vec_0 = np.array(vec_0)[:, np.newaxis]\n",
    "vec_1 = np.array(vec_1)[:, np.newaxis]\n",
    "\n",
    "print('constructed points')\n",
    "print(vec_0)\n",
    "print(vec_1)\n",
    "\n",
    "vecs = np.concatenate((vec_0, vec_1), axis=1)\n",
    "print('merged')\n",
    "print(vecs)\n",
    "\n",
    "proj_matrix = res['proj_matrix']\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('normalized')\n",
    "vecs_p /= vecs_p[3, :]\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "vecs_p = np.linalg.inv(view_matrix) @ vecs_p\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by view')\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "point_0 = (400, 650)\n",
    "point_1 = (430, 1180)\n",
    "name = names[2]\n",
    "res = results[2]\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.plot([point_0[1], point_1[1]], [point_0[0], point_1[0]], 'o')\n",
    "plt.axis('off')\n",
    "plt.imshow(im)\n",
    "\n",
    "depth = load_depth(name)\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.axis('on')\n",
    "# plt.gca().grid(color='r', linestyle='-', linewidth=1)\n",
    "plt.plot([point_0[1], point_1[1]], [point_0[0], point_1[0]], 'o')\n",
    "plt.imshow(depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "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",
    "# last column form\n",
    "ndc_0 = pixel_to_normalized(point_0, size)\n",
    "ndc_1 = pixel_to_normalized(point_1, size)\n",
    "vec_0 = [ndc_0[1], ndc_0[0], -depth[point_0], 1]\n",
    "vec_1 = [ndc_1[1], ndc_1[0], -depth[point_1], 1]\n",
    "vec_0 = np.array(vec_0)[:, np.newaxis]\n",
    "vec_1 = np.array(vec_1)[:, np.newaxis]\n",
    "\n",
    "print('constructed points')\n",
    "print(vec_0)\n",
    "print(vec_1)\n",
    "\n",
    "vecs = np.concatenate((vec_0, vec_1), axis=1)\n",
    "print('merged')\n",
    "print(vecs)\n",
    "\n",
    "proj_matrix = res['proj_matrix']\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('normalized')\n",
    "vecs_p /= vecs_p[3, :]\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "vecs_p = np.linalg.inv(view_matrix) @ vecs_p\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by view')\n",
    "print(vecs_p)\n",
    "\n",
    "print('points')\n",
    "vec_0_p = vecs_p[:, 0] \n",
    "vec_1_p = vecs_p[:, 1]\n",
    "print(vec_0_p)\n",
    "print(vec_1_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vec_0_p - vec_1_p))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "name = names[3]\n",
    "res = results[3]\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",
    "points = np.array([\n",
    "    [400, 700],\n",
    "    [400, 900]\n",
    "])\n",
    "vecs, _ = points_to_homo(points, res, load_depth(name))\n",
    "\n",
    "print('constructed points')\n",
    "print(vecs)\n",
    "\n",
    "proj_matrix = res['proj_matrix']\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vecs_p[:, 0] - vecs_p[:, 1]))\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "vecs_p = np.linalg.inv(view_matrix) @ vecs_p\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by view')\n",
    "print(vecs_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vecs_p[:, 0] - vecs_p[:, 1]))\n",
    "\n",
    "print('view matrix')\n",
    "print(view_matrix)\n",
    "\n",
    "print()\n",
    "\n",
    "name = names[2]\n",
    "res = results[2]\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",
    "points = np.array([\n",
    "    [400, 650],\n",
    "    [430, 1180]\n",
    "])\n",
    "vecs, _ = points_to_homo(points, res, load_depth(name))\n",
    "\n",
    "print('constructed points')\n",
    "print(vecs)\n",
    "\n",
    "proj_matrix = res['proj_matrix']\n",
    "vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by inversion')\n",
    "print(vecs_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vecs_p[:, 0] - vecs_p[:, 1]))\n",
    "\n",
    "view_matrix = res['view_matrix']\n",
    "vecs_p = np.linalg.inv(view_matrix) @ vecs_p\n",
    "vecs_p /= vecs_p[3, :]\n",
    "\n",
    "print('projected by view')\n",
    "print(vecs_p)\n",
    "\n",
    "print('distance')\n",
    "print(np.linalg.norm(vecs_p[:, 0] - vecs_p[:, 1]))\n",
    "\n",
    "print('view matrix')\n",
    "print(view_matrix)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### examining one point from 2 cameras"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "\n",
    "point2 = (440, 972)\n",
    "name = names[2]\n",
    "res = results[2]\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.plot(point2[1], point2[0], 'o')\n",
    "plt.axis('off')\n",
    "plt.imshow(im)\n",
    "\n",
    "point3 = (422, 435)\n",
    "name = names[3]\n",
    "res = results[3]\n",
    "im = Image.open(os.path.join(visualization.get_in_directory(), name + '.tiff'))\n",
    "fig = plt.figure(figsize=(12,12))\n",
    "plt.plot(point3[1], point3[0], 'o')\n",
    "plt.axis('off')\n",
    "plt.imshow(im)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def show_view_matrix(view_matrix):\n",
    "    print('view matrix')\n",
    "    print(view_matrix)\n",
    "    print('view translation')\n",
    "    print(view_matrix[0:3, 3])\n",
    "    print('view rotation')\n",
    "    print(view_matrix[0:3, 0:3])\n",
    "    print('determinant')\n",
    "    print(np.linalg.det(view_matrix[0:3, 0:3]))\n",
    "    print('euler angles')\n",
    "    print(rotationMatrixToEulerAngles(view_matrix[0:3, 0:3]))\n",
    "\n",
    "def use_inv_proj(proj_matrix, vecs):\n",
    "    vecs_p = np.linalg.inv(proj_matrix) @ vecs\n",
    "    vecs_p /= vecs_p[3, :]\n",
    "    print('projected by inversion')\n",
    "    print(vecs_p)\n",
    "    return vecs_p\n",
    "\n",
    "def use_inv_view(view_matrix, vecs_p):\n",
    "    vecs_w = np.linalg.inv(view_matrix) @ vecs_p\n",
    "    vecs_w /= vecs_p[3, :]\n",
    "    print('projected by view')\n",
    "    print(vecs_w)\n",
    "    show_view_matrix(view_matrix)\n",
    "    return vecs_w\n",
    "    \n",
    "def use_inv_world(world_matrix, vecs_p):\n",
    "    vecs_w = np.linalg.inv(world_matrix) @ vecs_p\n",
    "    vecs_w /= vecs_p[3, :]\n",
    "    print('projected by world')\n",
    "    print(vecs_w)\n",
    "    print('world matrix')\n",
    "    print(world_matrix)\n",
    "    return vecs_w\n",
    "    \n",
    "def is_rotation_matrix(R) :\n",
    "    Rt = np.transpose(R)\n",
    "    shouldBeIdentity = np.dot(Rt, R)\n",
    "    I = np.identity(3, dtype = R.dtype)\n",
    "    n = np.linalg.norm(I - shouldBeIdentity)\n",
    "    return n < 1e-6\n",
    "\n",
    "def create_rot_matrix(euler):\n",
    "    x = np.radians(euler.x)\n",
    "    y = np.radians(euler.y)\n",
    "    z = np.radians(euler.z)\n",
    "    Rx = np.array([[1, 0, 0],\n",
    "                   [0, cos(x), -sin(x)],\n",
    "                   [0, sin(x), cos(x)]], dtype=np.float)\n",
    "    Ry = np.array([[cos(y), 0, sin(y)],\n",
    "                   [0, 1, 0],\n",
    "                   [-sin(y), 0, cos(y)]], dtype=np.float)\n",
    "    Rz = np.array([[cos(z), -sin(z), 0],\n",
    "                   [sin(z), cos(z), 0],\n",
    "                   [0, 0, 1]], dtype=np.float)\n",
    "    result = Rz @ Ry @ Rx\n",
    "    return result\n",
    "\n",
    "def rotationMatrixToEulerAngles(R) :\n",
    "    assert(is_rotation_matrix(R))\n",
    "    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])\n",
    "    singular = sy < 1e-6\n",
    "    if not singular :\n",
    "        x = math.atan2(R[2,1] , R[2,2])\n",
    "        y = math.atan2(-R[2,0], sy)\n",
    "        z = math.atan2(R[1,0], R[0,0])\n",
    "    else :\n",
    "        x = math.atan2(-R[1,2], R[1,1])\n",
    "        y = math.atan2(-R[2,0], sy)\n",
    "        z = 0\n",
    "    return np.array([x, y, z])\n",
    "\n",
    "def use_inv_view_new(res, vecs_p):\n",
    "    view_matrix_new = np.zeros((4, 4))\n",
    "    view_matrix_new[0:3, 3] = res['camera_pos']\n",
    "    view_matrix_new[0:3, 0:3] = create_rot_matrix(res['camera_direction'])\n",
    "    view_matrix_new[3, 3] = 1\n",
    "\n",
    "    vecs_w_new = np.linalg.inv(view_matrix_new) @ vecs_p\n",
    "    vecs_w_new /= vecs_w_new[3, :]\n",
    "\n",
    "    print('projected by view new')\n",
    "    print(vecs_w_new)\n",
    "    show_view_matrix(view_matrix_new)\n",
    "    return vecs_w_new\n",
    "    \n",
    "name = names[2]\n",
    "res = results[2]\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",
    "proj_matrix = res['proj_matrix']\n",
    "view_matrix = res['view_matrix']\n",
    "world_matrix = res['world_matrix']\n",
    "\n",
    "vecs, _ = points_to_homo(np.array([point2]), res, name)\n",
    "\n",
    "print('constructed points')\n",
    "print(vecs)\n",
    "\n",
    "vecs_p = use_inv_proj(proj_matrix, vecs)\n",
    "vecs_w = use_inv_view(view_matrix, vecs_p)\n",
    "vecs_ww = use_inv_world(world_matrix, vecs_w)\n",
    "# vecs_w_new = use_inv_view_new(res, vecs_p)\n",
    "\n",
    "point2world = vecs_w.T[0:3]\n",
    "# point2world_new = vecs_w_new.T[0:3]\n",
    "\n",
    "print()\n",
    "\n",
    "name = names[3]\n",
    "res = results[3]\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",
    "proj_matrix = res['proj_matrix']\n",
    "view_matrix = res['view_matrix']\n",
    "world_matrix = res['world_matrix']\n",
    "\n",
    "vecs, _ = points_to_homo(np.array([point3]), res, name)\n",
    "\n",
    "print('constructed points')\n",
    "print(vecs)\n",
    "\n",
    "vecs_p = use_inv_proj(proj_matrix, vecs)\n",
    "vecs_w = use_inv_view(view_matrix, vecs_p)\n",
    "vecs_w = use_inv_view(view_matrix, vecs_p)\n",
    "vecs_ww = use_inv_world(world_matrix, vecs_w)\n",
    "# vecs_w_new = use_inv_view_new(res, vecs_p)\n",
    "\n",
    "point3world = vecs_w.T[0:3]\n",
    "# point3world_new = vecs_w.T[0:3]\n",
    "\n",
    "print()\n",
    "\n",
    "print('distance, should be zero')\n",
    "print(point2world - point3world)\n",
    "print(np.linalg.norm(point2world - point3world))\n",
    "\n",
    "# print('new distance, should be zero')\n",
    "# print(point2world_new - point3world_new)\n",
    "# print(np.linalg.norm(point2world_new - point3world_new))\n",
    "\n",
    "cam2world = results[2]['camera_pos']\n",
    "cam3world = results[3]['camera_pos']\n",
    "print('cameras positions')\n",
    "print(cam2world)\n",
    "print(cam3world)\n",
    "print('cameras distance')\n",
    "print(cam2world - cam3world)\n",
    "print(np.linalg.norm(cam2world - cam3world))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "res = results[2]\n",
    "proj_matrix = res['proj_matrix']\n",
    "view_matrix = res['view_matrix']\n",
    "world_matrix = res['world_matrix']\n",
    "print(world_matrix)\n",
    "print('determinant')\n",
    "print(np.linalg.det(world_matrix[0:3, 0:3]))\n",
    "view_2 = view_matrix\n",
    "\n",
    "res = results[3]\n",
    "proj_matrix = res['proj_matrix']\n",
    "view_matrix = res['view_matrix']\n",
    "world_matrix = res['world_matrix']\n",
    "print(world_matrix)\n",
    "print('determinant')\n",
    "print(np.linalg.det(world_matrix[0:3, 0:3]))\n",
    "view_3 = view_matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "print(view_2)\n",
    "print(np.linalg.det(view_2[0:3, 0:3]))\n",
    "print(view_3)\n",
    "print(np.linalg.det(view_3[0:3, 0:3]))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "r21 = (view_3[0:3, 0:3]) @ (view_2[0:3, 0:3].T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "t21 = view_3[0:3, 3] - r21 @ view_2[0:3, 3]\n",
    "print(t21)\n",
    "print(np.linalg.norm(t21))\n",
    "\n",
    "print(math.sqrt(2)*8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ndc = pixel_to_ndc((800, 1200), (900, 1600))\n",
    "print(ndc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "pixel = ndc_to_pixel(ndc, (900, 1600))\n",
    "print(pixel)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "res = results[0]\n",
    "view_matrix = res['view_matrix']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def is_rigid(M):\n",
    "    return is_rotation_matrix(M[0:3, 0:3]) and np.linalg.norm(M[3, :] - np.array([0, 0, 0, 1], dtype=M.dtype)) < 1e-6\n",
    "\n",
    "\n",
    "def inv_rigid(M):\n",
    "    assert is_rigid(M)\n",
    "    Mt = np.zeros_like(M)\n",
    "    Mt[0:3, 0:3] = np.transpose(M[0:3, 0:3])\n",
    "    Mt[0:3, 3] = - Mt[0:3, 0:3] @ M[0:3, 3]\n",
    "    Mt[3, 3] = 1\n",
    "    return Mt\n",
    "\n",
    "my_inv = inv_rigid(view_matrix)\n",
    "inv = np.linalg.inv(view_matrix)\n",
    "print(np.linalg.det(view_matrix))\n",
    "print(np.linalg.norm(my_inv - inv))\n",
    "print(np.abs(my_inv - inv))\n",
    "print(my_inv)\n",
    "print(inv)\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.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
