{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\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\n",
    "from visualization import load_depth\n",
    "from pointcloud_to_voxelmap import pointclouds_to_voxelmap\n",
    "from PIL import Image\n",
    "import pickle\n",
    "from voxelmaps import camera_to_pointcloud, load_scene_db_data, get_main_image_name, scene_to_pointcloud, scene_to_voxelmap, NoMainImageException, scene_to_voxelmap_with_map, get_main_image\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()\n",
    "cur = conn.cursor()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "There are 959 scenes\n"
     ]
    }
   ],
   "source": [
    "#run_id = 6\n",
    "#run_id = 19\n",
    "run_id = 3677\n",
    "\n",
    "cur.execute(\"\"\"SELECT snapshot_id, \\\n",
    "  ARRAY[st_x(camera_relative_rotation), st_y(camera_relative_rotation), st_z(camera_relative_rotation)] as camera_relative_rotation, \\\n",
    "  ARRAY[st_x(camera_pos), st_y(camera_pos), st_z(camera_pos)] as camera_pos \\\n",
    "  FROM snapshots \\\n",
    "  WHERE run_id = {} \\\n",
    "  AND camera_relative_rotation = st_makePoint(0, 0, 0) \\\n",
    "  ORDER BY timestamp ASC \\\n",
    "  \"\"\".format(run_id))\n",
    "\n",
    "scenes = []\n",
    "for row in cur:\n",
    "    res = dict(row)\n",
    "    res['camera_pos'] = np.array(res['camera_pos'])\n",
    "    scenes.append(res)\n",
    "\n",
    "print('There are {} scenes'.format(len(scenes)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "positions = np.array([i['camera_pos'] for i in scenes])\n",
    "diffs = np.diff(positions, axis=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(22284, 3)"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "diffs.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "diffs_len = np.linalg.norm(diffs, axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(22284,)"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "diffs_len.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "total driven length is 34340.62537517453\n"
     ]
    }
   ],
   "source": [
    "print('total driven length is {}'.format(diffs_len.sum()))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[  311.79238892, -1370.57763672,    31.82174301],\n",
       "       [  311.7928772 , -1370.57617188,    31.82192612],\n",
       "       [  311.79162598, -1370.54260254,    31.82613564],\n",
       "       [  311.73892212, -1370.41589355,    31.83475685],\n",
       "       [  311.60153198, -1370.24621582,    31.83952713],\n",
       "       [  311.36730957, -1369.98449707,    31.84394455]])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "positions[0:6, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[  4.88281250e-04,   1.46484375e-03,   1.83105469e-04],\n",
       "       [ -1.25122070e-03,   3.35693359e-02,   4.20951843e-03],\n",
       "       [ -5.27038574e-02,   1.26708984e-01,   8.62121582e-03],\n",
       "       [ -1.37390137e-01,   1.69677734e-01,   4.77027893e-03],\n",
       "       [ -2.34222412e-01,   2.61718750e-01,   4.41741943e-03]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "diffs[0:5, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.0015549 ,  0.03385537,  0.13750341,  0.21837889,  0.35124971])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.linalg.norm(diffs[0:5, :], axis=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0.21837888799360511"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.sqrt((-1.37390137e-01)**2 + 1.69677734e-01**2 + 4.77027893e-03 **2)"
   ]
  },
  {
   "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
}
