do_rfm.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "import os\n",
    "from tqdm import tqdm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "rayfile = \"/home/anuran/repos/Vulcan/bin/rayDump.rays\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load rays\n",
    "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",
    "        ('idx',  np.uint16),\n",
    "        ('pad',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",
    "rays0 = rays0[:int(len(rays0)//1081*1081)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# split rays by sensor THIS IS A HACK RIGHT NOW!\n",
    "# .... actually, don't bother"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3e563c81158444ebaddb8ff12709c22e",
       "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": [
      "31904634\n"
     ]
    }
   ],
   "source": [
    "#Debug Cell\n",
    "%matplotlib widget\n",
    "# quickly plot the bot path and rays\n",
    "substart=0\n",
    "subend=int(2.0e9)\n",
    "substart=int(substart//1081)*1081\n",
    "subend=int(subend//1081)*1081\n",
    "plt.plot(rays0['ex'][substart:subend][::],rays0['ey'][substart:subend][::1],',')\n",
    "plt.plot(rays0['sx'][substart:subend][::997],rays0['sy'][substart:subend][::997],',')\n",
    "print(len(rays0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "#HACK use only one pass for algo debug\n",
    "# rays0 =  rays0[substart:subend]\n",
    "# rays0 = rays0[:int(len(rays0)//1081*1081)]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": []
   },
   "source": [
    "# Hyperparameter Setting Instructions\n",
    "1. CELLS_PER_M should indicate a cell width less than the max displacement of the robot between scans. \n",
    "   1. Otherwise there may be gaps between adjacent rays in the same direction.\n",
    "2. Theta width should be >= laser angular spacing, and <= min peak half-width at worst laser plane tilt\n",
    "   1. If you can't satisfy both, mount your laser better, stiffen your suspension, or get a blurrier (higher divergence) laser!\n",
    "3. Max range should be set so max_turn_rate * max_range < cell_width\n",
    "4. Currently, the max distance is hardcoded at 200 cells\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Cell width: 0.05\n",
      "fcorns\n",
      " [[      28.25       13.39]\n",
      " [      70.21       77.7 ]]\n",
      "pxc_corns\n",
      " [[     564.97     -267.7 ]\n",
      " [    1404.27    -1554.07]]\n",
      "pxc_topleft\n",
      " [     564       -1555   ]\n",
      "pxc_btmright\n",
      " [    1405        -267   ]\n",
      "pxc_extent\n",
      " [     841        1288   ]\n",
      "px_corns\n",
      " [[       0.97     1287.3 ]\n",
      " [     840.27        0.93]]\n",
      "px_topleft\n",
      " [       0           0   ]\n",
      "px_btmright\n",
      " [     841        1288   ]\n",
      "px_extent\n",
      " [     841        1288   ]\n"
     ]
    }
   ],
   "source": [
    "## Find the max extent of the map and transforms\n",
    "\n",
    "keep=(rays0['r']<10000)*(rays0['r']<10000)\n",
    "rays=rays0[keep==1]\n",
    "## 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 = 20.0\n",
    "print(\"Cell width:\", 1/CELLS_PER_M)\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",
    "keep_for_window=(rays0['r']>0)*(rays0['r']<10)\n",
    "with np.printoptions(formatter=formatter):\n",
    "    T_px_f, px_extent = get_T_px_f_with_extent(rays[keep_for_window], debug=True)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "1f83a34b704b443aa18f720d2db9011d",
       "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 0x7f2f83dbfee0>]"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# convert to [start_cell_x,start_cell_y, d_cell, th] format\n",
    "f_spt = np.vstack((rays['sx'].ravel(),rays['sy'].ravel())).T.copy()\n",
    "f_ept = np.vstack((rays['ex'].ravel(),rays['ey'].ravel())).T.copy()\n",
    "px_spt = homp(T_px_f, f_spt)\n",
    "px_ept = homp(T_px_f, f_ept)\n",
    "px_d = np.sqrt(np.sum((px_ept-px_spt)**2,axis=1))\n",
    "px_th = np.mod(np.arctan2((px_ept-px_spt)[:,1], (px_ept-px_spt)[:,0]),2*np.pi)\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",
    "# px_d[rays0['r']<0]=0\n",
    "# %matplotlib widget\n",
    "# plt.plot(rays0['th'][rays0['r']>0],px_th[rays0['r']>0],',')\n",
    "th_spacing = 2*np.pi/360/4\n",
    "\n",
    "outrays = np.vstack([px_spt.T,px_d,px_th/th_spacing])\n",
    "%matplotlib widget\n",
    "plt.plot(px_ept[:,0],px_ept[:,1],'b,')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "th_spacing = 2*np.pi/360/4\n",
    "th_bins = int(np.round(2*np.pi / th_spacing))\n",
    "w_HIT = 1\n",
    "w_MISS = 4 # Reduce to 1 if known no motion\n",
    "bias = 0\n",
    "wide_threshold = 30*np.pi/180\n",
    "W, H = 800,1200\n",
    "HIT = np.zeros((W,H,th_bins),'int16')\n",
    "MISS = np.zeros((W,H,th_bins),'int16')\n",
    "RFM = np.zeros((W,H,th_bins),'int8') # -1=TRANSPARENT, 0=UNK, 1=REFLECT\n",
    "RFM_mr = np.zeros((W,H,th_bins),'int8')\n",
    "WIDE = np.zeros((W,H,th_bins),'bool')\n",
    "NARROW = np.zeros((W,H,th_bins),'bool')\n",
    "prerender = np.zeros((W,H),'bool')\n",
    "refl_cache = np.zeros((W,H),'int16')\n",
    "trans_cache = np.zeros((W,H),'int16')\n",
    "OCC = np.zeros((W,H),'int8')\n",
    "\n",
    "def RFM_update_cell(x,y,th):\n",
    "    # updates the RFM cell and the cached values \n",
    "    old_val = RFM[x,y,th]\n",
    "    new_val = np.sign(w_HIT*HIT[x,y,th]-w_MISS*MISS[x,y,th])\n",
    "    RFM[x,y,th] = new_val\n",
    "    if old_val == -1:\n",
    "        trans_cache[x,y] -=1\n",
    "    if old_val == 1:\n",
    "        refl_cache[x,y] -=1\n",
    "    if new_val == -1:\n",
    "        trans_cache[x,y] +=1\n",
    "    if new_val == 1:\n",
    "        refl_cache[x,y] +=1\n",
    "\n",
    "def quantize(coords):\n",
    "    return np.floor(coords).astype('int')\n",
    "    \n",
    "def quantize4(coords): # 4 pt xy antialiasing samples\n",
    "    coords = np.array(coords)\n",
    "    coords4 = (coords + (-0.5,-0.5,0.0), coords + (-0.5,0.5,0.0), \n",
    "               coords + (0.5,-0.5,0.0), coords + (0.5,0.5,0.0))\n",
    "    return np.floor(coords4).astype('int')\n",
    "    \n",
    "def accumulate(rays):\n",
    "    epsl = 1.\n",
    "    # prerender lets us stop rendering a ray that would go through a HIT\n",
    "    prerender[:,:]=0\n",
    "    for ray in rays:\n",
    "        # ray has a starting point, distance before it returned, and direction\n",
    "        [x_start, y_start, d, th] = ray\n",
    "        [slope_x, slope_y] = [cos(th*th_spacing), sin(th*th_spacing)]\n",
    "    \n",
    "        xyth = quantize4((x_start + d*slope_x, y_start + d*slope_y, th))\n",
    "        prerender[xyth[:,0],xyth[:,1]]+=1\n",
    "    \n",
    "    for ray in rays:\n",
    "        # ray has a starting point, distance before it returned, and direction\n",
    "        [x_start, y_start, d, th] = ray\n",
    "        [slope_x, slope_y] = [cos(th*th_spacing), sin(th*th_spacing)]\n",
    "    \n",
    "        for r in range(0,d-epsl):\n",
    "            xyth = quantize((x_start + r*slope_x, y_start + r*slope_y, th))\n",
    "            if not prerender[xyth[0],xyth[1]]:\n",
    "                MISS[xyth[0],xyth[1],xyth[2]]+=1\n",
    "                RFM_update_cell(xyth[0],xyth[1],xyth[2])\n",
    "            else:\n",
    "                break\n",
    "        xyth = quantize4((x_start + d*slope_x, y_start + d*slope_y, th))\n",
    "        HIT[xyth[:,0],xyth[:,1],xyth[:,2]]+=1\n",
    "        for xyth_sample in xyth:\n",
    "            RFM_update_cell(xyth_sample[0], xyth_sample[1], xyth_sample[2])\n",
    "    \n",
    "        \n",
    "def classify_wide_narrow():\n",
    "    WIDE[:,:]= refl_cache > wide_threshold\n",
    "    WIDE &= refl_cache/trans_cache > wide_min_ratio\n",
    "    NARROW[:,:] = (refl_cache>0) & (WIDE==False)\n",
    "\n",
    "def remove_motion():\n",
    "    RFM_mr[NARROW,:] = RFM[NARROW,:]-3 # map [-1,0,1]->[-4,-3,-2]\n",
    "    RFM_mr[WIDE,:] = RFM[WIDE,:]\n",
    "    stack = []\n",
    "    seed_locs=np.argwhere(WIDE)\n",
    "    for x,y in seed_locs:\n",
    "        if (  NARROW[x-1,y-1] or NARROW[x-1,y] or NARROW[x-1,y+1] or\n",
    "              NARROW[x,y-1] or NARROW[x,y+1] or\n",
    "              NARROW[x+1,y-1] or NARROW[x+1,y] or NARROW[x+1,y+1]  ):\n",
    "            thetas = RFM[x,y,:]\n",
    "            for th in thetas:\n",
    "                stack.append((x,y,th))\n",
    "    while(stack):\n",
    "        x,y,th = stack.pop\n",
    "        t2d_neighbors = ((x-1,y-1),(x-1,y),(x-1,y+1),\n",
    "                        (x,y-1),  (x,y)  ,(x,y+1),\n",
    "                        (x+1,y-1),(x+1,y),(x+1,y+1))\n",
    "        for x,y in t2d_neighbors:\n",
    "            if not NARROW(x,y): continue\n",
    "            for th in (th-1, th, th+1):\n",
    "                if RFM_mr[x,y,th] == -2: # Unvisited, NARROW, REFL\n",
    "                    RFM_mr[x,y,th] = 4\n",
    "                    stack.append((x,y,th))\n",
    "    # codes: -4:NAR,TRANS -3:NAR, UNK -2:NAR,DYN \n",
    "    #        -1:WIDE,TRANS 0:WIDE,UNK  1:WIDE,STATIC\n",
    "    #         4:NAR,CON\n",
    "    RFM_mr[NARROW,:] //= 4 # remap -4,-3,-2,4 -> -1,0,0,1; deleting motion\n",
    "    # Classify occupancy: -2=FREE, -1=DYNAMIC, 0=UNK, 1=STATIC\n",
    "    OCC[:,:] = 0 \n",
    "    OCC[trans_cache>0] = -2 # seen through defaults FREE\n",
    "    OCC[NARROW] = -1 # narrow defaults DYNAMIC\n",
    "    OCC[WIDE] = 1\n",
    "    # The following line is an optimization of\n",
    "    # OCC[np.any(RFM_mr==1,axis=1)] = 1\n",
    "    OCC[NARROW] += np.any(RFM_mr[NARROW,:]==1,axis=1)*2\n",
    "    OCC[NARROW & (transcache==0)] = 1 # Default uncontested cells to STATIC\n",
    "\n",
    "def classify_reflections(rays):\n",
    "    is_reflection = []\n",
    "    epsl = 1.999\n",
    "\n",
    "    for ray in rays:\n",
    "        [x_start, y_start, d, th] = ray\n",
    "        [slope_x, slope_y] = [cos(th*th_spacing), sin(th*th_spacing)]\n",
    "    \n",
    "        for r in range(0,d-epsl):\n",
    "            xyth = quantize((x_start + r*slope_x, y_start + r*slope_y, th))\n",
    "            if OCC[xyth[0], xyth[1]]:\n",
    "                is_reflection.append(True)\n",
    "                break\n",
    "        else:\n",
    "            is_reflection.append(False)\n",
    "    return is_reflection\n",
    "\n",
    "def process_scan(rays):\n",
    "    # This is the do-everything main function\n",
    "    accumulate(rays)\n",
    "    classify_wide_narrow()\n",
    "    remove_motion()\n",
    "    # Now the RFM estimate without motion removal is in RFM\n",
    "    # and the version with the motion removed is in RFM_mr.\n",
    "    # OCC holds the estimated occupancy classification\n",
    "    \n",
    "    # classify_rays() can be used to check rays for reflection in\n",
    "    # another thread, and build a new RFM without them"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Basic RFM\n",
    "The following calculates the basic RFM, accumulating HITs and MISSes. \\\n",
    "It does not account for reflections, and doesn't link up objects (we do that later)\n",
    "\n",
    "Typically runs ~80 scans/second"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "loclip = np.array([0,0,0]).reshape(3,1)\n",
    "hiclip = (np.array(HIT.shape)-1).reshape(3,1)\n",
    "def quantize(coords):# 3xN\n",
    "    return np.clip(np.floor(coords),loclip,hiclip).astype('int')\n",
    "\n",
    "def quantize4(coords): # 4 pt xy antialiasing samples # 3xN\n",
    "    coords = np.array(coords).T  # Nx3\n",
    "    aa = np.array(((-0.5,-0.5,0.0), (-0.5,0.5,0.0), \n",
    "                  ( 0.5,-0.5,0.0), ( 0.5,0.5,0.0) )) # 4x3\n",
    "    coords4 = aa + coords[:,np.newaxis,:] # N,4,3\n",
    "    coords4= coords4.reshape(-1,3).T\n",
    "    return np.floor(np.clip(coords4,loclip,hiclip)).astype('int')\n",
    "\n",
    "def RFM_update_cell(x,y,th):\n",
    "    #TODO Write me!\n",
    "    pass\n",
    "    \n",
    "# Define the accumulate operator, that renders rays\n",
    "def accumulate(rays, notref):\n",
    "    epsl = 1.\n",
    "    # prerender lets us stop rendering a ray that would go through a HIT\n",
    "    prerender[:,:]=0\n",
    "#     for ray in rays:\n",
    "    # ray has a starting point, distance before it returned, and direction\n",
    "    [x_start, y_start, d, th] = rays\n",
    "    [slope_x, slope_y] = [np.cos(th*th_spacing), np.sin(th*th_spacing)]\n",
    "\n",
    "    xyth = quantize4((x_start[notref] + d[notref]*slope_x[notref], y_start[notref] + d[notref]*slope_y[notref], th[notref]))\n",
    "    prerender[xyth[0],xyth[1]]=1\n",
    "    \n",
    "#     for ray in rays:\n",
    "    # ray has a starting point, distance before it returned, and direction\n",
    "    [x_start, y_start, d, th] = rays\n",
    "    [slope_x, slope_y] = [np.cos(th*th_spacing), np.sin(th*th_spacing)]\n",
    "    \n",
    "    \n",
    "    \n",
    "    live=np.ones_like(d)\n",
    "    r=np.full_like(d,0)\n",
    "    for r0 in range(0,200):\n",
    "#         print(r0, end=' ')\n",
    "        r[:]=r0\n",
    "        live[r0>=d-epsl]=0\n",
    "#         print(np.sum(live))\n",
    "        r[live==0]=100000\n",
    "        xyth = quantize((x_start + r*slope_x, y_start + r*slope_y, th))\n",
    "#         if r0>1\n",
    "        live*=(prerender[xyth[0],xyth[1]]==0)\n",
    "        MISS[xyth[0],xyth[1],xyth[2]]+=1\n",
    "        RFM_update_cell(xyth[0],xyth[1],xyth[2])\n",
    "    xyth = quantize((x_start[notref] + d[notref]*slope_x[notref], y_start[notref] + d[notref]*slope_y[notref], th[notref]))\n",
    "    HIT[xyth[0],xyth[1],xyth[2]]+=1\n",
    "    RFM_update_cell(xyth[0], xyth[1], xyth[2])\n",
    "print(outrays.shape)\n",
    "arays=outrays.copy()\n",
    "# arays[0]-=2300\n",
    "# arays[1]-=1500\n",
    "ls=0\n",
    "keep=(rays0['r']<10)*(rays0['r']>0)\n",
    "CHUNK_SIZE = 1081\n",
    "for s in tqdm(range(CHUNK_SIZE,arays.shape[1]+1,CHUNK_SIZE)):\n",
    "    \n",
    "#     print(int(s/arays.shape[1]*1000),end=' ')\n",
    "    tmprays=arays[:,ls:s][:,keep[ls:s]]\n",
    "    accumulate(tmprays,np.ones_like(tmprays[0],dtype=np.bool))\n",
    "    ls=s"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Draw Figures\n",
    "The following calculates the basic RFM in bulk (Faster in Python and good for prototyping).\n",
    "Then we take a slice out though the glass to make the figure.\n",
    "\n",
    "We don't discuss it in the paper, due to lack of space, but this also shows several other visualizations good for exploring the RFM\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "445ba97142354bb3a749da2a4947e83a",
       "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": "5b7769b5d8eb43a3ba5ec6b5b41f4ea9",
       "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": "9c47fe8faedc4890b5248d5f4aef2ba4",
       "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": [
       "(295, 425)"
      ]
     },
     "execution_count": 73,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Specialized debug cell\n",
    "%matplotlib widget\n",
    "disco=HIT/(np.float32(0.1)+HIT+MISS)\n",
    "RFM=HIT*w_HIT-MISS*w_MISS\n",
    "sud=HIT+MISS\n",
    "# del disco\n",
    "del sud\n",
    "# del disco\n",
    "plt.figure(1)\n",
    "ax=plt.subplot(221)\n",
    "# plt.imshow(np.clip(np.max((HIT),axis=2).T,-3,15))\n",
    "plt.imshow(np.clip(np.max((HIT),axis=2).T,-10,10))\n",
    "plt.title('Max HIT [0,10]')\n",
    "plt.subplot(222,sharex=ax,sharey=ax)\n",
    "mean_active = (np.sum(MISS,axis=2,dtype=np.float32)\n",
    "              /\n",
    "              (np.sum(MISS>0,axis=2)+np.float32(.0001)))[:,:,np.newaxis]\n",
    "plt.imshow(np.clip(mean_active[:,:,0].T,-1,10))\n",
    "plt.title(\"Average RFM probes [0,10]\")\n",
    "plt.subplot(223,sharex=ax,sharey=ax)\n",
    "plt.imshow(np.clip(np.max(HIT/(mean_active+0.01),axis=2).T,-1,3))\n",
    "plt.title('Normalized Max HIT')\n",
    "\n",
    "plt.subplot(224,sharex=ax,sharey=ax)\n",
    "plt.imshow(np.clip(np.max(HIT/(mean_active+0.01),axis=2).T,-1,3))\n",
    "plt.title('Example RFM Slice Line')\n",
    "plt.plot([340,360],[142,142],'r')\n",
    "\n",
    "del mean_active\n",
    "# # plt.imshow(prerender.T)\n",
    "# plt.figure(2)\n",
    "# plt.plot(np.max(np.max(HIT,axis=0),axis=0))\n",
    "# plt.plot(HIT[253,212]*1000.0)\n",
    "# plt.plot(-MISS[253,212]*1000.0)\n",
    "# plt.plot(HIT[235,418]*1000.0) #down\n",
    "# plt.plot(HIT[287,539]*1000.0) #left\n",
    "# plt.plot(HIT[290,493]*1000.0) #left\n",
    "# plt.plot(HIT[286,552]*1000.0) #left\n",
    "# plt.plot(HIT[227,552]*1000.0) #up\n",
    "# # plt.plot(rays[0],rays[1],',')\n",
    "# # x = plt.ginput(3)\n",
    "\n",
    "\n",
    "# plt.plot(np.sum(np.sum(HIT,axis=0),axis=0))\n",
    "\n",
    "\n",
    "plt.figure(4)\n",
    "plt.title(\"H Shapes Figure, Option 1\")\n",
    "# Need to take max of neighboring slices due to aliasing\n",
    "plt.imshow(np.clip(np.max(RFM[:,850:852,:],axis=1).T,0,2),aspect='auto') \n",
    "plt.xlim(320,540)\n",
    "# plt.ylim(0,700)\n",
    "# plt.imshow(np.clip(np.sum(disco[:,:,:],axis=0).T,0,2))\n",
    "plt.xlabel('x [1 unit = 5cm]')\n",
    "plt.ylabel('theta [720 units = 180deg]')\n",
    "\n",
    "plt.figure(5)\n",
    "plt.title(\"H Shapes Figure,Option 2\")\n",
    "plt.imshow(np.clip(np.max(RFM[:,142:143,:],axis=1).T,-2,2),aspect='auto',cmap='bwr_r')\n",
    "plt.xlabel('x [1 unit = 5cm]')\n",
    "plt.ylabel('theta [720 units = 180deg]')\n",
    "plt.xlim(295,425)\n",
    "\n",
    "\n",
    "# plt.figure(5)\n",
    "# plt.imshow(np.clip(disco[:,829,:].T,0,10))\n",
    "# plt.figure(6)\n",
    "# plt.imshow(np.clip(HIT[:,830,:].T,0,10))\n",
    "# plt.figure(7)\n",
    "# plt.imshow(np.clip(HIT[:,831,:].T,0,10))\n",
    "# plt.figure(8)\n",
    "# plt.imshow(np.clip(HIT[:,832,:].T,0,10))\n",
    "# plt.figure(9)\n",
    "# plt.imshow(np.clip(HIT[:,833,:].T,0,10))\n",
    "# plt.figure(11)\n",
    "# plt.imshow(np.clip(HIT[:,851,:].T,0,10))\n",
    "# plt.imshow(np.clip(HIT[:,:,736-360].T,0,10))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Figure captions\n",
    "*Option 1:* Glass panes are easily detectable in the Reflectance Field Map by their distinctive H shape. \n",
    "Shown here is a slice through the RFM along a wall made of several panes of glass, producing the repeated H patterns seen here.\n",
    "The arms of the H are formed by the edges of the glass or its frame which scatter light in many angles,\n",
    "and the cross bar is caused by the smooth face of the glass reflecting mostly perpendicularly.\n",
    "Note that despite the wide variation in visibility between different pieces of glass, and the gaps \n",
    "in some of the H arms due to occlusions, this wall is reliably detectable at 91deg = 364 units \n",
    "\n",
    "[TODO: figure out how to make Figure scale show degrees]\n",
    "\n",
    "Unlike intensity or multiecho methods, our's does not require a bright specular peak, or an object behind to be detected.\n",
    "And unlike Foster et al 2013[Cite], the gaps and secondary detection angles shown here do not confuse our detector.\n",
    "Even an occluder directly blocking the view of the glass only causes the loss of the blocked part.\n",
    "\n",
    "*Option 2:*: Similar text to Option 1, but mention Blue is reflective, Red is transparent, and white is areas of the \n",
    "reflectance field not probed, due to occlusions or the sensor path. \\\n",
    "Option two is made available to demostrate ability to show alternate color scheme, an alternate target with more regular \n",
    "(but less interesting!) pieces of glass, "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.,  0.,  0., ...,  0.,  0., -0.],\n",
       "       [ 0.,  0.,  0., ...,  0.,  0.,  0.],\n",
       "       [ 0.,  0.,  0., ...,  0.,  0.,  0.],\n",
       "       ...,\n",
       "       [ 0.,  0.,  0., ...,  0.,  0.,  0.],\n",
       "       [ 0.,  0.,  0., ...,  0.,  0.,  0.],\n",
       "       [-0.,  0.,  0., ...,  0.,  0., -0.]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Detect Reflections\n",
    "REF_BLOCK_THRESH = 0.3\n",
    "def classify_cells_bulk():\n",
    "    # Justification: mean_active is an approximation of how many times we should have seen a cell.\n",
    "    # It is generated by looking at the mean of HIT+MISS for cells that have ever had either, meaned along the theta axis\n",
    "    # Note that if there is a huge variation in the HIT+MISS sum, this could be inaccurate(i.e. if we take different paths \n",
    "    # near the point.)\n",
    "    # The score is how many times we saw/expected attempts. We take the max score over angles to represent the most opaque\n",
    "    # direction. Scores significantly above 1.0 indicate a bad condition\n",
    "    #\n",
    "    fullcount=MISS+HIT\n",
    "    mean_active = (np.sum(fullcount,axis=2,dtype=np.float32)\n",
    "                  /\n",
    "                  (np.sum(fullcount>0,axis=2)+np.float32(.0001)))[:,:,np.newaxis]\n",
    "    score = np.max(HIT/(mean_active+np.float32(.0001)),axis=2)\n",
    "    refl_cache[1:-1,1:-1]=(score>REF_BLOCK_THRESH)[1:-1,1:-1]\n",
    "    return score\n",
    "classify_cells_bulk()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "4a72d6103f014bd0aa4be01cc9669390",
       "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": [
       "Text(0.5, 1.0, 'Cells that stop rays for reflectance calculations')"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "plt.imshow(refl_cache.T)\n",
    "plt.title('Cells that stop rays for reflectance calculations')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Classify Reflection Rays\n",
    "Mark the depths where rays try to go through another object. \n",
    "PEN_DEPTH controls this. We set to 30cm to deal with the stupid 20cm short detections that seem to plague our laser.\n",
    "\n",
    "The basic idea is that all rays render MISSes, but only rays that travel less than PEN_DEPTH from the surface can render HITs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 200/200 [00:21<00:00,  9.40it/s]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "classified\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n"
     ]
    }
   ],
   "source": [
    "# classify reflectionness of a ray and clip to a given penetration depth\n",
    "PEN_DEPTH = np.ceil(0.3*CELLS_PER_M)\n",
    "def classify(rays):\n",
    "    pen_depth = PEN_DEPTH\n",
    "    # prerender lets us stop rendering a ray that would go through a HIT\n",
    "    prerender[:,:]=0\n",
    "    \n",
    "#     for ray in rays:\n",
    "    # ray has a starting point, distance before it returned, and direction\n",
    "    [x_start, y_start, d, th] = rays\n",
    "    [slope_x, slope_y] = [np.cos(th*th_spacing), np.sin(th*th_spacing)]\n",
    "    \n",
    "    \n",
    "    notref=np.ones_like(d,dtype=bool)\n",
    "    r=np.full_like(d,0)\n",
    "    for r0 in tqdm(range(0,200)):\n",
    "#         print(r0,end=', ')\n",
    "        r[:]=r0\n",
    "        xyth = quantize((x_start + r*slope_x, y_start + r*slope_y, th))\n",
    "#         if r0>1\n",
    "        stopped=(refl_cache[xyth[0],xyth[1]]==1)\n",
    "        notrefish = r0 > d[stopped]-pen_depth\n",
    "        notref[stopped]*=notrefish\n",
    "#         print(r0+pen_depth)\n",
    "        d[stopped]=np.minimum(d[stopped],r0+pen_depth)\n",
    "    rays = np.vstack([x_start, y_start, d, th])\n",
    "    return rays, notref\n",
    "\n",
    "arays=outrays.copy()\n",
    "\n",
    "arays,notref = classify(arays)\n",
    "print(\"classified\")\n",
    "\n",
    "#\n",
    "# The returned rays now have distances reflecting how far to render them. \n",
    "# If the distance is less than the original, we want to not render the HIT when we rerender"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "Hstore=[]\n",
    "Mstore=[]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Show Stabiliation of Reflection Removal:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3d1f9b9d741a436d8a8391bacca51b2c",
       "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": [
      "100%|██████████| 200/200 [00:21<00:00,  9.22it/s]\n",
      "100%|██████████| 2775/2775 [00:35<00:00, 78.30it/s]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "0e082d5b51254d20a337f7e249af9811",
       "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": [
      "100%|██████████| 200/200 [00:21<00:00,  9.27it/s]\n",
      "100%|██████████| 2775/2775 [00:36<00:00, 76.01it/s]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "375d59fe56b74cf3826d003057156583",
       "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": [
      "100%|██████████| 200/200 [00:22<00:00,  9.02it/s]\n",
      "100%|██████████| 2775/2775 [00:35<00:00, 77.62it/s]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "56df22226a3b49c9aed2f0167a5154d3",
       "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": [
      "100%|██████████| 200/200 [00:20<00:00,  9.58it/s]\n",
      "100%|██████████| 2775/2775 [00:35<00:00, 77.56it/s]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8f1ce95d571d46379d510607d608b811",
       "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": [
      "100%|██████████| 200/200 [00:22<00:00,  9.05it/s]\n",
      "100%|██████████| 2775/2775 [00:35<00:00, 77.92it/s]\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "55599bdfafd84f63bcb6beb888a8fa53",
       "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",
    "from tqdm import tqdm\n",
    "plt.imshow(refl_cache.T)\n",
    "\n",
    "for refitr in range(5):\n",
    "    Hstore.append(HIT)\n",
    "    Mstore.append(MISS)\n",
    "    \n",
    "    classify_cells_bulk()\n",
    "    \n",
    "    arays=outrays.copy()\n",
    "    arays,notref = classify(arays)\n",
    "\n",
    "    HIT = np.zeros((W,H,th_bins),'int16')\n",
    "    MISS = np.zeros((W,H,th_bins),'int16')\n",
    "    ls=0\n",
    "    keep=(rays0['r']<30)*(rays0['r']>0)\n",
    "    CHUNK_SIZE = 1081\n",
    "\n",
    "    for s in tqdm(range(CHUNK_SIZE,arays.shape[1]+1,CHUNK_SIZE)):\n",
    "        tmprays=arays[:,ls:s][:,keep[ls:s]]\n",
    "        nrtemp=notref[ls:s][keep[ls:s]]\n",
    "        accumulate(tmprays, nrtemp)\n",
    "        ls=s\n",
    "        \n",
    "    plt.figure()\n",
    "    plt.imshow(refl_cache.T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 84,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "2f3e6147d83446df9795133d9af5298b",
       "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": "f84818fc8d5241eea98bc7581dffcd09",
       "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": [
       "<Figure size 640x480 with 0 Axes>"
      ]
     },
     "execution_count": 84,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Visualize the 2d occupancy score\n",
    "%matplotlib widget\n",
    "score=classify_cells_bulk()\n",
    "plt.imshow(np.clip(score,-5,5).T)\n",
    "plt.figure()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Visualize all nearest points\n",
    "%matplotlib widget\n",
    "\n",
    "r=rays0['r'].copy()\n",
    "r=r.reshape(-1,1081)\n",
    "r[r<=0]=100\n",
    "m=np.argmin(r,axis=1)\n",
    "\n",
    "r0=rays0.reshape(-1,1081)\n",
    "print(m.shape)\n",
    "mr=np.take_along_axis(r0,m[:,np.newaxis],axis=1)\n",
    "print(mr)\n",
    "plt.plot(mr['ex'],mr['ey'],',')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[ True  True  True ...  True  True  True]\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 277/277 [00:19<00:00, 14.11it/s]\n"
     ]
    }
   ],
   "source": [
    "HIT = np.zeros((W,H,th_bins),'int16')\n",
    "MISS = np.zeros((W,H,th_bins),'int16')\n",
    "ls=0\n",
    "keep=(rays0['r']<30)*(rays0['r']>0)\n",
    "arays=outrays.copy()\n",
    "\n",
    "r=arays[2]\n",
    "r=r.reshape(-1,1081*3)\n",
    "m=np.min(r,axis=1)\n",
    "r*=0\n",
    "r[:]=m[:,np.newaxis]\n",
    "\n",
    "CHUNK_SIZE = 1081*10\n",
    "print(notref)\n",
    "for s in tqdm(range(CHUNK_SIZE,arays.shape[1]+1,CHUNK_SIZE)):\n",
    "    tmprays=arays[:,ls:s][:,keep[ls:s]]\n",
    "    nrtemp=np.zeros(tmprays.shape[1],np.bool)\n",
    "    accumulate(tmprays, nrtemp)\n",
    "    ls=s\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ff8b1c7a021447759dacd297f1a7d73b",
       "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 0x7fea3c09bb80>"
      ]
     },
     "execution_count": 65,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "plt.imshow(np.clip(np.max((MISS),axis=2).T,-1,1)*.5+np.clip(np.any((Hstore[-1]>Mstore[-1]),axis=2).T,-1,1))\n"
   ]
  }
 ],
 "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
}