project_rays.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "SOURCE = \"../bbb3_glass_eval_111716.log\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\" Install instructions\n",
    "Always un in jupyterlab\n",
    "\n",
    "- sudo apt install nodejs # didn't work\n",
    "- curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.38.0/install.sh | bash\n",
    "- nvm install --lts\n",
    "- sudo chmod a+rwx /usr/local/share/jupyter/lab/\n",
    "- mkdir /usr/local/share/jupyter/lab/extensions\n",
    "- jupyter labextension install @jupyter-widgets/jupyterlab-manager --debug # will need to chmod some stouff to make work\n",
    "\n",
    "- jupyter nbextension enable --py widgetsnbextension\n",
    "\"\"\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "1440*1200\n",
    "blob=np.zeros((800,12000,1440),np.uint16).reshape(36000)\n",
    "cv2.imwrite(\"testblob.png\", blob)\n",
    "del blob\n",
    "blob=cv2.imread(\"testblob.png\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Known bugs:\n",
    " - Code is inconsistent on whether scan period is the whole period or just for the active part of the scan (hack corrected)\n",
    "\n",
    "\n",
    "In 112016 the gyro(or odom rotation? not sure) readings need to be multiplied by  0.78(+-0.02) to be accurate(not tested on other runs)\n",
    "\n",
    "The intensity seems to fall off at a rate of 218.3 (unknown uncert) units per meter for the Hokuyo. Perhaps it reports in a log scale? Not normalized in any case.\n",
    "Below two meters, the intensity is pretty weird. 2-10 m is where the intensity seems to be sane. Somewhere between 0.5 and 1.0 m seems to be where most things report peak reflectivity\n",
    "\n",
    "Localization seems to be accurate to +-10 mrad."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "import os"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# rayfile = os.path.join(SOURCE, \"raw_lidar_dumps\",\"rays\",\"rays.rays\")\n",
    "# rayfile = os.path.join(SOURCE, \"raw_lidar_dumps\",\"rays\",\"rays_moving_unfixed.rays\")\n",
    "# rayfile = os.path.join(SOURCE, \"raw_lidar_dumps\",\"rays\",\"rays_moving_earlyts.rays\")\n",
    "# rayfile = os.path.join(SOURCE, \"raw_lidar_dumps\",\"rays\",\"rays_moving_hiqual.rays\")\n",
    "# rayfile = os.path.join(SOURCE, \"raw_lidar_dumps\",\"rays\",\"rays_manyparticle_highrecall.rays\")\n",
    "\n",
    "rayfile = \"/home/anuran/repos/Vulcan/bin/rayDump.rays\" # HACK"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "WARNING: file is not made of complete scans!\n",
      "Remainder: 952\n"
     ]
    }
   ],
   "source": [
    "rays_struct = np.dtype(\n",
    "    [\n",
    "        ('sx', np.float32),\n",
    "        ('sy', np.float32),\n",
    "        ('ex', np.float32),\n",
    "        ('ey', np.float32),\n",
    "        ('r' , np.float32),\n",
    "        ('th', np.float32),\n",
    "        ('i',  np.uint16),\n",
    "        ('pad',  np.uint16),\n",
    "        ('idx',  np.int32),\n",
    "        ('ts', np.float64), #remove for old ray file formats\n",
    "      ])\n",
    "\n",
    "# rays = np.memmap(rayfile,rays_struct, 'r',1)\n",
    "rays0 = np.fromfile(rayfile,rays_struct)\n",
    "# Clip rays0 to a multiple of 1081 rays, under the assumption that a scan is 1081 rays long\n",
    "if len(rays0)%1081 != 0:\n",
    "    print(\"WARNING: file is not made of complete scans!\")\n",
    "    print(\"Remainder:\", len(rays0)%1081)\n",
    "# For the new sensors, we will need to find a different way to split scans\n",
    "rays0 = rays0[:int(len(rays0)//1081*1081)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import matplotlib.pyplot as plt\n",
    "# from mpl_toolkits.mplot3d import Axes3D\n",
    "\n",
    "# skip = 100   # Skip every n points\n",
    "%gui qt\n",
    "# fig = plt.figure()\n",
    "# ax = fig.add_subplot(111, projection='3d')\n",
    "import mayavi.mlab\n",
    "mayavi.mlab.close(all=True)\n",
    "# fig = mayavi.mlab.figure(bgcolor=(1.0, 1.0, 1.0), size=(640, 360))\n",
    "# mayavi.mlab.points3d(rays0['ex'], rays0['ey'], rays0['th'],rays0['th'], colormap='spectral',mode= 'point')\n",
    "# mayavi.mlab.test_plot3d()\n",
    "# mayavi.mlab.close(all=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "438.99257000013404"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "rays1=rays0[rays0['idx']==0]\n",
    "rays2=rays0[rays0['idx']==1]\n",
    "rays0['ts'][-1]-rays0['ts'][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "<ipython-input-9-e50cb4296749>:6: RuntimeWarning: divide by zero encountered in divide\n",
      "  nan_th=rays1['th']/(rays1['r']>1.0)/(rays1['r']<30.0)\n",
      "<ipython-input-9-e50cb4296749>:6: RuntimeWarning: invalid value encountered in divide\n",
      "  nan_th=rays1['th']/(rays1['r']>1.0)/(rays1['r']<30.0)\n",
      "/home/anuran/.local/lib/python3.8/site-packages/numpy/lib/function_base.py:1447: RuntimeWarning: invalid value encountered in subtract\n",
      "  a = op(a[slice1], a[slice2])\n",
      "<ipython-input-9-e50cb4296749>:30: RuntimeWarning: invalid value encountered in divide\n",
      "  _=plt.hist(np.diff(np.diff(flat_rangle)/np.diff(rays1['ts'][::1081])),1000)\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "fb7880769228477e894460f05ad95448",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "# plt.imshow(np.diff(rays0['ts'].reshape(-1,1081)[:1000]-rays0['ts'][0],axis=0))\n",
    "# d=(d+np.pi/2)%\n",
    "# plt.plot(np.diff(rays0['th'].reshape(-1,1081)[:1000],axis=0)[:,100],'b,')\n",
    "# plt.plot(rays1['ts'],rays1['th'],'b,')\n",
    "nan_th=rays1['th']/(rays1['r']>1.0)/(rays1['r']<30.0)\n",
    "woof=np.diff(rays1['th'].reshape(-1,1081))\n",
    "dts=np.diff(rays1['ts'].reshape(-1,1081))\n",
    "dth=np.diff(nan_th.reshape(-1,1081))\n",
    "\n",
    "slope=dth/dts\n",
    "slope=slope[np.abs(dth)<np.pi/2]\n",
    "stopped=[]\n",
    "rangle=[]\n",
    "for row,rts in zip(nan_th.reshape(-1,1081),rays1['ts'].reshape(-1,1081)):\n",
    "    use = np.unwrap(row[np.isfinite(row)])\n",
    "    use_ts = rts[np.isfinite(row)]\n",
    "    if len(use)==0:\n",
    "        print('err')\n",
    "        print(row)\n",
    "        break\n",
    "    slope = (use[-1]-use[0])/(use_ts[-1]-use_ts[0])\n",
    "    off=np.mean(use-(use_ts-rts[0])*slope)\n",
    "    stopped.append(row-(rts-rts[0])*slope-off)\n",
    "    rangle.append((rts-rts[0])*slope+off)\n",
    "stopped = np.vstack(stopped)\n",
    "rangle = np.vstack(rangle)\n",
    "flat_rangle = np.unwrap(rangle[:,0].ravel()*2)/2\n",
    "# plt.plot(rays1['ts'][::1081],flat_rangle,'b,')\n",
    "_=plt.hist(np.diff(np.diff(flat_rangle)/np.diff(rays1['ts'][::1081])),1000)\n",
    "plt.gca().set_yscale(\"log\")\n",
    "# plt.plot((stopped.ravel()+np.pi)%(2*np.pi)-np.pi,'b,')\n",
    "# expect_th = (rays1['ts']-rays1['ts'][0])*np.float64(np.median(slope[slope>0]))%(2*np.pi)\n",
    "# plt.plot(rays1['ts'][rays1['r']>1],(expect_th-rays1['th'])[rays1['r']>1]%(2*np.pi),'b,')\n",
    "# plt.plot(np.diff(flat_rangle)/np.diff(rays1['ts'][::1081]))\n",
    "# plt.hist(slope[np.isfinite(np.abs(slope))],1000)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "# TODO: transition to floating point timestamps, not microseconds\n",
    "odofile = \"/home/anuran/repos/Vulcan/bin/odoDump.rays\" # HACK\n",
    "odo_struct = np.dtype(\n",
    "    [\n",
    "        ('ts', np.float64),#TODO: this is a double now!\n",
    "        ('x', np.float32),\n",
    "        ('y', np.float32),\n",
    "        ('th', np.float32),\n",
    "        ('pad' , np.float32),\n",
    "        ('lx', np.float32),\n",
    "        ('ly', np.float32),\n",
    "      ])\n",
    "def odosub(odo):\n",
    "    fields = ['ts','x','y','th']\n",
    "    for f in fields:\n",
    "        odo[f]-=odo[0][f]\n",
    "    \n",
    "\n",
    "# rays = np.memmap(rayfile,rays_struct, 'r',1)\n",
    "poses = np.fromfile(odofile,odo_struct)\n",
    "imu= poses[0::3]\n",
    "odo= poses[1::3]\n",
    "lp = poses[2::3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5702d181a2434021a9b69932d6c5e1d8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "756087864.8878378\n",
      "2554686165.6213484\n",
      "2653254129.686415\n",
      "2700661017.166998\n",
      "-346858747.92639446\n"
     ]
    }
   ],
   "source": [
    "# This cell checks odometry for smoothness\n",
    "%matplotlib widget\n",
    "def d_dt(t,x,dt,order=1):\n",
    "    if order>1:\n",
    "        x=d_dt(t,x,dt,order-1)\n",
    "    if order==0:\n",
    "        return x\n",
    "    dt = dt/2\n",
    "    lot = np.clip(t-0.001*dt, t[0], t[-1])\n",
    "    hit = np.clip(t+dt, t[0], t[-1])\n",
    "    assert(np.all(hit-lot > 0))\n",
    "    lox = np.interp(lot,t,x)\n",
    "    hix = np.interp(hit,t,x)\n",
    "    return (hix-lox)/(hit-lot)\n",
    "# first, resample to 100hz\n",
    "odo_struct_rt = np.dtype(\n",
    "    [\n",
    "        ('ts', np.float64),\n",
    "        ('x', np.float32),\n",
    "        ('y', np.float32),\n",
    "        ('th', np.float32),\n",
    "        ('pad' , np.float32),\n",
    "        ('lx', np.float32),\n",
    "        ('ly', np.float32),\n",
    "      ])\n",
    "ots = (odo['ts']-odo['ts'][0])/1e6\n",
    "hz=2\n",
    "assert(np.all(np.diff(ots)>=0))\n",
    "tsr = np.max(ots)-np.min(ots)\n",
    "ts100 = np.linspace(ots[0],ots[-1],np.maximum(int(tsr*hz),2))\n",
    "odo100 = np.zeros_like(ts100,dtype=odo_struct_rt)\n",
    "odo100['ts'] = ts100\n",
    "for key in ['x','y','th','lx','ly']:\n",
    "    odo100[key]=np.interp(ts100,ots,odo[key])\n",
    "\n",
    "for hz in [40,20,10,5,2]:\n",
    "    # now check smoothness\n",
    "#     plt.plot(np.unwrap(odo['th']))\n",
    "    dk_dt = d_dt(ots,np.unwrap(odo['y']),1/hz,2)#/(d_dt(ots,np.unwrap(odo['th']),1/(2*hz))+0.1)\n",
    "    plt.plot(ots,(dk_dt),)\n",
    "    print(np.max(dk_dt))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0bdf08270eba4c3eb6a66d36098f99f2",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f8343ce96d0>]"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "# odosub(odo)\n",
    "# plt.plot(lp['ts']/1e6,np.sqrt(lp['x']**2+lp['y']**2), ',')\n",
    "# plt.plot(lp['ts']/1e6,lp['th'], ',')\n",
    "\n",
    "# plt.plot(rays0['ts'],np.sqrt(rays0['sx']**2+rays0['sy']**2), ',')\n",
    "# plt.plot(rays0['ts'][647::1081],rays0['th'][647::1081], ',')\n",
    "# plt.plot(np.arange(1081*20)%1081,rays0['r'][1081*200:1081*200+1081*20],',')\n",
    "\n",
    "plt.plot(odo['ts']/1e6,odo['x'],'b')\n",
    "\n",
    "\n",
    "# plt.plot(odo['x'],odo['y'], ',')\n",
    "# ax1 = plt.subplot(211)\n",
    "# ax2 = plt.subplot(212,sharex=ax1)\n",
    "# ax1.plot(odo['ts'][:-1]/1e6,np.diff(np.unwrap(odo['th'])), ',')\n",
    "# ax2.plot(rays0['ts'][500::1081][:-1],(np.diff(rays0['th'][500::1081])+np.pi/4)%(np.pi/2)-np.pi/4, ',')\n",
    "\n",
    "# ax2.plot(odo['ts']/1e6,odo['x'], ',')\n",
    "\n",
    "# plt.plot(odo['ts']/1e6,np.unwrap(odo['th']),',')\n",
    "# plt.plot(imu['ts']/1e6,np.unwrap((imu['th'])))\n",
    "plt.plot(odo['ts']/1e6,np.unwrap(odo['th']),'b')\n",
    "plt.plot(odo['ts']/1e6,np.unwrap(lp['th']),'r')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "2c4cacf358404c9b89d83acf683ca00d",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# As a hack to split scans up by sensor, we will look for sudden large jumps in the ray \n",
    "# starting position. As long as the localization doesn't jump too much, this hack should work.\n",
    "# Plot the cdf of jumps to find a good cutoff.\n",
    "# (as of this writing a 0.5m jump looks like a good cutoff)\n",
    "%matplotlib widget\n",
    "\n",
    "tmp=np.sqrt(np.diff(rays0['sx'],append=[0])**2+np.diff(rays0['sy'],append=[0])**2)\n",
    "plt.plot(np.sort(tmp),',')\n",
    "\n",
    "SENSOR_JUMP_THRESHOLD=0.5 # by inspection of the plot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "853ccbd99cff4f028beda41f628219fc",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a9f9be6793114517917027aa8f8e0e04",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "<ipython-input-12-f0d3a60601ae>:71: RuntimeWarning: invalid value encountered in true_divide\n",
      "  d1y=np.diff(c1y)/np.diff(c1x)\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f8343c24a90>]"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Use index to read sensor (uncomment block after if you want to use position hack instead\n",
    "rays1=rays0[rays0['idx']==0]\n",
    "rays2=rays0[rays0['idx']==1]\n",
    "\n",
    "# # Use the jump threshold to detect changes of sensor\n",
    "# restarts=np.sqrt(np.diff(rays0['sx'],append=[0])**2+np.diff(rays0['sy'],append=[0])**2)>SENSOR_JUMP_THRESHOLD\n",
    "\n",
    "# # scans=rays0[:len(rays0)-len(rays0)%2162].reshape(-1,2162)\n",
    "# raysr = rays0[restarts]# HACK to separate scans, gives the ray right before a restart\n",
    "# idx=np.arange(len(restarts))[restarts]\n",
    "# print(\"GCD of scan restarts:\", np.gcd.reduce(idx[:-1]+1), \n",
    "#       \"\\nThis should be a multiple of the scan size if it's constant\")\n",
    "# rs=[]\n",
    "# sn=[]\n",
    "# n=0\n",
    "# for s,e in zip(idx[:-1:2],idx[1::2]):\n",
    "#     rays0[s+1:e+1]['idx']=0\n",
    "#     rs.append(rays0[s+1:e+1])\n",
    "#     sn.append(np.zeros(e-s)+n)\n",
    "#     n+=1\n",
    "# rays1=np.hstack(rs)\n",
    "# scannum1=np.hstack(sn)\n",
    "# # scannum1=scannum1[rays1['r']>=0]\n",
    "# # rays1=rays1[rays1['r']>=0]\n",
    "\n",
    "# rs=[rays0[:idx[0]+1]]\n",
    "# sn=[]\n",
    "# n=0\n",
    "# for s,e in zip(idx[1::2],idx[2::2]):\n",
    "#     rays0[s+1:e+1]['idx']=1\n",
    "#     rs.append(rays0[s+1:e+1])\n",
    "#     sn.append(np.zeros(e-s)+n)\n",
    "#     assert(len(rays0[s+1:e+1])%1081==0)\n",
    "#     n+=1\n",
    "\n",
    "# rays2=np.hstack(rs)\n",
    "# scannum2=np.hstack(sn)\n",
    "\n",
    "# print(\"Timestamp of first ray:\",rays1['ts'][0])\n",
    "\n",
    "%matplotlib widget\n",
    "# plt.plot(rays1['sx'],',')\n",
    "# plt.plot(rays0['ts']-rays0['ts'][0],rays0['sx'],',')\n",
    "# t=rays1['ts']-rays1['ts'][0]\n",
    "# plt.plot(t,rays0['th']-300*t,',')\n",
    "# plt.plot(rays1['ts'],np.mod(rays1['th']-2*np.pi*t/0.025025,2*np.pi),',')\n",
    "# plt.plot(rays1['ts'],rays1['th'],',')\n",
    "# plt.plot(rays1['ts'][540::1081],rays1['th'][540::1081],',')\n",
    "# plt.plot(rays0['r']>0,',')\n",
    "# _=plt.hist(np.diff(rays0['sx']),1000,range=[-.001,.001])\n",
    "# plt.plot(np.arange(len(rays0))[restarts]%2162,',')\n",
    "# plt.plot(np.diff(rays0[restarts]['ts'][::2]),',')\n",
    "\n",
    "thtmp = rays1['th'].reshape(-1,1081)\n",
    "rtmp = rays1['r'].reshape(-1,1081)\n",
    "\n",
    "\n",
    "\n",
    "# print(rays2['th'].reshape(-1,1081)[:,540][0]-rays1['th'].reshape(-1,1081)[:,540][0])\n",
    "c1x=rays1['ts'].reshape(-1,1081)[:,540]-rays1['ts'][0]\n",
    "c1y=np.unwrap(thtmp[:,540])\n",
    "c2x=rays2['ts'].reshape(-1,1081)[:,540]-rays2['ts'][0]\n",
    "c2y=np.unwrap(np.mod(rays2['th'].reshape(-1,1081)[:,540]+3.145+np.pi,2*np.pi)-np.pi)\n",
    "plt.plot((lp['ts']-lp['ts'][0])/1e6,lp['th']+c1y[0], 's')\n",
    "plt.plot(c1x, c1y,'k.')\n",
    "plt.plot(c2x, c2y,'r.')\n",
    "plt.title(\"Plot of reference ray angle over time for both sensors\")\n",
    "\n",
    "\n",
    "plt.figure()\n",
    "d1y=np.diff(c1y)/np.diff(c1x)\n",
    "d1x=(c1x[:-1]+c1x[1:])/2\n",
    "plt.plot(np.diff(d1y)/np.diff(d1x),'.')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ab09a5abe68c4eec81e5592314691592",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f8343c33760>]"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "# plt.plot(rays1['ts'],rays1['sx'],',')\n",
    "# plt.plot(rays2['ts'],rays2['sx'],',')\n",
    "\n",
    "dx=np.diff(rays1['sx'])\n",
    "dy=np.diff(rays1['sy'])\n",
    "dd=np.sqrt(dx**2+dy**2)\n",
    "dth=np.diff(rays1['th'])\n",
    "dt=np.diff(rays1['ts'])\n",
    "intertime=0.5*rays1['ts'][:-1]+0.5 * rays1['ts'][1:]\n",
    "\n",
    "\n",
    "# plt.plot(intertime, dd/dt,',')\n",
    "plt.plot( np.sort(dth/dt),',')\n",
    "# plt.plot(rays1['ts'],rays1['th'],',')\n",
    "\n",
    "# plt.plot(np.unwrap(rays1['th'])/np.pi,rays1['sx'],',')\n",
    "# plt.plot(np.sort(np.diff(rays0['sx'])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1479396473.584265\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f4b0f042d2a146468febd723eabcf4e3",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "510e22b5bde24c0589418220a91a29ab",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "e97fb92c61a54808a68817f177d89175",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "4c1d749ea87d4a3a8ebaf1d6788872e2",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "c20313c93ceb4740819eeb777a639b3a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f8343b00d90>]"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "## This cell tests for gyro scale error. \n",
    "#  If the plot shows a clear linear trend when GYRO_SCALE=1.0,\n",
    "#  there is scale error. Adjust GYRO_SCALE until there is no \n",
    "#  clear linear trend. This corresponds to the amount the gyro \n",
    "#  needs to be multipled by.\n",
    "#\n",
    "# Input is \"rays1\" which is the rays from a single scanner.\n",
    "# Right now it is hardcoded that a scan is 1081 rays.\n",
    "# First scan is assumed to be stationary. If it's not, things get a little weird.\n",
    "\n",
    "GYRO_SCALE=1.0 #0.95\n",
    "GYRO_OFFSET = 0.0005#0.0014 # this number is 0.0005 even for perfect odometry! I wonder why?\n",
    "\n",
    "%matplotlib widget\n",
    "scans=rays1.reshape(-1,1081) #organize into scans\n",
    "\n",
    "# Fill in the angles of the first scan to find offset angles of each ray\n",
    "offth = scans['th']\n",
    "tmp=scans['th'][0] # first scan angles\n",
    "idxtmp=np.arange(len(tmp))[tmp!=0]\n",
    "tmp2=tmp[tmp!=0] # first scan angles that are validish\n",
    "    # Add in endpoints\n",
    "slope=(tmp2[-1]-tmp2[0])/(idxtmp[-1]-idxtmp[0]) # Use the average slope to extrapolate endpoints\n",
    "intercept=tmp2[0]-idxtmp[0]*slope\n",
    "idxtmp=np.hstack((0,idxtmp,10000))\n",
    "tmp2=np.hstack((intercept,tmp2,10000*slope+intercept))\n",
    "offsets=np.interp(np.arange(len(tmp)),idxtmp,tmp2,left=0,right=0)\n",
    "\n",
    "# Subtract the offsets to find the robot angle over tme\n",
    "rotor=scans['th']-offsets\n",
    "decent_angles=np.unwrap(rotor.ravel()[scans['r'].ravel()>0])\n",
    "print(scans['ts'][0][0])\n",
    "decent_ts=scans['ts'][scans['r']>0].ravel()\n",
    "delt=np.diff(decent_angles,prepend=0)\n",
    "slop=np.diff(decent_angles)/np.diff(decent_ts)\n",
    "dupls=np.arange(len(slop))[np.diff(decent_ts)>.005] # jumps in timestamp indicating edge of scan\n",
    "slop[dupls]=(slop[dupls+1]+slop[dupls-1])/2 # smooth out derivative at jumps\n",
    "slop_rescale = (slop*GYRO_SCALE-GYRO_OFFSET)\n",
    "incrs=slop_rescale*np.diff(decent_ts) # factor for correcting gyro scale error\n",
    "regen_ang=np.cumsum(np.hstack((decent_angles[0],incrs)))\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(offsets,'.')\n",
    "plt.figure()\n",
    "plt.plot(decent_ts,decent_angles,',')\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(decent_angles,decent_angles-regen_ang)\n",
    "plt.title(\"Adjust GYRO_SCALE to remove slope from this line\")\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(decent_ts,decent_angles)\n",
    "plt.plot(decent_ts,regen_ang)\n",
    "plt.title(\"Adjust GYRO_OFFSET to make flat areas in both lines flat\")\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(decent_ts,decent_angles-regen_ang)\n",
    "# At end of cell:\n",
    "# \"decent_angles\" is the angles from SLAM, using original odometry/IMU to do the slope within the scan\n",
    "# \"regen_ang\" is the angles using only odometry/IMU, corrected for scale and bias\n",
    "# \"decent_ts\" is the timestamps shared by both"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    },
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "38b7d390188d4b60897ccf94b562d62a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "timestep 0.025114774703979492\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "c0c777e5af214aea888db9af3cc6bae0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f8343a80b80>]"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "# plt.plot(decent_ts,decent_angles)\n",
    "plt.plot(decent_ts,regen_ang,',')\n",
    "\n",
    "its = np.linspace(decent_ts[0],decent_ts[-1], 1.29*len(scans))\n",
    "print(\"timestep\",np.diff(its)[0])\n",
    "iang = np.interp(its,decent_ts,decent_angles)\n",
    "plt.plot(its,iang,'.')\n",
    "     \n",
    "kernel_size = 10\n",
    "kernel = np.ones(kernel_size) / kernel_size\n",
    "smoothed = np.convolve(np.hstack((iang,iang[::-1])), kernel, mode='same')[:len(iang)]#poor man's 'mirror' end conditions\n",
    "smoothed2 = np.convolve(smoothed, kernel, mode='same')\n",
    "smoothed3 = np.convolve(smoothed2, kernel, mode='same')\n",
    "comp_smooth=smoothed2+(smoothed2-smoothed3)*2\n",
    "\n",
    "# help(plt.hist)\n",
    "plt.plot(its,smoothed)\n",
    "# plt.plot(its,smoothed2)\n",
    "# plt.plot(its,smoothed3)\n",
    "# plt.plot(its,comp_smooth,'.')\n",
    "# plt.plot(its,iang-comp_smooth)\n",
    "\n",
    "# plt.hist(np.diff(smoothed)*10,1000,(-0.07,0.07))\n",
    "# plt.hist(iang-smoothed,1000,(-0.07,0.07))\n",
    "ira=np.interp(its,decent_ts,regen_ang)\n",
    "# plt.plot(its,(ira-comp_smooth)*100)\n",
    "# plt.plot(its,(ira-iang)*100)\n",
    "plt.plot(lp['ts']/1e6,np.unwrap(lp['th'])-np.mean(np.unwrap(lp['th'])), markersize=2)\n",
    "\n",
    "\n",
    "plt.figure()\n",
    "tmp=(np.diff(smoothed,prepend=0)+np.diff(smoothed,append=smoothed[-1]))*10\n",
    "# plt.plot(its,(np.diff(smoothed,prepend=0)+np.diff(smoothed,append=smoothed[-1]))*10)\n",
    "plt.plot(its,iang-ira,'.')\n",
    "plt.plot(its,np.diff(iang,prepend=0),'.')\n",
    "# plt.plot(its,smoothed-ira)\n",
    "\n",
    "# plt.plot(its,smoothed-ira -tmp*.7)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "# No idea what this cell did. Disabling.\n",
    "\n",
    "# %matplotlib widget\n",
    "# # plt.plot(decent_ts,decent_angles-regen_ang,',')\n",
    "\n",
    "# slop_rate=.030/100\n",
    "# tosum = np.hstack((decent_angles[0],incrs))\n",
    "# dts = np.hstack((0,np.diff(decent_ts)))\n",
    "# appr = regen_ang\n",
    "\n",
    "# ang=0\n",
    "# cors=[]\n",
    "# angs=regen_ang.copy()\n",
    "# for i in range(len(tosum)):\n",
    "#     ang+=tosum[i]\n",
    "#     corr=np.sign(decent_angles[i]-ang)*(dts[i]*slop_rate+0.1*np.abs(tosum[i]))\n",
    "# #     print(decent_angles[i],ang,end=':')\n",
    "#     ang+=corr\n",
    "#     angs[i]=ang\n",
    "# #     print(ang)\n",
    "# fig, axs = plt.subplots(2,sharex=True)\n",
    "# axs[0].plot(decent_ts,angs,',')\n",
    "# axs[0].plot(decent_ts,decent_angles,',')\n",
    "# axs[1].plot(decent_ts,np.cumsum(np.sign(decent_angles-angs)))\n",
    "    \n",
    "    \n",
    "# # for i in range(10):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# No idea what this cell did. Disabling.\n",
    "\n",
    "# %matplotlib widget\n",
    "# for i in range(11):\n",
    "#     corr = np.sign(decent_angles-appr)*dts*slop_rate\n",
    "\n",
    "#     appr=np.cumsum(tosum+corr)\n",
    "#     print(np.sum(np.abs(decent_angles-appr)))\n",
    "\n",
    "\n",
    "# plt.plot(decent_ts,decent_angles-appr,',')\n",
    "# print(np.sum(np.abs(decent_angles-appr)))\n",
    "# # plt.plot(corr)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# No idea what this cell did. Disabling.\n",
    "\n",
    "# %matplotlib widget\n",
    "\n",
    "# plt.plot(decent_ts,np.hstack((0,slop_rescale)),',')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "orininal rays:\t (33653692,)\n",
      "kept rays:\t (27761887,)\n"
     ]
    }
   ],
   "source": [
    "rays=rays0\n",
    "print(\"orininal rays:\\t\",rays.shape)\n",
    "r=rays['r']\n",
    "keep=(r<10) & (r>0)\n",
    "rays=rays[keep]\n",
    "print(\"kept rays:\\t\",rays.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3af1b6580c9d46f49b91669051b42d23",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "dd3d813fb49d4c8e9c08dcce7827e2ec",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(IntSlider(value=127, description='number', max=8000, min=2), Output()), _dom_classes=('w…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import ipywidgets\n",
    "%matplotlib widget\n",
    "from ipywidgets import interact, interact_manual\n",
    "from IPython.display import display,clear_output\n",
    "# rays2=rays1\n",
    "# https://towardsdatascience.com/interactive-controls-for-jupyter-notebooks-f5c94829aee6\n",
    "# reveals issues: https://stackoverflow.com/questions/61715937/how-to-enable-matplotlib-blitting-to-create-fast-realtime-graph-in-jupyterlab\n",
    "# \n",
    "number = 300\n",
    "plt.plot(rays['ex'][number*1081:(number+1)*1081],rays['ey'][number*1081:(number+1)*1081],'r,',c=[.5,.5,.5,.5])\n",
    "\n",
    "@interact(number=(2,8000,1))\n",
    "def display_scan(number=127):\n",
    "    xlim=plt.gca().get_xlim()\n",
    "    ylim=plt.gca().get_ylim()\n",
    "    plt.gcf().clear()\n",
    "    \n",
    "#     plt.plot(rays2['ex'][number*1081:(number+1)*1081],rays2['ey'][number*1081:(number+1)*1081],'r,',c=[.5,.5,.5,.5])\n",
    "    number -= 1\n",
    "    plt.plot(rays2['ex'][number*1081:(number+1)*1081],rays2['ey'][number*1081:(number+1)*1081],'r.')\n",
    "    number += 1\n",
    "    plt.plot(rays2['ex'][number*1081:(number+1)*1081],rays2['ey'][number*1081:(number+1)*1081],'g.')\n",
    "    number +=1 \n",
    "    plt.plot(rays2['ex'][number*1081:(number+1)*1081],rays2['ey'][number*1081:(number+1)*1081],'b.')\n",
    "    plt.plot(rays2['ex'][number*1081:(number)*1081+768],rays2['ey'][number*1081:(number)*1081+768],'c.')\n",
    "    if rays2['ts'][number*1081]-rays2['ts'][(number-1)*1081]>1/30:\n",
    "        plt.plot(rays2['ex'][number*1081:(number)*1081+384],rays2['ey'][number*1081:(number)*1081+384],'k.')\n",
    "        plt.title('Timestamp jump = Long (1)')\n",
    "    else:\n",
    "        plt.plot(rays2['ex'][number*1081:(number)*1081+384],rays2['ey'][number*1081:(number)*1081+384],'k.')\n",
    "        plt.title('Timestamp jump = Short (0)')\n",
    "    number -=1\n",
    "    plt.plot(rays2['sx'][number*1081:(number+1)*1081],rays2['sy'][number*1081:(number+1)*1081],'b.')\n",
    "\n",
    "    if xlim[1]!=1:\n",
    "        plt.xlim(xlim)\n",
    "        plt.ylim(ylim)\n",
    "#     plt.gcf().canvas.draw()\n",
    "    s=plt.gcf().canvas\n",
    "    _, _, w, h = s.figure.bbox.bounds\n",
    "#     s._png_is_old = True\n",
    "#     s.handle_refresh(1)\n",
    "#     print(s._png_is_old)\n",
    "    s.manager.resize(w, h)\n",
    "    \n",
    "#     plt.gcf().canvas.set_image_mode('full')\n",
    "    \n",
    "\n",
    "# plt.figure()\n",
    "# for n in range(166,200):\n",
    "#     plt.plot(rays2['ey'][n*1081:(n+1)*1081])\n",
    "# plt.plot(rays2['ey'][198*1081:(198+1)*1081]) # weird jump at 383 to 384. Appears to repeat several data points (13?)\n",
    "\n",
    "\n",
    "# DISCOVERY: The hokuyo has a glitch where sometimes the points [384:] are from one scan in the future relative to the early points!\n",
    "# DISCOVERY2: Sometimes its the first points that go to the future!\n",
    "# DISCOVERY3: It happens to both sensors\n",
    "# DISCOVERY4: Actually there are at least 3 blocks of points [0:384][384:768][768:] that can jump to the future, as well as the timestamp. None of the 4 have a clear relation\n",
    "# I wonder if the possibility of this is detectable from the timestamps? Like does it always happen before or after a missing scan? A: seems unpredictable :-(\n",
    "# Also I notice that it can persist for a couple of scans, so that several in a row are split.\n",
    "\n",
    "# Laser id 0\n",
    "# Observations start at 2245\n",
    "#                             a      a|\n",
    "# # 5678901234567890123456789012345678|901234567890123456789\n",
    "# S 0001100100110110011011010!00!00!!0|!!0000110110010011\n",
    "# C 011001001000100010010?100100100010|0100110010010001001\n",
    "# B 0101000100100100010010010010010001|001010100100010010 // Higher rays jumped (not sure, but mostly [384:768]\n",
    "# A 1100010010001001000100100100100100|100010001001001000 // [0:384] jumped\n",
    "# Y 0101001001001000100100100010010010|010011001000100100 // timestamp jumped\n",
    "\n",
    "# Laser id 0\n",
    "#  2590\n",
    "#  90123456789012345678901234567890\n",
    "#Y 101000010000100000100001000001\n",
    "#C 101000010001000001000001000001\n",
    "#B 100100001000001000010000010000\n",
    "#A 100000100010000010000010000010\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "# utility to find the ray nearest a certain point\n",
    "def nearray(rays, x,y):\n",
    "    return np.argmin((rays['ex']-x)**2+(rays['ey']-y)**2)\n",
    "\n",
    "def plotcolored(x,y,phi):\n",
    "    # plots multicolored data with phi indicating color angle\n",
    "    rgb_cycle = np.vstack((            # Three sinusoids\n",
    "    .5*(1.+np.cos(phi          )), # scaled to [0,1]\n",
    "    .5*(1.+np.cos(phi+2*np.pi/3)), # 120° phase shifted.\n",
    "    .5*(1.+np.cos(phi-2*np.pi/3)))).T\n",
    "    idx = np.round(phi%(2*np.pi)/np.pi*30) #30 divisions for now\n",
    "    for i in np.unique(idx):\n",
    "        plt.plot(x[idx==i],y[idx==i],',',c=rgb_cycle[idx==i][0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1123170315f2406d8bde221183184644",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "31061\n",
      "23429771\n"
     ]
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "cut=0\n",
    "raysd=rays[(rays['i']>=cut)]#&(rays['idx']==1)]\n",
    "span=540\n",
    "# plt.plot(raysd['ex'][5925215-span:5925215+span],raysd['ey'][5925215-span:5925215+span],'r,')\n",
    "# print(raysd['ex'][11706031])\n",
    "plt.gca().set_facecolor('xkcd:black')\n",
    "cycle_time = 10.0\n",
    "phi=(raysd['ts']-raysd['ts'][0])*np.pi*2/cycle_time\n",
    "rgb_cycle = np.vstack((            # Three sinusoids\n",
    "    .5*(1.+np.cos(phi          )), # scaled to [0,1]\n",
    "    .5*(1.+np.cos(phi+2*np.pi/3)), # 120° phase shifted.\n",
    "    .5*(1.+np.cos(phi-2*np.pi/3)))).T\n",
    "plotcolored(raysd['ex'],raysd['ey'],phi)\n",
    "# line=plt.plot(raysd['ex'],raysd['ey'],',')#s=1,linewidth=0,c=rgb_cycle,antialiased=False)\n",
    "# help(line[0])\n",
    "\n",
    "lpc = (lp['ts']>np.min(raysd['ts'])) & (lp['ts']<np.max(raysd['ts']))\n",
    "plt.plot(lp['x'][lpc],lp['y'][lpc])\n",
    "print(np.sum(lpc>0))\n",
    "\n",
    "# # Plot Odometry\n",
    "# plt.plot(rays['sx'][::10],rays['sy'][::10],'r,')\n",
    "# plt.plot(odo['x'],odo['y'], ',')\n",
    "# plt.plot(lp['lx'],lp['ly'], 'c.')\n",
    "# plt.gca().grid('on')\n",
    "plt.gca().set_aspect('equal','box')\n",
    "\n",
    "# #Plot a slice of interest\n",
    "# s1=533456\n",
    "# s2=534456\n",
    "# plt.plot(rays1['ex'][s1:s2],rays1['ey'][s1:s2],'g.')\n",
    "# plt.plot(rays1['sx'][s1:s2],rays1['sy'][s1:s2],'b.')\n",
    "\n",
    "\n",
    "# plt.plot(raysd['ex'][slc:10000000],raysd['ey'][slc:10000000],'b,')\n",
    "\n",
    "\n",
    "# plt.plot(rays1['sx'],rays1['sy'],'r,')\n",
    "# plt.plot(raysd['sx'][1::2],raysd['sy'][1::2],'k,')\n",
    "# plt.plot(raysr['sx'][::2],raysr['sy'][::2],'r.-')\n",
    "# plt.plot(raysr['sx'][1::2],raysr['sy'][1::2],'k.-')\n",
    "# plt.show()\n",
    "# plt.figure()\n",
    "# plt.plot(raysd['th'],'b,')\n",
    "\n",
    "\n",
    "\n",
    "ags=np.argsort(odo['ts'])\n",
    "\n",
    "# plt.plot(odo['ts'][:-1]/1e6,1/40*np.diff(odo['x'])/(np.diff(odo['ts'])/1e6),',')\n",
    "# plt.plot(odo['ts'][:-1]/1e6,1/40*np.diff(lp['th'])/(np.diff(lp['ts'])/1e6),'r,')\n",
    "# plt.plot(odo['ts']/1e6,odo['th'])\n",
    "\n",
    "print(nearray(raysd,16.08,-2.72))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[(4743979136498050342, 38.492737, 68.95678 , -0.03687911, 4.5729974e-41, 38.92543 , 68.75069 )\n",
      " (4743979136498050342, 38.492737, 68.95678 , -0.03687911, 4.5729974e-41, 38.92543 , 68.75069 )\n",
      " (4743979136498076850, 38.492737, 68.95678 , -0.03687911, 4.5729974e-41, 38.112957, 69.240974)\n",
      " (4743979136498076850, 38.492737, 68.95678 , -0.03687911, 4.5729974e-41, 38.112957, 69.240974)\n",
      " (4743979136498286033, 38.544315, 68.978806, -0.03882421, 2.3509887e-38, 38.16509 , 69.26374 )\n",
      " (4743979136498365230, 38.52776 , 69.00385 , -0.04929638, 4.5729974e-41, 38.957863, 68.792404)\n",
      " (4743979136498390995, 38.557693, 69.01329 , -0.03992172, 4.5729974e-41, 38.17878 , 69.29864 )\n",
      " (4743979136498470192, 38.54246 , 69.0203  , -0.0418326 , 4.5729974e-41, 38.97413 , 68.812065)\n",
      " (4743979136498495958, 38.602333, 69.026184, -0.04493878, 4.5729974e-41, 38.224857, 69.31343 )\n",
      " (4743979136498574924, 38.554256, 69.01455 , -0.04249115, 4.5729974e-41, 38.98579 , 68.80603 )\n",
      " (4743979136498600920, 38.535244, 69.021   , -0.04435968, 4.5729974e-41, 38.1576  , 69.30803 )\n",
      " (4743979136498679886, 38.541023, 69.01769 , -0.04342902, 4.5729974e-41, 38.97236 , 68.80877 )\n",
      " (4743979136498705883, 38.554947, 69.02978 , -0.04165119, 4.5729974e-41, 38.17653 , 69.31578 )\n",
      " (4743979136498784849, 38.571804, 69.0163  , -0.04029698, 4.5729974e-41, 39.00379 , 68.80872 )\n",
      " (4743979136498810845, 38.571823, 69.0068  , -0.0431133 , 4.5729974e-41, 38.19382 , 69.29336 )]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "870f50a443674db8be6fcdff15fda990",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ 3.8549557e+01  6.9022667e+01 -4.1704800e-02]\n",
      "[38.549557, 69.02267, -0.0417048]\n"
     ]
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "print(lp[0:15])\n",
    "plt.plot(lp[4:50]['x'],lp[4:50]['y'],'b.')\n",
    "sloc = np.mean(np.vstack((lp[15:50]['x'],lp[15:50]['y'],lp[15:50]['th'])),axis=1)\n",
    "print(sloc)\n",
    "plt.plot(sloc[0],sloc[1],'r.')\n",
    "print(list(np.round(sloc,8)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a87cb1f521ae4809a040b74a48db00c7",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f6b6046b820>]"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#Debug Odometry directly\n",
    "\n",
    "%matplotlib widget\n",
    "\n",
    "t=odo['ts']/1e6\n",
    "x=odo['x']\n",
    "y=odo['y']\n",
    "th=odo['th']\n",
    "ex = x - np.sin(th)*4\n",
    "ey = y + np.cos(th)*4\n",
    "\n",
    "plt.plot(ex,ey,'.')\n",
    "\n",
    "ex = x + np.cos(th)*4\n",
    "ey = y + np.sin(th)*4\n",
    "\n",
    "plt.plot(ex,ey,'.')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ex -23.340164 30.148087 53.48825\n",
      "ey -11.5921545 75.36302 86.95518\n"
     ]
    }
   ],
   "source": [
    "## Find the max extent of the map\n",
    "np.max(rays['ex'])\n",
    "for c in ['ex','ey']:\n",
    "    print(c, np.min(rays[c]),np.max(rays[c]), np.max(rays[c])-np.min(rays[c]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "fcorns\n",
      " [[     -23.34      -11.59]\n",
      " [      30.15       75.36]]\n",
      "pxc_corns\n",
      " [[   -2334.02     1159.22]\n",
      " [    3014.81    -7536.3 ]]\n",
      "pxc_topleft\n",
      " [   -2335       -7537   ]\n",
      "pxc_btmright\n",
      " [    3015        1160   ]\n",
      "pxc_extent\n",
      " [    5350        8697   ]\n",
      "px_corns\n",
      " [[       0.98     8696.22]\n",
      " [    5349.81        0.7 ]]\n",
      "px_topleft\n",
      " [       0           0   ]\n",
      "px_btmright\n",
      " [    5350        8697   ]\n",
      "px_extent\n",
      " [    5350        8697   ]\n"
     ]
    }
   ],
   "source": [
    "## Figure out rasterization\n",
    "# Floor of float coords is integer coords\n",
    "# Images are y flipped\n",
    "\n",
    "def homp(T,pts):\n",
    "    tmppts = pts @ T[:-1,:-1].T + T[:-1,-1]\n",
    "    denom = pts @ T[-1:,:-1].T + T[-1,-1]\n",
    "    return tmppts/denom\n",
    "\n",
    "\n",
    "CELLS_PER_M = 100.0\n",
    "\n",
    "formatter = {'float':lambda x:np.format_float_positional(x,precision=2,fractional=True,trim='-',pad_left=8,pad_right=2)}\n",
    "\n",
    "def get_T_px_f_with_extent(rays, debug=False):\n",
    "    # The corners in world space\n",
    "    f_corns = np.array(\n",
    "        [[np.min(rays['ex']),np.min(rays['ey'])],\n",
    "         [np.max(rays['ex']),np.max(rays['ey'])]])\n",
    "    T_pxc_f = np.array(\n",
    "        [[1*CELLS_PER_M, 0             , 0],\n",
    "         [0            , -1*CELLS_PER_M, 0],\n",
    "         [0            , 0             , 1]])\n",
    "    pxc_corns=homp(T_pxc_f,f_corns)\n",
    "    pxc_topleft = np.array([  np.floor(np.min(pxc_corns[:,0])), np.floor(np.min(pxc_corns[:,1]))  ]) #inclusive\n",
    "    pxc_btmright= np.array([  np.floor(np.max(pxc_corns[:,0])), np.floor(np.max(pxc_corns[:,1]))  ]) + 1 #exclusive\n",
    "    pxc_extent = pxc_btmright-pxc_topleft\n",
    "\n",
    "    T_px_f = T_pxc_f.copy()\n",
    "    T_px_f[:-1,-1] = -pxc_topleft\n",
    "\n",
    "    px_corns=homp(T_px_f,f_corns)\n",
    "    px_topleft = np.array([  np.floor(np.min(px_corns[:,0])), np.floor(np.min(px_corns[:,1]))  ]) #inclusive\n",
    "    px_btmright= np.array([  np.floor(np.max(px_corns[:,0])), np.floor(np.max(px_corns[:,1]))  ]) + 1 #exclusive\n",
    "    px_extent = px_btmright-px_topleft\n",
    "\n",
    "    assert (np.all(pxc_extent == px_extent))\n",
    "\n",
    "    if debug:\n",
    "        print(\"fcorns\\n\",f_corns)\n",
    "\n",
    "        for var in ['pxc_corns',\n",
    "                    'pxc_topleft',\n",
    "                    'pxc_btmright',\n",
    "                    'pxc_extent',\n",
    "                    'px_corns',\n",
    "                    'px_topleft',\n",
    "                    'px_btmright',\n",
    "                    'px_extent',\n",
    "                    ]:\n",
    "            print(var+'\\n', eval(var))\n",
    "    return T_px_f, px_extent.astype('int64')\n",
    "\n",
    "with np.printoptions(formatter=formatter):\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays, debug=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a0b4f28fd05e4416848e6b0dc8a3187a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "maxhits= 1229.0\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a409449f19b94c34b5e646f95ee452ce",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fe6d0c37ee0>]"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# rasterize hits\n",
    "\n",
    "def get_raster_coords(rays, T_px_f):\n",
    "    # implicit inputs: T_px_f\n",
    "    f_ept = np.vstack((rays['ex'].ravel(),rays['ey'].ravel())).T.copy()\n",
    "    px_coords = homp(T_px_f, f_ept)\n",
    "    px_coords = px_coords.reshape(rays['ex'].shape+(2,))\n",
    "    return px_coords\n",
    "\n",
    "px_coords = get_raster_coords(rays0, T_px_f)\n",
    "\n",
    "bad0=rays0['r']<=0\n",
    "bad0|=rays0['r']>59\n",
    "\n",
    "img = np.zeros(px_extent[::-1])\n",
    "\n",
    "# clip out bad pixels\n",
    "bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "px_coords = px_coords[bad==0,:]\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "np.add.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), 1)\n",
    "%matplotlib widget\n",
    "\n",
    "imsc=img.copy()\n",
    "imsc[img!=0]=np.log10(img[img!=0])+0.301\n",
    "\n",
    "\n",
    "\n",
    "plt.imshow(imsc)\n",
    "import cv2\n",
    "cv2.imwrite(\"rendered_image_linear.png\",img.astype(np.uint16))\n",
    "print('maxhits=',np.max(img))\n",
    "plt.figure()\n",
    "plt.plot(img[900,:],'.')\n",
    "# plt.hist(rays0['r'],100)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "14bc9c3b5984401ca398cd00426ffe34",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "<matplotlib.image.AxesImage at 0x7f930c9c9b80>"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# It turns out that gaussian blurring and selecting where >1 produces a very good approximation of where the walls are\n",
    "# without much reflection. This cell does that calculation\n",
    "%matplotlib widget\n",
    "\n",
    "def produce_wall_mask(rays0):\n",
    "    import cv2\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays) # extents and transform\n",
    "    px_coords = get_raster_coords(rays0, T_px_f) # pixel coordinates\n",
    "    img = np.zeros(px_extent[::-1]) #image to fill\n",
    "\n",
    "    # clip out bad rays\n",
    "    bad0=rays0['r']<=0\n",
    "    bad0|=rays0['r']>59\n",
    "    # clip out bad pixels\n",
    "    bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "    px_coords = px_coords[bad==0,:]\n",
    "\n",
    "    np.add.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), 1)\n",
    "    \n",
    "    gaussian_func = lambda img:cv2.GaussianBlur(img,(61,61),sigmaX=3,borderType=cv2.BORDER_DEFAULT)\n",
    "\n",
    "    local_mean = gaussian_func(img)\n",
    "    return local_mean>1.0\n",
    "wall_mask=produce_wall_mask(rays0)\n",
    "plt.imshow(wall_mask)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "97b9509563074b46912880232847aca4",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "def produce_fwhm_mask(rays0):\n",
    "    import cv2\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays) # extents and transform\n",
    "    px_coords = get_raster_coords(rays0, T_px_f) # pixel coordinates\n",
    "    img = np.zeros(px_extent[::-1]) #image to fill\n",
    "\n",
    "    # clip out bad rays\n",
    "    bad0=rays0['r']<=0\n",
    "    bad0|=rays0['r']>59\n",
    "    # clip out bad pixels\n",
    "    bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "    px_coords = px_coords[bad==0,:]\n",
    "\n",
    "    np.add.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), 1)\n",
    "    \n",
    "    kernel = np.ones((10,10), np.uint8)\n",
    "    dil=cv2.dilate(img, kernel, borderType=cv2.BORDER_DEFAULT)\n",
    "    plt.imshow((img>=dil/2)*(img>3))\n",
    "\n",
    "    return (img>dil/2)\n",
    "fwhm_mask=produce_fwhm_mask(rays0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 87,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bf293a368b8b47808274bf2407bb4e1f",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "6cedca9af16e49759468d62a8d0dddae",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f515aa93be0>]"
      ]
     },
     "execution_count": 87,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# load gt data and estimate normals\n",
    "%matplotlib widget\n",
    "import cv2\n",
    "\n",
    "gtrgb=cv2.imread(\"gtmap.png\")\n",
    "# plt.imshow((gtrgb[:,:,1]>0)+0.5*fwhm_mask)\n",
    "gtwall = gtrgb[:,:,1]>0\n",
    "gtwall_coords = np.array(np.nonzero(gtwall))[::-1].T+0.5\n",
    "\n",
    "\n",
    "# see the distribution of coordinates by distance\n",
    "from scipy.spatial import KDTree\n",
    "tree= KDTree(gtwall_coords)\n",
    "d,nn=tree.query(px_coords[::360],distance_upper_bound=10.0)\n",
    "# d,nn=tree.query(px_coords[:108100],distance_upper_bound=10.0)\n",
    "\n",
    "# histogram out to +- 0.5 cell spacing is linear ramp, because it is an expanding circle. Beyond that it should approach gaussian in absense of localization error\n",
    "# arclen=arcsin(CELL_SPACING/2/x)*4/(2*pi)*(2*pi*r)\n",
    "# density = exp(-2*(x/SD)**2) # where x points in the normal direction\n",
    "REGRESSION_SD=2.0\n",
    "plt.hist(d[d<10],100)\n",
    "x=np.linspace(0.01,10,1000)\n",
    "plt.plot(x,np.arcsin(np.clip(0.5/x,0,1))*4*x*np.exp(-(x/REGRESSION_SD)**2)*1000)#1000 scale factor for visualization, adjust as needed\n",
    "\n",
    "# plt.plot(px_coords[:108100,0],px_coords[:108100,1],',')\n",
    "plt.figure()\n",
    "plt.plot(gtwall_coords[:,0],gtwall_coords[:,1],',')\n",
    "# x=np.linspace(0,10)\n",
    "# plt.plot(x,np.exp(-(x/4)**2)*10000)\n",
    "# print(nn)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "14929\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "b2a8f6da939348759425ae883e7258a6",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[-218.34532374]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0451ec9cd3244c82b01d2dab4b25a586",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "<matplotlib.image.AxesImage at 0x7f930c8fda90>"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "def produce_max_inten_image(rays0):\n",
    "    import cv2\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays) # extents and transform\n",
    "    px_coords = get_raster_coords(rays0, T_px_f) # pixel coordinates\n",
    "    img = np.zeros(px_extent[::-1]) #image to fill\n",
    "\n",
    "    # clip out bad rays\n",
    "    bad0=rays0['r']<=0\n",
    "    bad0|=rays0['r']>59\n",
    "    bad0|=np.abs(rays0['th']+1.627-np.pi*3/2)>.0305\n",
    "    # clip out bad pixels\n",
    "    bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "    px_coords = px_coords[bad==0,:]\n",
    "    \n",
    "    inten = rays0['i'][bad==0]\n",
    "\n",
    "    np.maximum.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), inten)\n",
    "#     plt.imshow(img)\n",
    "    f2=np.all(np.abs(np.floor(px_coords)-[2101,4168])<1000,axis=1) #top right:[3773,960] , mid left:[363,3871], alcove:[2018,2032], inner pillar:[2483,713]\n",
    "    print(np.sum(f2))\n",
    "    \n",
    "    angs = np.interp(rays0['ts'],decent_ts,decent_angles)\n",
    "#     plt.plot(rays0['ts'][bad==0][f2],(rays0['th'])[bad==0][f2],'.')\n",
    "#     plt.plot(rays0['r'][bad==0][f2],rays0['i'][bad==0][f2],'.')\n",
    "    plt.plot(rays0['th'][bad==0][f2],rays0['i'][bad==0][f2]+rays0['r'][bad==0][f2]*0,',')\n",
    "#     plt.plot(rays0['r'][bad==0][f2],rays0['i'][bad==0][f2]+rays0['r'][bad==0][f2]*0,',')\n",
    "\n",
    "#     plt.plot(rays0['ts'][bad==0][f2],rays0['i'][bad==0][f2],'.')\n",
    "#     plt.plot(rays0['ts'][bad==0][f2],rays0['r'][bad==0][f2]*100,'.')\n",
    "#     plt.plot(rays0['ts'][bad==0][f2],rays0['th'][bad==0][f2]*1000,'.')\n",
    "\n",
    "\n",
    "#     plt.plot(rays0['i'][bad==0][f2],'.')\n",
    "\n",
    "    plt.plot([2.37,7.93],[3105,1891])\n",
    "    print(np.diff([3105,1891])/np.diff([2.37,7.93]))\n",
    "    \n",
    "    return img\n",
    "maxinten=produce_max_inten_image(rays0)\n",
    "plt.figure()\n",
    "plt.imshow(maxinten>0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "expected best for line: 7.519884823893004\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "900c79b2db524b089c8471c45bcd820c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "730015816d624078aedf7d84145b8f0d",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(858, 1257)\n",
      "best observed: 3.6316730282930405\n"
     ]
    }
   ],
   "source": [
    "#try some local enhancement:\n",
    "%matplotlib widget\n",
    "\n",
    "import cv2\n",
    "gaussian_func = lambda img:cv2.GaussianBlur(img,(61,61),sigmaX=3,borderType=cv2.BORDER_DEFAULT)\n",
    "DECENT_LINE_FRACTION = 0.7\n",
    "\n",
    "local_mean = gaussian_func(img)\n",
    "\n",
    "line_img = np.zeros((300,300))\n",
    "line_img[150]=100\n",
    "print(\"expected best for line:\",100/np.max(gaussian_func(line_img)))\n",
    "# plt.imshow(np.clip(img/(local_mean+1),0,10)>.8)\n",
    "kernel_max = 1/np.max(img/(local_mean+0.0000001)) # appoximation of the center value of the gaussian kernel\n",
    "gaussian_without_center=(local_mean-img*kernel_max)/(1-kernel_max)#mean estimate ignoring center pixel\n",
    "denom=np.clip(local_mean,.0001,1e9) # considered gaussian_without_center, but nope \n",
    "qual_analog=np.clip(np.clip(img-np.sqrt(img),0,1e9)/denom,0,50)#lower bound of conf interval scales with sqrt\n",
    "determiner = (qual_analog>0.7)*(img>0)\n",
    "# plt.imshow((qual_analog>0.7) +(img>0))  \n",
    "plt.imshow(img>0)\n",
    "plt.figure()\n",
    "plt.plot(img[630:645,1257],'.')\n",
    "print(np.unravel_index(np.argmax(img),img.shape))\n",
    "# plt.imshow(qual_analog>.7)\n",
    "# plt.plot(np.sort(qual_analog.ravel()))\n",
    "print(\"best observed:\",np.max(img)/local_mean.ravel()[np.argmax(img)])\n",
    "# plt.imshow((img>2)*0.5+(np.clip(img/(local_mean+1),0,10)>.8)*0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# find local maxima for realigning scans\n",
    "%matplotlib widget\n",
    "\n",
    "kernels=np.array([\n",
    "        [\n",
    "            [0,0,1],\n",
    "            [0,1,0],\n",
    "            [1,0,0.0],\n",
    "        ],\n",
    "        [\n",
    "            [0,1,0],\n",
    "            [0,1,0],\n",
    "            [0,1,0],\n",
    "        ],\n",
    "        [\n",
    "            [1,0,0],\n",
    "            [0,1,0],\n",
    "            [0,0,1],\n",
    "        ],\n",
    "        [\n",
    "            [0,0,0],\n",
    "            [1,1,1],\n",
    "            [0,0,0],\n",
    "        ],\n",
    "    ]).astype(np.uint8)\n",
    "sol=np.zeros_like(local_mean)\n",
    "gaussian_func_s = lambda img:cv2.GaussianBlur(img,(61,61),sigmaX=3,borderType=cv2.BORDER_DEFAULT)\n",
    "ridgeimg=gaussian_func_s(img)\n",
    "for kernel in kernels:\n",
    "    sol+=(ridgeimg==cv2.dilate(ridgeimg, kernel, iterations=1))\n",
    "plt.imshow(np.clip(sol,0,8)*(local_mean>0.5)+(img>20))\n",
    "print(\"nonmax\",np.sum(img>0),np.sum((img>0)*(sol>0)),np.sum(img>1),np.sum((img>1)*(sol>0)))\n",
    "print(np.sum((sol>0)*(local_mean>0.5)))\n",
    "\n",
    "cores=np.array(np.nonzero((sol>0)*(local_mean>0.5))).T+[0.5,0.5]\n",
    "\n",
    "from scipy.spatial import KDTree\n",
    "\n",
    "tree=KDTree(cores)\n",
    "T_px_f, px_extent = get_T_px_f_with_extent(rays) # extents and transform\n",
    "px_coords = get_raster_coords(rays0, T_px_f) # pixel coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "18135937\n"
     ]
    },
    {
     "ename": "IndexError",
     "evalue": "index 126014 is out of bounds for axis 0 with size 126014",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mIndexError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-31-4494db87119b>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0md\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mmatchidx\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtree\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mquery\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpx_coords\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;36m100000\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0meps\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mdistance_upper_bound\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m20\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 4\u001b[0;31m \u001b[0merror\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mpx_coords\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mcores\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mmatchidx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      5\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfigure\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      6\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mhist\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mrays0\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'r'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m>\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mIndexError\u001b[0m: index 126014 is out of bounds for axis 0 with size 126014"
     ]
    }
   ],
   "source": [
    "print(len(px_coords))\n",
    "\n",
    "d,matchidx = tree.query(px_coords[:,::-1][:100000], eps=2,distance_upper_bound=20) # can only query 10k pts per second :-(\n",
    "error = px_coords[:,::-1]-cores[matchidx]\n",
    "plt.figure()\n",
    "plt.hist(d[rays0['r']>0], )\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cimshow(img):\n",
    "    dpi = 200 # Affects only tick and text sizes\n",
    "    margin = 0.05/(np.max(img.shape)/100) # (5% of the width/height of the figure...)\n",
    "    xpixels, ypixels = img.shape[1], img.shape[0]\n",
    "\n",
    "    # Make a figure big enough to accomodate an axis of xpixels by ypixels\n",
    "    # as well as the ticklabels, etc...\n",
    "    figsize = (1 + margin) * ypixels / dpi, (1 + margin) * xpixels / dpi\n",
    "\n",
    "    fig = plt.figure(figsize=figsize, dpi=dpi)\n",
    "    # Make the axis the right size...\n",
    "    ax = fig.add_axes([margin, margin, 1 - 2*margin, 1 - 2*margin])\n",
    "\n",
    "    ax.imshow(img, interpolation='none')\n",
    "    return ax"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f8f158adc2194c6f9c093c0fa29cd8e0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1676189\n",
      "1692366\n",
      "1698652\n",
      "1698657\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "160640ac83ef498fb719c9209e8983f3",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1c12d4f2e2fa45aab2e43b4c601d747a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-0.10840484271694317\n",
      "3.861872264348472\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 93,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "import scipy.signal as signal\n",
    "\n",
    "inten=rays1['i'].reshape(-1,1081)\n",
    "plt.imshow(inten[:3000])\n",
    "\n",
    "#(img>2)*0.5+(np.clip(img/(local_mean+1),0,10)>.8)*0.5\n",
    "px_xy = get_raster_coords(rays2.reshape(-1,1081), T_px_f)\n",
    "bad=rays2['r'].reshape(-1,1081)<=0\n",
    "bad|=rays2['r'].reshape(-1,1081)>59\n",
    "print(np.sum(bad))\n",
    "bad|=px_xy[:,:,0]<0\n",
    "bad|=px_xy[:,:,1]<0\n",
    "# bad|=np.any(px_xy,axis=2)<0\n",
    "print(np.sum(bad))\n",
    "bad|=px_xy[:,:,0]>img.shape[1]\n",
    "print(np.sum(bad))\n",
    "bad|=px_xy[:,:,1]>img.shape[0]\n",
    "print(np.sum(bad))\n",
    "\n",
    "\n",
    "px_inds = tuple(np.rollaxis(np.floor(px_xy)[...,::-1].astype('int32'),-1))\n",
    "# print(px_inds.shape)\n",
    "flatinds = np.ravel_multi_index(px_inds,img.shape,mode='clip') # need to hand linearize to specify what to do with bad inds\n",
    "img[0,:]=0\n",
    "img[:,0]=0\n",
    "img_samp = img.ravel()[flatinds].reshape(-1,1081)\n",
    "mean_samp = local_mean.ravel()[flatinds].reshape(-1,1081)\n",
    "detr_samp = determiner.ravel()[flatinds].reshape(-1,1081)\n",
    "\n",
    "qual=np.clip(np.clip(img_samp-np.sqrt(img_samp),0,1e9)/(mean_samp+0.00001),0,10)\n",
    "metric = np.sum(qual>1.0,axis=1)/np.sum(bad==False,axis=1)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(metric)\n",
    "# plt.plot(np.sum(img_samp>10,axis=1))\n",
    "# plt.plot(metric,'.')\n",
    "\n",
    "cutoff=signal.order_filter(metric,np.ones(61),45)\n",
    "metric_flattened = metric-cutoff\n",
    "metric_flattened[:31]=metric_flattened[31]\n",
    "metric_flattened[-31:]=metric_flattened[-31]\n",
    "to_keep=metric_flattened\n",
    "to_keep=to_keep.reshape(-1,1)+np.zeros(1081)\n",
    "# print(to_keep[100:120,100:110])\n",
    "plt.plot(cutoff)\n",
    "# plt.plot(metric_flattened)\n",
    "plt.plot(np.sort(metric))\n",
    "\n",
    "# display filtered points\n",
    "imf1=np.zeros_like(img)\n",
    "# px_inds=np.array(px_inds)\n",
    "# np.maximum.at(imf.ravel(), flatinds, (metric-signal.order_filter(metric,np.ones(21),5)).ravel())\n",
    "# np.maximum.at(imf1.ravel(), flatinds[bad==False], -to_keep[bad==False])\n",
    "np.maximum.at(imf1.ravel(), flatinds[bad==False], to_keep[bad==False]+1)\n",
    "\n",
    "# np.maximum.at(imf1.ravel(), flatinds, to_keep==51)\n",
    "\n",
    "# np.add.at(imf1.ravel(), flatinds, (-1.0*(to_keep<0)))\n",
    "# np.add.at(imf1.ravel(), flatinds[bad==False], (1.0*(to_keep>0))[bad==False])\n",
    "# np.add.at(imf1.ravel(), flatinds, 1.0)\n",
    "\n",
    "im=np.clip(imf1,-100,1)+determiner*0.2\n",
    "plt.figure()\n",
    "plt.imshow(img/(local_mean+10))\n",
    "ce=5112\n",
    "plt.plot(px_xy[ce-1,:,0],px_xy[ce-1,:,1],'r.')\n",
    "plt.plot(px_xy[ce,:,0],px_xy[ce,:,1],'.')\n",
    "plt.plot(px_xy[ce+1,:,0],px_xy[ce+1,:,1],'.')\n",
    "\n",
    "# cimshow(im[:2000,:5000])\n",
    "print(metric_flattened[2198])\n",
    "print(np.max(img/(local_mean+10)))\n",
    "goodgrad=img/(local_mean+10)\n",
    "cv2.imwrite('tmp.png',goodgrad/3.8*256)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 92,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 92,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# rasterize hits by time\n",
    "\n",
    "%matplotlib widget\n",
    "colorang=rays0['ts']\n",
    "colorvec=2*np.vstack([np.cos(colorang),np.sin(colorang)])\n",
    "rv=[1,0.0]\n",
    "gv=[np.cos(2/3*np.pi),np.sin(2/3*np.pi)]\n",
    "bv=[np.cos(4/3*np.pi),np.sin(4/3*np.pi)]\n",
    "th=np.linspace(0,7)\n",
    "r=np.clip(np.dot(rv,colorvec),-1,1)*0.5+0.5\n",
    "g=np.clip(np.dot(gv,colorvec),-1,1)*0.5+0.5\n",
    "b=np.clip(np.dot(bv,colorvec),-1,1)*0.5+0.5\n",
    "# plt.plot(r)\n",
    "# plt.plot(g)\n",
    "# plt.plot(b)\n",
    "\n",
    "\n",
    "f_ept = np.vstack((rays0['ex'],rays0['ey'])).T.copy()\n",
    "bad0=rays0['r']<=0\n",
    "bad0|=rays0['r']>5\n",
    "\n",
    "im3=np.zeros(list(px_extent[::-1])+[3])\n",
    "px_coords = homp(T_px_f, f_ept)\n",
    "\n",
    "px_xyrgb = np.hstack((px_coords,r[:,np.newaxis],g[:,np.newaxis],b[:,np.newaxis]))\n",
    "bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "del px_coords\n",
    "\n",
    "px_xyrgb = px_xyrgb[bad==0]\n",
    "px_xy=np.floor(px_xyrgb[:,0:2]).astype('int64')\n",
    "\n",
    "lindex = np.ravel_multi_index((px_xy[:,1],px_xy[:,0]), px_extent[::-1],mode='raise')\n",
    "\n",
    "im3.ravel()[3*lindex]=px_xyrgb[:,2]\n",
    "im3.ravel()[3*lindex+1]=px_xyrgb[:,3]\n",
    "im3.ravel()[3*lindex+2]=px_xyrgb[:,4]\n",
    "\n",
    "# plt.imshow(im3)\n",
    "import cv2\n",
    "cv2.imwrite('color_by_time.png',\n",
    "            (np.concatenate(\n",
    "                (im3[:,:,::-1]*255.9,255*np.ones(list(px_extent[::-1])+[1])),axis=2\n",
    "            )*255).astype('uint16'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# dump every second of data into a file\n",
    "timedir='time_imgs'\n",
    "start_time = np.min(rays0['ts'])\n",
    "duration = np.ceil(np.max(rays0['ts'])-np.min(rays0['ts']))\n",
    "import cv2\n",
    "\n",
    "def produce_basic_coords(rays0):\n",
    "    import cv2\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays) # extents and transform\n",
    "    px_coords = get_raster_coords(rays0, T_px_f) # pixel coordinates\n",
    "    img = np.zeros(px_extent[::-1]) #image to fill\n",
    "\n",
    "    # clip out bad rays\n",
    "    bad0=rays0['r']<=0\n",
    "    bad0|=rays0['r']>59\n",
    "    # clip out bad pixels\n",
    "    bad = np.any(px_coords<0,axis=1) + np.any(px_coords >= px_extent,axis=1) + bad0\n",
    "    px_coords = px_coords[bad==0,:]\n",
    "    return px_coords\n",
    "\n",
    "img = np.zeros(px_extent[::-1],np.uint8) #image to fill\n",
    "    \n",
    "   \n",
    "for i in range(int(duration)):\n",
    "    name = '{}.png'.format(i)\n",
    "    select=rays0['ts']>=start_time+i\n",
    "    select&= rays0['ts']<start_time+i+1\n",
    "    px_coords = produce_basic_coords(rays0[select])\n",
    "    np.maximum.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), 255)\n",
    "    print(name)\n",
    "    cv2.imwrite(name, img)\n",
    "    np.minimum.at(img, tuple(np.floor(px_coords.T)[::-1].astype('int32')), 0)\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "# BELOW HERE IS ALL EXPERIMENTAL STUFF\n",
    "# TODO Cell: Display only the points that have a closer start point than any other start point \n",
    "# i.e. the point which is when the robot was closest to that spot.\n",
    "\n",
    "\n",
    "%matplotlib widget\n",
    "#as a simple approximation, lets see if using a certain offset foward and backward in time is okay\n",
    "offset = 1000\n",
    "\n",
    "def dist(pt,pts):\n",
    "    return np.sqrt(np.sum((pt-pts)**2,axis=1))\n",
    "\n",
    "sx = rays1['sx']#use rays only from one laser\n",
    "sy = rays1['sy']\n",
    "spt= np.vstack((sx,sy)).T\n",
    "\n",
    "ex = rays1['ex']\n",
    "ey = rays1['ey']\n",
    "ept=np.vstack((ex,ey)).T\n",
    "\n",
    "#remove far rays\n",
    "rays11=rays1[dist(ept,spt)<10]\n",
    "scannum11=scannum1[dist(ept,spt)<10]\n",
    "\n",
    "\n",
    "\n",
    "#reunpack the ray that remain\n",
    "\n",
    "sx = rays11['sx']#use rays only from one laser\n",
    "sy = rays11['sy']\n",
    "spt= np.vstack((sx,sy)).T\n",
    "\n",
    "ex = rays11['ex']\n",
    "ey = rays11['ey']\n",
    "ept=np.vstack((ex,ey)).T\n",
    "\n",
    "# # check that we are closer that\n",
    "# fsx = sx[offset:]\n",
    "# fsy = sy[offset:]\n",
    "# bsx = sx[:-offset]\n",
    "# bsy = sy[:-offset]\n",
    "# cex = ex[offset//2:-offset//2]\n",
    "# cey = ex[offset//2:-offset//2]\n",
    "\n",
    "\n",
    "rsx=raysr['sx']\n",
    "rsy= raysr['sy']\n",
    "rpt= np.vstack((rsx,rsy)).T\n",
    "\n",
    "\n",
    "probes = np.arange(0,len(sx),1000) # some points where we can get the true answer\n",
    "# probes = [5025083]\n",
    "\n",
    "def dist(pt,pts):\n",
    "    return np.sqrt(np.sum((pt-pts)**2,axis=1))\n",
    "sols=[] #the ones with a good closest approach\n",
    "bsols=[] # the ones that never seem to have a good closest approach\n",
    "stats=[]\n",
    "for p in probes:\n",
    "    pex=ex[p]\n",
    "    pey=ey[p]\n",
    "    ppt=np.array([pex,pey])\n",
    "    d=dist(ppt, rpt)\n",
    "    d=dist(ppt, spt)\n",
    "    timecost=np.abs(scannum11[p]-scannum11)/300\n",
    "#     plt.clf()\n",
    "#     plt.plot(timecost)\n",
    "#     plt.plot(d)\n",
    "#     plt.plot(d+timecost)\n",
    "    d3=dist(ppt,ept)#distance to original endpoint\n",
    "\n",
    "#     break\n",
    "    closest = np.argmin(d+timecost)\n",
    "#     plt.plot(d)\n",
    "\n",
    "#     print(closest,pex,pey)\n",
    "    cpt=spt[closest]\n",
    "    d2=dist(cpt,spt)#distance to best center\n",
    "#     plt.plot(d2)\n",
    "#     d3=dist(ppt,ept)#distance to original endpoint\n",
    "#     plt.plot(d3+d2)\n",
    "    cost=d3+d2\n",
    "    remin=np.argmin(cost)\n",
    "    print(p,remin,cost[remin])\n",
    "    if cost[remin]<0.1:\n",
    "        sols.append(remin)\n",
    "    else:\n",
    "#         plt.plot(d)\n",
    "#         plt.plot(d3)\n",
    "        bsols.append(remin)\n",
    "#         break\n",
    "    stats.append(cost[remin])\n",
    "plt.plot(ex[sols],ey[sols],\"b.\")\n",
    "plt.plot(ex[bsols],ey[bsols],\"r.\")\n",
    "\n",
    "# rsx[closest],rsy[closest]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "\n",
    "%matplotlib widget\n",
    "plt.close('all')\n",
    "\n",
    "# plt.plot(rays11['ex'],rays11['ey'],'b,')\n",
    "\n",
    "# tpti = 1000000\n",
    "# tpt=np.array([rays11['ex'][tpti],rays11['ey'][tpti]])\n",
    "# stpt=np.array([rays11['sx'][tpti],rays11['sy'][tpti]])\n",
    "# # plt.plot(raysr['sx'][::2],raysr['sy'][::2],'r.-')\n",
    "# plt.plot(raysr['sx'][1::2],raysr['sy'][1::2],'k.-')\n",
    "\n",
    "# plt.plot(rays11['ex'][tpti],rays11['ey'][tpti],'r.')\n",
    "# plt.plot(rays11['sx'][tpti],rays11['sy'][tpti],'g.')\n",
    "\n",
    "# print(len(raysr),np.max(scannum11))\n",
    "# dr=dist(stpt, rpt)\n",
    "# iraysr = np.argmin(dr)\n",
    "# print(iraysr,scannum11[tpti])\n",
    "# plt.figure()\n",
    "# plt.plot(dr)\n",
    "\n",
    "# plt.figure()\n",
    "dr=dist(spt,ept)\n",
    "plt.plot(dr,'-')\n",
    "\n",
    "def advance(line, marker, mask):\n",
    "    left=line[:-1]\n",
    "    line[1:]=np.minimum(left,line[1:])\n",
    "    line[1:]=np.maximum(line[1:],mask[1:])\n",
    "#     left[left<line[1:]]left[left<line[1:]]\n",
    "    return line\n",
    "# advance(dr,dr,dr-0.1)\n",
    "line=dr.copy()\n",
    "marker=dr\n",
    "mask=dr-0.05\n",
    "for i in range(100):\n",
    "    line=advance(line, marker, mask)\n",
    "line=line[::-1]\n",
    "marker = marker[::-1]\n",
    "mask = mask[::-1]\n",
    "for i in range(100):\n",
    "    line=advance(line, marker, mask)\n",
    "line=line[::-1]\n",
    "marker = marker[::-1]\n",
    "mask = mask[::-1]\n",
    "plt.plot(line,'-')\n",
    "localmins = (line==dr)\n",
    "plt.figure()\n",
    "plt.plot(rays11['ex'],rays11['ey'],'b,')\n",
    "plt.plot(rays11['ex'][localmins],rays11['ey'][localmins],'r.')\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "plt.plot(ex[sols],ey[sols],\"b.\")\n",
    "plt.plot(ex[bsols],ey[bsols],\"r.\")\n",
    "plt.plot(ex[probes],ey[probes],\"g,\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "plt.hist(stats,500)"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}