{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import visualization\n",
    "import os\n",
    "from gta_math import points_to_homo, ndc_to_view, construct_proj_matrix, view_to_world, construct_view_matrix, ndcs_to_pixels, grid_to_ndc_pcl_linear_view\n",
    "from visualization import load_depth, save_pointcloud_csv\n",
    "import progressbar\n",
    "from pointcloud_to_voxelmap import pointclouds_to_voxelmap\n",
    "from joblib import Parallel, delayed\n",
    "from configparser import ConfigParser\n",
    "from PIL import Image\n",
    "import pickle\n",
    "import json\n",
    "from voxelmaps import camera_to_pointcloud, load_scene_db_data, get_main_image_name, scene_to_voxelmap, get_main_image, scene_to_voxelmap_with_map, ndc_pcl_to_grid_linear_view\n",
    "import voxelmaps"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ini_file = \"gta-postprocessing.ini\"\n",
    "visualization.multi_page = False\n",
    "visualization.ini_file = ini_file\n",
    "\n",
    "conn = visualization.get_connection_pooled()\n",
    "cur = conn.cursor()\n",
    "\n",
    "CONFIG = ConfigParser()\n",
    "CONFIG.read(ini_file)\n",
    "in_directory = CONFIG[\"Images\"][\"Tiff\"]\n",
    "out_directory = CONFIG[\"Images\"][\"MlDatasetVoxel\"]\n",
    "out_inspect_directory = r'D:\\showing-pointclouds'\n",
    "\n",
    "#voxelmaps.MAX_DISTANCE = 500\n",
    "voxelmaps.MAX_DISTANCE = 25"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#scene_id = '623b6ace-2a9c-4067-8c5f-42cb46d56f53' # from 4cameras on car run, I think\n",
    "scene_id = '386b407b-586c-4d88-9d41-8dc2a0b70e70' # from voxelmap run\n",
    "\n",
    "cameras = load_scene_db_data(scene_id)\n",
    "\n",
    "base_name = get_main_image_name(cameras)\n",
    "voxels, values, map_voxel_size, map_obj = scene_to_voxelmap_with_map(cameras)\n",
    "occupied_voxels = voxels[:, values >= 0]\n",
    "pointcloud = camera_to_pointcloud(get_main_image(cameras))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "cam = get_main_image(cameras)\n",
    "proj_matrix = cam['proj_matrix']\n",
    "view_matrix = cam['view_matrix']\n",
    "width = cam['width']\n",
    "height = cam['height']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "x_min = -1\n",
    "x_max = 1\n",
    "y_min = -1\n",
    "y_max = 1\n",
    "x_range = 240\n",
    "y_range = 160\n",
    "z_meters_min = 1.5\n",
    "z_meters_max = voxelmaps.MAX_DISTANCE\n",
    "# z min calc\n",
    "z_min = proj_matrix @ [1, 1, -z_meters_max, 1]\n",
    "z_min = z_min[2] / z_min[3]\n",
    "# z max calc\n",
    "z_max = proj_matrix @ [1, 1, -z_meters_min, 1]\n",
    "z_max = z_max[2] / z_max[3]\n",
    "z_range = 100\n",
    "#z_range = 2000\n",
    "x_bin = (x_max - x_min) / x_range\n",
    "y_bin = (y_max - y_min) / y_range\n",
    "z_bin = (z_max - z_min) / z_range\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# now I need to calculate correct Z points, deformed in NDC, linear in view frustum\n",
    "z_view_bin = (z_meters_max - z_meters_min) / z_range\n",
    "#X_view, Y_view, Z_view, W_view = np.mgrid[1:2:1, 1:2:1, -z_meters_max:-z_meters_min+1e-7:z_view_bin, 1:2:1]  # W is here as homo coordinate\n",
    "X_view, Y_view, Z_view, W_view = np.meshgrid(np.linspace(1,2,1), np.linspace(1,2,1), np.linspace(-z_meters_max,-z_meters_min,z_range), np.linspace(1,2,1))\n",
    "view_positions = np.vstack([X_view.ravel(), Y_view.ravel(), Z_view.ravel(), W_view.ravel()])\n",
    "ndc_positions = proj_matrix @ view_positions\n",
    "ndc_positions /= ndc_positions[3, :]\n",
    "ndc_z = ndc_positions[2, :]\n",
    "ndc_z_tiled = np.tile(ndc_z,(x_range, y_range, 1))[:, :, :, np.newaxis]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#X, Y, Z, W = np.mgrid[x_min:x_max:x_bin, y_min:y_max:y_bin, z_min:z_max:z_bin, 1:2:1]  # W is here as homo coordinate\n",
    "X, Y, Z, W = np.meshgrid(np.linspace(x_min,x_max,x_range), np.linspace(y_min,y_max,y_range), np.linspace(z_min,z_max,z_range), np.linspace(1,2,1))\n",
    "positions = np.vstack([X.ravel(), Y.ravel(), Z.ravel(), W.ravel()])\n",
    "positions_fixed = np.vstack([X.ravel(), Y.ravel(), ndc_z_tiled.ravel(), W.ravel()])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "C:\\ProgramData\\Anaconda3\\lib\\site-packages\\ipykernel_launcher.py:11: RuntimeWarning: invalid value encountered in greater\n",
      "  # This is added back by InteractiveShellApp.init_path()\n",
      "C:\\ProgramData\\Anaconda3\\lib\\site-packages\\ipykernel_launcher.py:12: RuntimeWarning: invalid value encountered in greater\n",
      "  if sys.path[0] == '':\n",
      "C:\\ProgramData\\Anaconda3\\lib\\site-packages\\ipykernel_launcher.py:24: RuntimeWarning: invalid value encountered in greater\n",
      "C:\\ProgramData\\Anaconda3\\lib\\site-packages\\ipykernel_launcher.py:25: RuntimeWarning: invalid value encountered in greater\n"
     ]
    }
   ],
   "source": [
    "params = {\n",
    "    'width': width,\n",
    "    'height': height,\n",
    "    'proj_matrix': proj_matrix,\n",
    "}\n",
    "\n",
    "points_view = ndc_to_view(positions, proj_matrix)\n",
    "points_world = view_to_world(points_view, view_matrix)\n",
    "# here I find corresponding voxels for generated points, by obtaining voxelmap reference\n",
    "voxel_values = map_obj.get_voxels(points_world[0:3, :], np.zeros((points_world.shape[1], 1)))\n",
    "occupied_selected_voxels_view = points_view[:, voxel_values > 0]\n",
    "occupied_ndc_positions = positions[:, voxel_values > 0]\n",
    "\n",
    "w = np.ones((1,occupied_voxels.shape[1]))\n",
    "voxels_homo = np.vstack([occupied_voxels, w])\n",
    "voxels_view = view_matrix @ voxels_homo\n",
    "w = np.ones((1,pointcloud.shape[1]))\n",
    "pointcloud_homo = np.vstack([pointcloud, w])\n",
    "pointcloud_view = view_matrix @ pointcloud_homo\n",
    "\n",
    "points_view_fixed = ndc_to_view(positions_fixed, proj_matrix)\n",
    "points_world_fixed = view_to_world(points_view_fixed, view_matrix)\n",
    "voxel_values_fixed = map_obj.get_voxels(points_world_fixed[0:3, :], np.zeros((points_world_fixed.shape[1], 1)))\n",
    "occupied_selected_voxels_view_fixed = points_view_fixed[:, voxel_values_fixed > 0]\n",
    "occupied_ndc_positions_fixed = positions_fixed[:, voxel_values_fixed > 0]\n",
    "\n",
    "occupied_ndc_grid = ndc_pcl_to_grid_linear_view(x_range, y_range, z_range, occupied_ndc_positions_fixed, proj_matrix, z_meters_min, z_meters_max)\n",
    "occupied_ndc_positions_reconstructed = grid_to_ndc_pcl_linear_view(occupied_ndc_grid, proj_matrix, z_meters_min, z_meters_max)\n",
    "occupied_ndc_positions_reconstructed = np.hstack((occupied_ndc_positions_reconstructed, np.ones((occupied_ndc_positions_reconstructed.shape[0], 1)))).T\n",
    "\n",
    "points_view_reconstructed = ndc_to_view(occupied_ndc_positions_reconstructed, proj_matrix)\n",
    "\n",
    "# unknown voxel values are nan, do we show then, so we can use them for invalid blacklist\n",
    "invalid_voxels_view = points_view_fixed[:, np.isnan(voxel_values_fixed)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "save_pointcloud_csv(points_view.T[:, 0:3], '{}/{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(points_view_fixed.T[:, 0:3], '{}/fixed-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(positions.T[:, 0:3], '{}/ndc-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(occupied_selected_voxels_view.T[:, 0:3], '{}/voxels-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(voxels_view.T[:, 0:3], '{}/orig-voxels-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(occupied_ndc_positions.T[:, 0:3], '{}/ndc-occupied-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(pointcloud_view.T[:, 0:3], '{}/orig-pointcloud-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(occupied_selected_voxels_view_fixed.T[:, 0:3], '{}/voxels-fixed-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(occupied_voxels.T[:, 0:3], '{}/all-voxels-{}.csv'.format(out_inspect_directory, base_name))\n",
    "save_pointcloud_csv(points_view_reconstructed.T[:, 0:3], '{}/reconstructed-{}.csv'.format(out_inspect_directory, base_name))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "occupied_ndc_grid.tofile('{}/{}.bin'.format(out_inspect_directory, base_name), sep=';')\n",
    "np.save('{}/{}'.format(out_inspect_directory, base_name), occupied_ndc_grid)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# now I create x X y X z grid with 0s and 1s as grid\n",
    "# so now I have data in pointcloud. And I need to convert these NDC values\n",
    "# into indices, so x:[-1, 1] into [0, 239], y:[-1, 1] to [0, 159], \n",
    "# and z:[z_min, z_max] into [0, 99]\n",
    "voxelmap_ndc_grid = np.zeros((x_range, y_range, z_range), dtype=np.bool)\n",
    "vecs = ndcs_to_pixels(occupied_ndc_positions[0:2, :], (y_range, x_range))\n",
    "vec_y = vecs[0, :]\n",
    "vec_x = vecs[1, :]\n",
    "vec_z = ((occupied_ndc_positions[2, :] - z_min) / z_bin).astype(np.int32)\n",
    "voxelmap_ndc_grid[vec_x, vec_y, vec_z] = 1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "voxels.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "w = np.ones((1,voxels.shape[1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "voxels_homo.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "points_world[0:3, :].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "positions.T[:, 0:3].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "occupied_voxels.T[:, 0:3].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#rgb_img_name = os.path.join(r'D:\\generic-dataset', '{}.jpg'.format('2018-03-30--06-00-56--114'))\n",
    "rgb_img_name = os.path.join(in_directory, '{}.tiff'.format(base_name))\n",
    "im = Image.open(rgb_img_name)\n",
    "im = im.convert(mode=\"RGB\")\n",
    "arr = np.array(im)\n",
    "\n",
    "plt.figure(figsize=(20, 20))\n",
    "plt.axis('off')\n",
    "plt.imshow(arr)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "depth = load_depth(base_name)\n",
    "\n",
    "plt.figure(figsize=(20, 20))\n",
    "plt.axis('off')\n",
    "plt.imshow(depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "for camera in cameras:\n",
    "    depth = load_depth(camera['imagepath'])\n",
    "    plt.figure(figsize=(20, 20))\n",
    "    plt.axis('off')\n",
    "    plt.imshow(depth, cmap='gray')\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for camera in cameras:\n",
    "    rgb_img_name = os.path.join(in_directory, '{}.tiff'.format(camera['imagepath']))\n",
    "    im = Image.open(rgb_img_name)\n",
    "    im = im.convert(mode=\"RGB\")\n",
    "    arr = np.array(im)\n",
    "    plt.figure(figsize=(20, 20))\n",
    "    plt.axis('off')\n",
    "    plt.imshow(arr)\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "base_name"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(occupied_ndc_positions.min(axis=1))\n",
    "display(occupied_ndc_positions.max(axis=1))\n",
    "occupied_ndc_positions.shape\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# for debug purposes, now I visualize the voxelmap in NDC as image\n",
    "new_depth = np.zeros((y_range, x_range))\n",
    "vecs = ndcs_to_pixels(occupied_ndc_positions_fixed[0:2, :], (y_range, x_range))\n",
    "vec_y = vecs[0, :]\n",
    "vec_x = vecs[1, :]\n",
    "vec_z = occupied_ndc_positions_fixed[2, :]\n",
    "new_depth[vec_y, vec_x] = vec_z\n",
    "\n",
    "%matplotlib notebook\n",
    "\n",
    "plt.figure(figsize=(10, 6))\n",
    "plt.axis('off')\n",
    "plt.imshow(new_depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### now image from bool grid, which will be directly output of neural network"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "occupied_ndc_grid.shape\n",
    "np.argmax(occupied_ndc_grid == True, axis=2)[100, 100]\n",
    "occupied_ndc_grid[150, 80, :]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "occupied_ndc_grid = np.flip(occupied_ndc_grid, axis=2)\n",
    "\n",
    "# now I have just boolean for each value\n",
    "# so I create mask to assign higher value to booleans in higher index\n",
    "target_width = occupied_ndc_grid.shape[0]\n",
    "target_height = occupied_ndc_grid.shape[1]\n",
    "depth_size = occupied_ndc_grid.shape[2]\n",
    "\n",
    "new_depth = np.argmax(occupied_ndc_grid, axis=2)\n",
    "new_depth = new_depth.T\n",
    "\n",
    "new_depth *= int(255/depth_size)\n",
    "\n",
    "%matplotlib notebook\n",
    "\n",
    "plt.figure(figsize=(10, 7))\n",
    "plt.axis('off')\n",
    "plt.imshow(new_depth, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(new_depth.max())\n",
    "print(new_depth.min())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "display(vecs.min(axis=1))\n",
    "display(vecs.max(axis=1))\n",
    "vecs.shape\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3840000,)\n",
      "2935884\n"
     ]
    },
    {
     "ename": "ValueError",
     "evalue": "max must be larger than min in range parameter.",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mValueError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-24-a2e3838b81f4>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mfigure\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 5\u001b[1;33m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mhist\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mvoxel_values_fixed\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mflatten\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mbins\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;36m20\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      6\u001b[0m \u001b[0mplt\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mshow\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\ProgramData\\Anaconda3\\lib\\site-packages\\matplotlib\\pyplot.py\u001b[0m in \u001b[0;36mhist\u001b[1;34m(x, bins, range, density, weights, cumulative, bottom, histtype, align, orientation, rwidth, log, color, label, stacked, normed, hold, data, **kwargs)\u001b[0m\n\u001b[0;32m   3002\u001b[0m                       \u001b[0mhisttype\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mhisttype\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0malign\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0malign\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0morientation\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0morientation\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3003\u001b[0m                       \u001b[0mrwidth\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mrwidth\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mlog\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mlog\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mcolor\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mcolor\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mlabel\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mlabel\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 3004\u001b[1;33m                       stacked=stacked, normed=normed, data=data, **kwargs)\n\u001b[0m\u001b[0;32m   3005\u001b[0m     \u001b[1;32mfinally\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   3006\u001b[0m         \u001b[0max\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_hold\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mwashold\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\ProgramData\\Anaconda3\\lib\\site-packages\\matplotlib\\__init__.py\u001b[0m in \u001b[0;36minner\u001b[1;34m(ax, *args, **kwargs)\u001b[0m\n\u001b[0;32m   1708\u001b[0m                     warnings.warn(msg % (label_namer, func.__name__),\n\u001b[0;32m   1709\u001b[0m                                   RuntimeWarning, stacklevel=2)\n\u001b[1;32m-> 1710\u001b[1;33m             \u001b[1;32mreturn\u001b[0m \u001b[0mfunc\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0max\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m*\u001b[0m\u001b[0margs\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mkwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   1711\u001b[0m         \u001b[0mpre_doc\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0minner\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m__doc__\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   1712\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mpre_doc\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\ProgramData\\Anaconda3\\lib\\site-packages\\matplotlib\\axes\\_axes.py\u001b[0m in \u001b[0;36mhist\u001b[1;34m(***failed resolving arguments***)\u001b[0m\n\u001b[0;32m   6205\u001b[0m             \u001b[1;31m# this will automatically overwrite bins,\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   6206\u001b[0m             \u001b[1;31m# so that each histogram uses the same bins\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m-> 6207\u001b[1;33m             \u001b[0mm\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mbins\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mhistogram\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m[\u001b[0m\u001b[0mi\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mbins\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mweights\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mw\u001b[0m\u001b[1;33m[\u001b[0m\u001b[0mi\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m**\u001b[0m\u001b[0mhist_kwargs\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m   6208\u001b[0m             \u001b[0mm\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mm\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mastype\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mfloat\u001b[0m\u001b[1;33m)\u001b[0m  \u001b[1;31m# causes problems later if it's an int\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m   6209\u001b[0m             \u001b[1;32mif\u001b[0m \u001b[0mmlast\u001b[0m \u001b[1;32mis\u001b[0m \u001b[1;32mNone\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\ProgramData\\Anaconda3\\lib\\site-packages\\numpy\\lib\\function_base.py\u001b[0m in \u001b[0;36mhistogram\u001b[1;34m(a, bins, range, normed, weights, density)\u001b[0m\n\u001b[0;32m    665\u001b[0m     \u001b[1;32mif\u001b[0m \u001b[0mmn\u001b[0m \u001b[1;33m>\u001b[0m \u001b[0mmx\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    666\u001b[0m         raise ValueError(\n\u001b[1;32m--> 667\u001b[1;33m             'max must be larger than min in range parameter.')\n\u001b[0m\u001b[0;32m    668\u001b[0m     \u001b[1;32mif\u001b[0m \u001b[1;32mnot\u001b[0m \u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mall\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0misfinite\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m[\u001b[0m\u001b[0mmn\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmx\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    669\u001b[0m         raise ValueError(\n",
      "\u001b[1;31mValueError\u001b[0m: max must be larger than min in range parameter."
     ]
    }
   ],
   "source": [
    "print(voxel_values_fixed.flatten().shape)\n",
    "print(sum(np.isnan(voxel_values_fixed)))\n",
    "\n",
    "plt.figure()\n",
    "plt.hist(voxel_values_fixed.flatten(), bins=20)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### playing with depths projections"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "z_meters_min = 1.5\n",
    "z_meters_max = voxelmaps.MAX_DISTANCE\n",
    "# z min calc\n",
    "z_min = proj_matrix @ [1, 1, -z_meters_max, 1]\n",
    "z_min = z_min[2] / z_min[3]\n",
    "# z max calc\n",
    "z_max = proj_matrix @ [1, 1, -z_meters_min, 1]\n",
    "z_max = z_max[2] / z_max[3]\n",
    "z_range = 100\n",
    "z_bin = (z_max - z_min) / (z_range)\n",
    "#X, Y, Z, W = np.mgrid[1:2:1, 1:2:1, z_min:z_max:z_bin, 1:2:1]  # W is here as homo coordinate\n",
    "X, Y, Z, W = np.meshgrid(np.linspace(1,2,1), np.linspace(1,2,1), np.linspace(z_min,z_max,z_range), np.linspace(1,2,1))\n",
    "positions = np.vstack([X.ravel(), Y.ravel(), Z.ravel(), W.ravel()])\n",
    "points_view = ndc_to_view(positions, proj_matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "positions[2, :].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(15,15))\n",
    "plt.plot(positions[2, :], points_view[2, :])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(positions[2, :].max())\n",
    "print(positions[2, :].min())\n",
    "print(points_view[2, :].max())\n",
    "print(points_view[2, :].min())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "z_bin = (z_meters_max - z_meters_min) / (z_range)\n",
    "#X, Y, Z, W = np.mgrid[1:2:1, 1:2:1, -z_meters_max:-z_meters_min:z_bin, 1:2:1]  # W is here as homo coordinate\n",
    "X, Y, Z, W = np.meshgrid(np.linspace(1,2,1), np.linspace(1,2,1), np.linspace(-z_meters_max,-z_meters_min,z_range), np.linspace(1,2,1))\n",
    "view_positions = np.vstack([X.ravel(), Y.ravel(), Z.ravel(), W.ravel()])\n",
    "ndc_positions = proj_matrix @ view_positions\n",
    "ndc_positions /= ndc_positions[3, :]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(15,15))\n",
    "plt.plot(view_positions[2, :], ndc_positions[2, :])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(ndc_positions[2, :].max())\n",
    "print(ndc_positions[2, :].min())\n",
    "print(view_positions[2, :].max())\n",
    "print(view_positions[2, :].min())\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "view_positions[2, :].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "list(np.mgrid[-z_meters_max:-z_meters_min+1e-7:z_bin])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "view_positions[2, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "z_bin"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "-1.9850000000000563 + z_bin"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "-z_meters_max + (z_bin * 99)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ndc_positions[2, :]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# new NDC sampling, with nonlinear Z axis which will be mapped to linear distances in view frustum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "z_meters_min = 1.5\n",
    "z_meters_max = voxelmaps.MAX_DISTANCE\n",
    "# z min calc\n",
    "z_min = proj_matrix @ [1, 1, -z_meters_max, 1]\n",
    "z_min = z_min[2] / z_min[3]\n",
    "# z max calc\n",
    "z_max = proj_matrix @ [1, 1, -z_meters_min, 1]\n",
    "z_max = z_max[2] / z_max[3]\n",
    "z_range = 100\n",
    "z_bin = (z_max - z_min) / z_range\n",
    "#X, Y, Z, W = np.mgrid[1:2:1, 1:2:1, z_min:z_max+1e-7:z_bin, 1:2:1]  # W is here as homo coordinate\n",
    "X, Y, Z, W = np.meshgrid(np.linspace(1,2,1), np.linspace(1,2,1), np.linspace(z_min,z_max,z_range), np.linspace(1,2,1))\n",
    "positions = np.vstack([X.ravel(), Y.ravel(), ndc_positions[2, :], W.ravel()])\n",
    "points_view = ndc_to_view(positions, proj_matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(15,15))\n",
    "plt.plot(np.linspace(0, 1, len(points_view[2, :])), points_view[2, :])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "ndc_z_tiled = np.tile(ndc_z,(240, 160, 1))[:, :, :, np.newaxis]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Z.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Z[:].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Z[0, :, 0, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ndc_z_tiled[0, :, 0, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Z - ndc_z_tiled"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zmax = 50\n",
    "zmin = 1.5\n",
    "z_bin = (zmax - zmin) / (z_range - 1)\n",
    "print(np.mgrid[-zmax:-zmin:z_bin])\n",
    "print(np.mgrid[-zmax:-zmin:z_bin].shape)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zmax = 30\n",
    "zmin = 1.5\n",
    "z_bin = (zmax - zmin) / (z_range - 1)\n",
    "print(np.mgrid[-zmax:-zmin:z_bin])\n",
    "print(np.mgrid[-zmax:-zmin:z_bin].shape)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zmax = 30\n",
    "zmin = 1.5\n",
    "print(np.linspace(-zmax, -zmin, z_range))\n",
    "print(np.linspace(-zmax, -zmin, z_range).shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "zmax = 50\n",
    "zmin = 1.5\n",
    "print(np.linspace(-zmax, -zmin, z_range))\n",
    "print(np.linspace(-zmax, -zmin, z_range).shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "rgb_im = Image.open(os.path.join(out_directory, '2018-03-07--17-46-32--369.jpg'))\n",
    "plt.figure(figsize=(10, 7))\n",
    "plt.axis('off')\n",
    "plt.imshow(rgb_im)\n",
    "plt.show()\n",
    "\n",
    "rgb_im = Image.open(os.path.join(out_directory, '2018-03-07--18-01-55--862.jpg'))\n",
    "plt.figure(figsize=(10, 7))\n",
    "plt.axis('off')\n",
    "plt.imshow(rgb_im)\n",
    "plt.show()\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
}
