do_rfm_v2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "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": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# rayfile = \"/home/anuran/repos/Vulcan/bin/rayDump.rays\"\n",
    "rayfile = \"/bbb3_glass_eval_111716.rays\"\n",
    "raypath = os.getcwd()+rayfile\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "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",
    "        ('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(raypath, 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": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# #Debug Cell\n",
    "# %matplotlib widget\n",
    "# # quickly plot the bot path and rays\n",
    "# substart=int(8.0e6)\n",
    "# subend=int(11.0e6)\n",
    "# substart=int(substart//1081)*1081\n",
    "# subend=int(subend//1081)*1081\n",
    "# rayss = rays0[substart:subend]\n",
    "# rayss = rayss[np.abs(rayss['r'])<50]\n",
    "# plt.plot(rayss['ex'][::],rayss['ey'][::1],',')\n",
    "# plt.plot(rayss['sx'][::997],rayss['sy'][::997],',')\n",
    "# del rayss\n",
    "\n",
    "# print(\"Total starting rays:\", len(rays0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# #HACK use only one pass for algo debug\n",
    "# rays0 =  rays0[substart:subend]\n",
    "# rays0 = rays0[:int(len(rays0)//1081*1081)]\n",
    "\n",
    "# # HACK: pull out the rays for Fig 1\n",
    "# ok=(rays0['ts']>rays0['ts'][0]+208) & (rays0['ts']<rays0['ts'][0]+225)\n",
    "# rays0=rays0[ok]\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": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Cell width: 0.05\n",
      "fcorns\n",
      " [[      28.71       12.34]\n",
      " [      70.15       76.33]]\n",
      "pxc_corns\n",
      " [[     574.11     -246.81]\n",
      " [    1402.93    -1526.6 ]]\n",
      "pxc_topleft\n",
      " [     574       -1527   ]\n",
      "pxc_btmright\n",
      " [    1403        -246   ]\n",
      "pxc_extent\n",
      " [     829        1281   ]\n",
      "px_corns\n",
      " [[       0.11     1280.19]\n",
      " [     828.93        0.4 ]]\n",
      "px_topleft\n",
      " [       0           0   ]\n",
      "px_btmright\n",
      " [     829        1281   ]\n",
      "px_extent\n",
      " [     829        1281   ]\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",
    "\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)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "e83a8f77731c4135b07d7958b37cb823",
       "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 0x7f70124f5640>]"
      ]
     },
     "execution_count": 8,
     "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",
    "\n",
    "th_spacing = 2*np.pi/360/4\n",
    "\n",
    "outrays = np.vstack([px_spt.T,px_d,px_th/th_spacing])\n",
    "notref = np.ones_like(outrays[0], dtype=bool)\n",
    "\n",
    "%matplotlib widget\n",
    "plt.plot(px_ept[:,0],px_ept[:,1],'b,')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "th_spacing = 2*np.pi/360/4 # 1/4 degree\n",
    "th_bins = int(np.round(2*np.pi / th_spacing)) \n",
    "W, H = 800,1200\n",
    "\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 / th_spacing\n",
    "epsl = 1.\n",
    "\n",
    "# Derived constants\n",
    "# REF_BLOCK_THRESH = w_MISS/(w_MISS+w_HIT) # the theoretical way. Fraction of hits to total that means we have a reflective voxel\n",
    "REF_BLOCK_THRESH = 0.3 # where does 0.3 come from? Don't know. Tested it somehow\n",
    "\n",
    "\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",
    "vox_refl = np.zeros((W,H,th_bins),'float32')  \n",
    "CLASSIFIED_RFM = np.zeros((W,H,th_bins),'bool')  \n",
    "countvis = np.zeros((W,H),'uint16')\n",
    "counttrans = np.zeros((W,H),'uint16')\n",
    "countratio = np.zeros((W,H),'float32')\n",
    "\n",
    "\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",
    "    # 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": 10,
   "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",
    "    \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])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The basic RFM doer. \n",
    "def doRFM():\n",
    "    print(\"Building RFM\")\n",
    "    MISS[...] = 0\n",
    "    HIT[...] = 0\n",
    "    arays = outrays.copy()\n",
    "\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",
    "            tmprays=arays[:,ls:s][:,keep[ls:s]]\n",
    "            nrtemp=notref[ls:s][keep[ls:s]]\n",
    "            accumulate(tmprays, nrtemp)\n",
    "            ls=s\n",
    "    plt.imshow(np.sum(MISS,axis=2))\n",
    "    assert np.max(MISS)>0\n",
    "    vox_refl[...] = HIT/(np.float32(0.1)+HIT+MISS) # classify \n",
    "    CLASSIFIED_RFM[...] = vox_refl > REF_BLOCK_THRESH\n",
    "    countvis[...] = np.sum(CLASSIFIED_RFM, axis=2).astype(np.uint16)\n",
    "    counttrans[...] = np.sum((CLASSIFIED_RFM==0)&(MISS>0), axis=2).astype(np.uint16)\n",
    "    countratio[...] = np.float32(1.0*countvis/(countvis+counttrans+0.0000000001))\n",
    "    refl_cache[...] = (countratio > 0.5) | (countvis > 12)  # More reflective than not or a suspiciously wide range of sightings"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Building RFM\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 21901/21901 [06:37<00:00, 55.14it/s]\n"
     ]
    }
   ],
   "source": [
    "doRFM()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "# fig = plt.figure()\n",
    "plt.close('all')\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "jp-MarkdownHeadingCollapsed": true,
    "tags": []
   },
   "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": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# # Specialized debug cell\n",
    "# %matplotlib widget\n",
    "# vox_refl=HIT/(np.float32(0.1)+HIT+MISS)\n",
    "# RFM=HIT*w_HIT-MISS*w_MISS\n",
    "# sud=HIT+MISS\n",
    "# # del vox_refl\n",
    "# del sud\n",
    "# # del vox_refl\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",
    "\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.sum(HIT[:,845:855,:],axis=1).T,0,2),aspect='auto') \n",
    "# plt.xlim(320,540)\n",
    "# # plt.ylim(0,700)\n",
    "# # plt.imshow(np.clip(np.sum(vox_refl[:,:,:],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",
    "\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",
    "[TODO: figure out how to make Figure scale show degrees] \\\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": 15,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "# Classify reflective/transparent per voxel\n",
    "# Dead, but kept for interest. For some reason this was considered better than the ratio thing at some point\n",
    "# Now I rememeber. Its because glass has a low visibility ratio, but a high ratio at a cell level\n",
    "# but it may have few misses, so being > a randomly hit cell s \n",
    "# basically a cell with 1 hit and no misses on an unseen location is better than 1 hit and no miss on a heavily seen locaiton \n",
    "\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": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "6183e73367dc46879b34aa77382f593f",
       "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": 16,
     "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": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# classify reflectionness of a ray and clip to a given penetration depth\n",
    "PEN_DEPTH = np.ceil(0.101*CELLS_PER_M)\n",
    "def classify(rays, pen_depth=None):\n",
    "    rays=rays.copy()\n",
    "    if pen_depth is None:\n",
    "        pen_depth = PEN_DEPTH\n",
    "    HACK_FACTOR = 2 # ensure that detected reflections still count as seen through for this distance\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",
    "        \n",
    "#         if r0>1\n",
    "        stopped=(refl_cache[xyth[0],xyth[1]]!=0)\n",
    "        notrefish = r0 > d[stopped]-pen_depth\n",
    "        notref[stopped] *= notrefish\n",
    "#         print(r0+pen_depth)\n",
    "       \n",
    "        d[stopped]=np.minimum(d[stopped],r0+pen_depth+HACK_FACTOR) \n",
    "        \n",
    "    rays = np.vstack([x_start, y_start, d, th])\n",
    "    return rays, notref\n",
    "\n",
    "# Uncomment the following to classify right away\n",
    "# arays=outrays.copy()\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\n",
    "# \"notref\" keeps track of which rays should render their endpoint\n",
    "\n",
    "# This operation is ~1/3 the work of the normal RFM render, but I haven't optimized it, so it \n",
    "# runs much slower when the ray list is too big fo the cache"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Hstore=[]\n",
    "# Mstore=[]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Show Stabiliation of Reflection Removal:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %matplotlib widget\n",
    "\n",
    "# from tqdm import tqdm\n",
    "# plt.imshow(refl_cache.T)\n",
    "\n",
    "# for refitr in range(2):\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": 20,
   "metadata": {},
   "outputs": [],
   "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.imshow((score>0.0).T)\n",
    "\n",
    "# plt.figure()\n",
    "# plt.imshow(np.clip(np.max(HIT,axis=2),0,10).T)\n",
    "# print(np.sum(HIT>0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "# Utility that unions two sets of labelled data\n",
    "def unify_labels(rfmcomps, rfmcomps2):\n",
    "    # unify labels is mostly a wrapper around fuse, but with the optimization that we only try to \n",
    "    # fuse nonzero labels, since we know they're the same\n",
    "    def fuse(A,B):\n",
    "        # Assumes A and B are label matricies with different labels and we need to fuse them\n",
    "        # Any repeated label between the two corresponds\n",
    "        # Assumes that the labels go from 0 to max(max(A),max(B))\n",
    "        # Also assumes all labels in A are <= to the ones in B\n",
    "        assert(np.all(A<=B))\n",
    "  \n",
    "        lookup = np.arange(np.maximum(np.max(A),np.max(B))+1, dtype=np.int32)\n",
    "        np.minimum.at(lookup,B,A) # Bs now lookup the lowest A they connect to, As loopback\n",
    "\n",
    "        B=lookup[B] #Now all Bs are <= A\n",
    "\n",
    "        np.minimum.at(lookup,A,B)\n",
    "        #As lookup the smallest B they connect to, Bs lookup the smallest A or better\n",
    "        B=lookup[lookup[B]] \n",
    "\n",
    "        extr=(A!=B) \n",
    "        remA=A[extr]\n",
    "        remB=B[extr]\n",
    "        if len(remA)==0:\n",
    "            \n",
    "            return B\n",
    "        recur = fuse(remB,remA)\n",
    "        B[extr] = recur\n",
    "        return B\n",
    "    \n",
    "    nz=fuse(rfmcomps[rfmcomps!=0].ravel(),rfmcomps2[rfmcomps2!=0].ravel())\n",
    "    rfmcomps[rfmcomps!=0]=nz\n",
    "    return rfmcomps\n",
    "\n",
    "# fuse(np.array([1,2,2,3,3,4,4]),np.array([5,6,7,7,8,8,9]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "# This cell produces a label array rfmcomps, that labels everything that is connected in RFM space, accounting for the wraparound of theta\n",
    "# It works by labelling connected components, rolling Pi around, relabelling, and then merging the resulting label set\n",
    "\n",
    "from scipy.ndimage import label\n",
    "from scipy.ndimage import generate_binary_structure\n",
    "# help(generate_binary_structure)\n",
    "\n",
    "%matplotlib widget\n",
    "vox_refl[...] = HIT/(np.float32(0.1)+HIT+MISS)\n",
    "structure=generate_binary_structure(3,3)\n",
    "rfmcomps, count = label(vox_refl>0.3, structure)\n",
    "\n",
    "roll_vox_refl=np.roll(vox_refl, int(vox_refl.shape[2]//2), axis=2)\n",
    "rfmcomps2, count = label(roll_vox_refl>0.3, structure)\n",
    "del roll_vox_refl\n",
    "offset=np.max(rfmcomps)+1\n",
    "rfmcomps2=np.roll(rfmcomps2, -int(rfmcomps2.shape[2]//2), axis=2)+offset\n",
    "rfmcomps2[rfmcomps2==offset]=0\n",
    "rfmcomps = unify_labels(rfmcomps, rfmcomps2)\n",
    "del rfmcomps2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "# # Visualize the connected components in RFM space\n",
    "# import numpy as np\n",
    "# # to_show=np.random.rand(3,100)\n",
    "# to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "# per=np.random.permutation(int(np.max(rfmcomps))+1)\n",
    "# %gui qt\n",
    "\n",
    "# import mayavi.mlab\n",
    "# mayavi.mlab.close(all=True)\n",
    "\n",
    "# mayavi.mlab.points3d(to_show[0], to_show[1], to_show[2],per[rfmcomps[to_show].astype(int)], colormap='spectral', mode = 'points')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "                      vox_refl:  5.1 GiB\n",
      "                      rfmcomps:  5.1 GiB\n",
      "                           HIT:  2.6 GiB\n",
      "                          MISS:  2.6 GiB\n",
      "                           RFM:  1.3 GiB\n",
      "                        RFM_mr:  1.3 GiB\n",
      "                          WIDE:  1.3 GiB\n",
      "                        NARROW:  1.3 GiB\n",
      "                CLASSIFIED_RFM:  1.3 GiB\n",
      "                          rays: 903.1 MiB\n",
      "                       outrays: 722.5 MiB\n",
      "                        px_spt: 361.3 MiB\n",
      "                        px_ept: 361.3 MiB\n",
      "                         f_spt: 180.6 MiB\n",
      "                         f_ept: 180.6 MiB\n",
      "                          px_d: 180.6 MiB\n",
      "                         px_th: 180.6 MiB\n",
      "                          keep: 22.6 MiB\n",
      "               keep_for_window: 22.6 MiB\n",
      "                        notref: 22.6 MiB\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "def sizeof_fmt(num, suffix='B'):\n",
    "    ''' by Fred Cirera,  https://stackoverflow.com/a/1094933/1870254, modified'''\n",
    "    for unit in ['','Ki','Mi','Gi','Ti','Pi','Ei','Zi']:\n",
    "        if abs(num) < 1024.0:\n",
    "            return \"%3.1f %s%s\" % (num, unit, suffix)\n",
    "        num /= 1024.0\n",
    "    return \"%.1f %s%s\" % (num, 'Yi', suffix)\n",
    "\n",
    "for name, size in sorted(((name, sys.getsizeof(value)) for name, value in locals().items()),\n",
    "                         key= lambda x: -x[1])[:20]:\n",
    "    print(\"{:>30}: {:>8}\".format(name, sizeof_fmt(size)))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "cc9cfe93343049a98b2c29ee5bd5754a",
       "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": [
    "# This cell removes motion from RFM, given a label array for connected components\n",
    "HIGHLY_VISIBLE = (countvis>wide_threshold)\n",
    "selection_color = np.max(rfmcomps)+1\n",
    "\n",
    "%matplotlib widget\n",
    "plt.imshow(HIGHLY_VISIBLE.T)\n",
    "\n",
    "# Build an array that has the selection_color on highly visible items, and the old color on everythng else\n",
    "rfmcomps2 = (rfmcomps > 0)*HIGHLY_VISIBLE[:,:,np.newaxis]*selection_color\n",
    "rfmcomps2[rfmcomps2==0]=rfmcomps[rfmcomps2==0]\n",
    "\n",
    "# # Debug: check the selelection array\n",
    "# to_show=np.nonzero(rfmcomps2!=rfmcomps2[-3,-3,-3])\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "# %gui qt\n",
    "# import mayavi.mlab\n",
    "# mayavi.mlab.points3d(to_show[0], to_show[1], to_show[2],per[rfmcomps2[to_show].astype(int)], colormap='spectral',mode= 'point')\n",
    "\n",
    "# Flood from the seeds\n",
    "rfmcomps = unify_labels(rfmcomps, rfmcomps2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(800, 1200, 1440)\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8e7891626d4f4b22bd65d23f12d81a59",
       "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 0x7f6f971956a0>"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Visualize after flood fill\n",
    "%matplotlib widget\n",
    "print(rfmcomps.shape)\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "selected = np.sum(rfmcomps==newselcolor,axis=2)\n",
    "plt.imshow(np.clip(selected.T,0,15))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3, 3745371)\n",
      "(3, 149686)\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<mayavi.modules.glyph.Glyph at 0x7f6a9e17b810>"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "%gui qt\n",
    "\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "\n",
    "import mayavi.mlab\n",
    "to_show=np.array(to_show).reshape(3,-1)\n",
    "print(to_show.shape)\n",
    "to_show=to_show[:,to_show[1]>=106]\n",
    "to_show=to_show[:,to_show[1]<=121]\n",
    "to_show=to_show[:,to_show[0]>=218]\n",
    "to_show=to_show[:,to_show[0]<=475]\n",
    "to_show=to_show[:,rfmcomps[tuple(to_show)]==newselcolor]\n",
    "\n",
    "print(to_show.shape)\n",
    "# to_show=to_show[:,to_show[1]>106]\n",
    "color = np.sum(vox_refl>0.3,axis=2)[tuple(to_show[:2])]\n",
    "# color=score[tuple(to_show[:2])]\n",
    "\n",
    "# color=per[rfmcomps[tuple(to_show)].astype(int)]\n",
    "mayavi.mlab.close(all=True)\n",
    "mayavi.mlab.points3d(to_show[0], to_show[1], to_show[2], color, colormap='spectral', mode= 'point')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1.090575\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "331d119b681d4f2da0027143afd80a24",
       "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, 'Cells seen but never missed. Keep by default:')"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Show cells that are default kept\n",
    "%matplotlib widget\n",
    "\n",
    "nevermissed = ((np.sum(HIT,axis=2)>0)&(np.sum(MISS,axis=2)==0))\n",
    "my_dpi=100\n",
    "sc=1.*1.11*.9825\n",
    "print(sc)\n",
    "plt.figure(figsize=(nevermissed.shape[0]/my_dpi*sc, nevermissed.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "# nevermissed[:,1::2]=1\n",
    "plt.imshow(nevermissed.T)\n",
    "plt.tight_layout()\n",
    "\n",
    "plt.title(\"Cells seen but never missed. Keep by default:\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f733356569af49ad968d0d22d274bcd9",
       "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 0x7f6a9a4abee0>"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "allkept = 0.5*nevermissed+selected\n",
    "plt.imshow(allkept.T>0)\n",
    "# plt.plot(notref,'b,')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Now we have a nice view. \n",
    "# Time to remove reflections and do it all over again"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Hstore=[HIT]\n",
    "# Mstore=[MISS]\n",
    "refl_cache[...] = (allkept!=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 200/200 [02:30<00:00,  1.33it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Building RFM\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 21901/21901 [04:53<00:00, 74.66it/s]\n"
     ]
    }
   ],
   "source": [
    "keeprays=(rays0['r']<10)*(rays0['r']>0)\n",
    "\n",
    "arays,notref = classify(outrays)\n",
    "doRFM()\n",
    "#countvis[...] \n",
    "#counttrans[...] \n",
    "#countratio[...] \n",
    "#refl_cache[...]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "225f0aadba91400ab0bc585fe105f9c6",
       "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": [
    "from scipy.ndimage import label\n",
    "from scipy.ndimage import generate_binary_structure\n",
    "\n",
    "vox_refl=HIT/(np.float32(0.1)+HIT+MISS)\n",
    "structure=generate_binary_structure(3,3)\n",
    "rfmcomps, count = label(vox_refl>0.3, structure)\n",
    "\n",
    "roll_vox_refl=np.roll(vox_refl, int(vox_refl.shape[2]//2), axis=2)\n",
    "rfmcomps2, count = label(roll_vox_refl>0.3, structure)\n",
    "del roll_vox_refl\n",
    "offset=np.max(rfmcomps)+1\n",
    "rfmcomps2=np.roll(rfmcomps2, -int(rfmcomps2.shape[2]//2), axis=2)+offset\n",
    "rfmcomps2[rfmcomps2==offset]=0\n",
    "rfmcomps = unify_labels(rfmcomps, rfmcomps2)\n",
    "del rfmcomps2\n",
    "\n",
    "# This cell removes motion from RFM, given a label array for connected components\n",
    "wide_threshold = 1/12 * HIT.shape[2]\n",
    "CLASSIFIED_RFM = vox_refl>REF_BLOCK_THRESH\n",
    "HIGHLY_VISIBLE = (np.sum(CLASSIFIED_RFM, axis=2)>wide_threshold)\n",
    "selection_color = np.max(rfmcomps)+1\n",
    "\n",
    "%matplotlib widget\n",
    "plt.imshow(HIGHLY_VISIBLE.T)\n",
    "plt.title('Highly Visible Locations')\n",
    "\n",
    "# Build an array that has the selection_color on highly visible items, and the old color on everythng else\n",
    "rfmcomps2 = (rfmcomps > 0)*HIGHLY_VISIBLE[:,:,np.newaxis]*selection_color\n",
    "rfmcomps2[rfmcomps2==0]=rfmcomps[rfmcomps2==0]\n",
    "\n",
    "# Flood from the seeds\n",
    "rfmcomps = unify_labels(rfmcomps, rfmcomps2)\n",
    "\n",
    "# # Debug\n",
    "# to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "# %gui qt\n",
    "# import mayavi.mlab\n",
    "# mayavi.mlab.close(all=True)\n",
    "# mayavi.mlab.points3d(to_show[0], to_show[1], to_show[2],per[rfmcomps[to_show].astype(int)], colormap='spectral', mode= 'point')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "# Show cells that are default kept\n",
    "%matplotlib widget\n",
    "\n",
    "nevermissed = ((np.sum(HIT,axis=2)>0)&(np.sum(MISS,axis=2)==0))\n",
    "my_dpi=100\n",
    "sc=1.*1.11*.9825\n",
    "print(sc)\n",
    "plt.figure(figsize=(nevermissed.shape[0]/my_dpi*sc, nevermissed.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "plt.imshow(nevermissed.T)\n",
    "plt.tight_layout()\n",
    "\n",
    "plt.title(\"Cells seen but never missed. Keep by default:\")\n",
    "\n",
    "# Visualize after flood fill\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "selected = np.sum(rfmcomps==newselcolor,axis=2)\n",
    "\n",
    "plt.figure(figsize=(selected.shape[0]/my_dpi*sc, selected.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "plt.imshow(np.clip(selected.T,0,15))\n",
    "\n",
    "# Everything We Keep\n",
    "plt.figure(figsize=(selected.shape[0]/my_dpi*sc, selected.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "allkept = 0.5*nevermissed+selected\n",
    "plt.imshow(allkept.T)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Build Ratio Metric for Occupancy \n",
    "countvis = np.sum(CLASSIFIED_RFM, axis=2).astype(np.uint16)\n",
    "counttrans = np.sum((CLASSIFIED_RFM==0)&(MISS>0), axis=2).astype(np.uint16)\n",
    "countratio = np.float32(1.0*countvis/(countvis+counttrans+0.0000000001))\n",
    "\n",
    "markedrfc = (rfmcomps>0)*countratio[:,:,np.newaxis]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Saving to: 1479396473584265_1479396811672591.info\n",
      "\n",
      "# Transform between map and pixels (opengl flooring convention):\n",
      "T_px_f= [[20.0, 0.0, -574.0], [0.0, -20.0, 1527.0], [0.0, 0.0, 1.0]]\n",
      "# Image info\n",
      "W,H,th_bins= (800, 1200, 1440)\n",
      "# Which rays were used:\n",
      "start_time= 1479396473.584265\n",
      "end_time= 1479396811.672591\n",
      "# Base dataset:\n",
      "rayfile= /bbb3_glass_eval_111716.rays\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "<ipython-input-35-7795d00dc3c7>:37: RuntimeWarning: invalid value encountered in divide\n",
      "  cv2.imwrite(\"countratio.png\" , (countvis*1.0/(counttrans+countvis)*255).astype('uint8'))\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "infofile = str(round(rays0['ts'][0]*1e6))+\"_\"+str(round(rays0['ts'][-1]*1e6))+\".info\"\n",
    "print(\"Saving to:\",infofile)\n",
    "print()\n",
    "\n",
    "original_stdout = sys.stdout # Save a reference to the original standard output\n",
    "\n",
    "with open(infofile, 'w') as f:\n",
    "    sys.stdout = f # Change the standard output to the file we created.\n",
    "    print(\"# Transform between map and pixels (opengl flooring convention):\")\n",
    "    print(\"T_px_f=\",T_px_f.tolist())\n",
    "    print(\"# Image info\")\n",
    "    print(\"W,H,th_bins=\",(W,H,th_bins))\n",
    "    print(\"# Which rays were used:\")\n",
    "    print(\"start_time=\", str(np.min(rays0['ts'])))\n",
    "    print(\"end_time=\", str(np.max(rays0['ts'])))\n",
    "    print(\"# Base dataset:\")\n",
    "    print(\"rayfile=\",str(rayfile))\n",
    "    sys.stdout = original_stdout \n",
    "with open(infofile, 'r') as f:\n",
    "    print(f.read())\n",
    "import cv2\n",
    "\n",
    "def writeim(name, IM):\n",
    "    # storage format is 16 bit unsigned\n",
    "    layers=[IM[:,:,layer] for layer in range(IM.shape[2])]\n",
    "    batches=[np.hstack(layers[off:off+40]) for off in range(0,IM.shape[2],40)]\n",
    "    rearranged = np.vstack(batches)\n",
    "    cv2.imwrite(name,rearranged.astype(np.uint16))\n",
    "writeim(\"MISS.png\", MISS)\n",
    "writeim(\"HIT.png\", HIT)\n",
    "cv2.imwrite(\"refl_cache.png\",refl_cache)\n",
    "cv2.imwrite(\"allkept.png\" , ((allkept>0)*255).astype(np.uint8))\n",
    "countvis = np.sum(CLASSIFIED_RFM, axis=2).astype(np.uint16)\n",
    "counttrans = np.sum((CLASSIFIED_RFM==0)&(MISS>0), axis=2).astype(np.uint16)\n",
    "cv2.imwrite(\"countvis.png\" , countvis)\n",
    "cv2.imwrite(\"counttrans.png\" , counttrans)\n",
    "cv2.imwrite(\"countratio.png\" , (countvis*1.0/(counttrans+countvis)*255).astype('uint8'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# GENERATE FIGURES"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### H Figure (RUN BEFORE REMOVALS)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "23100b174f6647c4a453df3ac1d0213c",
       "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, 1723619)\n",
      "(3, 48414)\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<mayavi.modules.glyph.Glyph at 0x7f6a9e3fcd10>"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "## H figure from North Wall, with background removed\n",
    "# Colors show how widely visible the cell is\n",
    "# Requires \"/bbb3_glass_eval_111716.rays\" dataset, with\n",
    "#   substart=0\n",
    "#   subend=int(2.0e6)\n",
    "%matplotlib widget\n",
    "plt.imshow(np.clip(np.sum(vox_refl,axis=2),0,15).T, cmap='gray')\n",
    "# plt.imshow(allkept.T>0)\n",
    "plt.ylim(250,0)\n",
    "plt.xlim(200,500)\n",
    "# y=[106:121]\n",
    "# x=[218:475]\n",
    "plt.plot([218,218,475,475,218],[106,121,121,106,106],'c')\n",
    "\n",
    "to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "%gui qt\n",
    "\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "\n",
    "import mayavi.mlab\n",
    "to_show=np.array(to_show).reshape(3,-1)\n",
    "print(to_show.shape)\n",
    "to_show=to_show[:,to_show[1]>=106]\n",
    "to_show=to_show[:,to_show[1]<=121]\n",
    "to_show=to_show[:,to_show[0]>=218]\n",
    "to_show=to_show[:,to_show[0]<=475]\n",
    "to_show=to_show[:,rfmcomps[tuple(to_show)]==newselcolor]\n",
    "\n",
    "print(to_show.shape)\n",
    "# to_show=to_show[:,to_show[1]>106]\n",
    "# color = np.sum(vox_refl>0.3,axis=2)[tuple(to_show[:2])]\n",
    "color=(HIT+MISS)[tuple(to_show)]\n",
    "# color=score[tuple(to_show[:2])]\n",
    "\n",
    "# color=per[rfmcomps[tuple(to_show)].astype(int)]\n",
    "mayavi.mlab.close(all=True)\n",
    "mayavi.mlab.points3d(to_show[0], to_show[1], to_show[2], color, colormap='spectral', mode= 'point')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5d35b4c849574bfb84fb81f45f41dd78",
       "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": "bdc8ac3971ea4c7fae516ff69e1627ba",
       "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, 1723619)\n",
      "(3, 897)\n"
     ]
    }
   ],
   "source": [
    "## H figure from circle region, with background removed ------------------------------\n",
    "# Colors show how widely visible the cell is\n",
    "# Requires \"/bbb3_glass_eval_111716.rays\" dataset, with\n",
    "#   substart=int(8e6)\n",
    "#   subend=int(11e6)\n",
    "%matplotlib widget\n",
    "plt.imshow(np.clip(np.sum(vox_refl,axis=2),0,15).T, cmap='gray_r')\n",
    "# plt.imshow(allkept.T>0)\n",
    "plt.ylim(450,150)\n",
    "plt.xlim(350,500)\n",
    "# y=[106:121]\n",
    "# x=[218:475]\n",
    "plt.plot([218,218,475,475,218],[106,121,121,106,106],'c')\n",
    "th=np.linspace(0,2*np.pi)\n",
    "r=58\n",
    "plt.plot(r*np.cos(th)+407,r*np.sin(th)+291,\"g\")\n",
    "r=63\n",
    "plt.plot(r*np.cos(th)+407,r*np.sin(th)+291,\"g\")\n",
    "\n",
    "plt.plot(px_ept[:,0][notref==0]-0.5,px_ept[:,1][notref==0]-0.5,'b,')\n",
    "\n",
    "plt.figure()\n",
    "plt.imshow(counttrans+countvis*10)\n",
    "\n",
    "\n",
    "## 3D half --------------------------------------------------------------------------\n",
    "to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "%gui qt\n",
    "\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "\n",
    "import mayavi.mlab\n",
    "to_show=np.array(to_show).reshape(3,-1)\n",
    "print(to_show.shape)\n",
    "r=np.sqrt((to_show[0]-407)**2+(to_show[1]-291)**2)\n",
    "keep = (r>58) & (r<63)\n",
    "to_show=to_show[:,keep]\n",
    "to_show=to_show[:,rfmcomps[tuple(to_show)]==newselcolor]\n",
    "\n",
    "print(to_show.shape)\n",
    "# to_show=to_show[:,to_show[1]>106]\n",
    "\n",
    "color = np.clip((np.sum(vox_refl>0.3,axis=2)[tuple(to_show[:2])])/4,0,wide_threshold/4*2)\n",
    "color = countratio[tuple(to_show[:2])]\n",
    "\n",
    "# color=(HIT+MISS)[tuple(to_show)]\n",
    "# color=score[tuple(to_show[:2])]\n",
    "\n",
    "# color=per[rfmcomps[tuple(to_show)].astype(int)]\n",
    "mayavi.mlab.close(all=True)\n",
    "\n",
    "mayavi.mlab.points3d(to_show[0], -to_show[1], to_show[2]/4, color*100, colormap='spectral', mode= 'point')\n",
    "f = mayavi.mlab.gcf()\n",
    "camera = f.scene.camera\n",
    "\n",
    "camera.position=[407,-291,400]\n",
    "camera.view_up=[0,1,0]\n",
    "camera.focal_point = [407,-291,180]\n",
    "camera.view_angle=70\n",
    "\n",
    "# Need to roll mouse to rerender\n",
    "# 1. Enable legend\n",
    "# 2. set ticks to 11\n",
    "# 3. set legend actor to tick format %-#6.1g\n",
    "# 4. label %reflective\n",
    "# 5. set legend actor Vertical Title Separation to 5\n",
    "\n",
    "# Try:\n",
    "# Whiter background for color contrast\n",
    "# Red for walls\n",
    "# Blue for glass\n",
    "# "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3cfa40731bec43569fa349ceaa3d90b4",
       "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": "11cb5a5e574d4b67b39c6124b021b4ef",
       "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, 1723619)\n",
      "(3, 0)\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Exception occurred in traits notification handler for object: <mayavi.core.lut_manager.LUTManager object at 0x7f701230f860>, trait: data_range, old value: [0. 1.], new value: [ 1.e+299 -1.e+299]\n",
      "Traceback (most recent call last):\n",
      "  File \"/home/anuran/.local/lib/python3.8/site-packages/traits/trait_notifiers.py\", line 342, in __call__\n",
      "    self.handler(*args)\n",
      "  File \"/home/anuran/.local/lib/python3.8/site-packages/mayavi/core/lut_manager.py\", line 434, in _data_range_changed\n",
      "    assert value[0] <= value[1]\n",
      "AssertionError\n"
     ]
    },
    {
     "ename": "OverflowError",
     "evalue": "(34, 'Numerical result out of range')",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mOverflowError\u001b[0m                             Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-38-cf2cd7086aeb>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     55\u001b[0m \u001b[0mmayavi\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmlab\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mclose\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mall\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;32mTrue\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     56\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 57\u001b[0;31m \u001b[0mmayavi\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmlab\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpoints3d\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mto_show\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[0mto_show\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mto_show\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0;36m4\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcolor\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0;36m100\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcolormap\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m'spectral'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmode\u001b[0m\u001b[0;34m=\u001b[0m \u001b[0;34m'point'\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     58\u001b[0m \u001b[0mf\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmayavi\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmlab\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mgcf\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     59\u001b[0m \u001b[0mcamera\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mf\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mscene\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcamera\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/.local/lib/python3.8/site-packages/mayavi/tools/helper_functions.py\u001b[0m in \u001b[0;36mthe_function\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m     36\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     37\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0mthe_function\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0margs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m**\u001b[0m\u001b[0mkwargs\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;32m---> 38\u001b[0;31m         \u001b[0;32mreturn\u001b[0m \u001b[0mpipeline\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0margs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m**\u001b[0m\u001b[0mkwargs\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     39\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     40\u001b[0m     \u001b[0;32mif\u001b[0m \u001b[0mhasattr\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpipeline\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'doc'\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;32m~/.local/lib/python3.8/site-packages/mayavi/tools/helper_functions.py\u001b[0m in \u001b[0;36m__call__\u001b[0;34m(self, *args, **kwargs)\u001b[0m\n\u001b[1;32m     81\u001b[0m             \u001b[0mscene\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdisable_render\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mTrue\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     82\u001b[0m         \u001b[0;31m# Then call the real logic\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 83\u001b[0;31m         \u001b[0moutput\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m__call_internal__\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m*\u001b[0m\u001b[0margs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m**\u001b[0m\u001b[0mkwargs\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     84\u001b[0m         \u001b[0;31m# And re-enable the rendering, if needed.\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     85\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0mscene\u001b[0m \u001b[0;32mis\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/.local/lib/python3.8/site-packages/mayavi/tools/helper_functions.py\u001b[0m in \u001b[0;36m__call_internal__\u001b[0;34m(self, *args, **kwargs)\u001b[0m\n\u001b[1;32m    179\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0mscale_factor\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;34m'auto'\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    180\u001b[0m             \u001b[0mg\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mglyph\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mglyph\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mscale_factor\u001b[0m \u001b[0;34m=\u001b[0m\u001b[0;31m \u001b[0m\u001b[0;31m\\\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 181\u001b[0;31m                              \u001b[0mtools\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_typical_distance\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mg\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmlab_source\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdataset\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    182\u001b[0m             \u001b[0mg\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mglyph\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mglyph\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mclamping\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mTrue\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    183\u001b[0m         \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/.local/lib/python3.8/site-packages/mayavi/tools/tools.py\u001b[0m in \u001b[0;36m_typical_distance\u001b[0;34m(data_obj)\u001b[0m\n\u001b[1;32m    249\u001b[0m     \"\"\"\n\u001b[1;32m    250\u001b[0m     \u001b[0mx_min\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mx_max\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0my_min\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0my_max\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mz_min\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mz_max\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata_obj\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbounds\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 251\u001b[0;31m     distance = numpy.sqrt(((x_max - x_min) ** 2 + (y_max - y_min) ** 2 +\n\u001b[0m\u001b[1;32m    252\u001b[0m                            \u001b[0;34m(\u001b[0m\u001b[0mz_max\u001b[0m \u001b[0;34m-\u001b[0m \u001b[0mz_min\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m**\u001b[0m \u001b[0;36m2\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m/\u001b[0m \u001b[0;34m(\u001b[0m\u001b[0;36m4\u001b[0m \u001b[0;34m*\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    253\u001b[0m                            data_obj.number_of_points ** (0.33)))\n",
      "\u001b[0;31mOverflowError\u001b[0m: (34, 'Numerical result out of range')"
     ]
    }
   ],
   "source": [
    "## H figure from circle region, with background removed Fig 1 version ------------------------------\n",
    "# Colors show how widely visible the cell is\n",
    "# Requires \"/bbb3_glass_eval_111716.rays\" dataset, with\n",
    "#   substart=int(8e6)\n",
    "#   subend=int(11e6)\n",
    "%matplotlib widget\n",
    "plt.imshow(np.clip(np.sum(vox_refl,axis=2),0,15).T, cmap='gray_r')\n",
    "# plt.imshow(allkept.T>0)\n",
    "xc=234\n",
    "yc=304\n",
    "plt.ylim(450-291+yc,150-291+yc)\n",
    "plt.xlim(350-407+xc,500-407+xc)\n",
    "# y=[106:121]\n",
    "# x=[218:475]\n",
    "plt.plot([218,218,475,475,218],[106,121,121,106,106],'c')\n",
    "\n",
    "th=np.linspace(0,2*np.pi)\n",
    "r=58\n",
    "plt.plot(r*np.cos(th)+xc,r*np.sin(th)+yc,\"g\")\n",
    "r=63\n",
    "plt.plot(r*np.cos(th)+xc,r*np.sin(th)+yc,\"g\")\n",
    "\n",
    "plt.plot(px_ept[:,0][notref==0]-0.5,px_ept[:,1][notref==0]-0.5,'b,')\n",
    "\n",
    "plt.figure()\n",
    "plt.imshow(counttrans+countvis*10)\n",
    "\n",
    "## 3D half --------------------------------------------------------------------------\n",
    "to_show=np.nonzero(rfmcomps!=rfmcomps[-3,-3,-3])\n",
    "\n",
    "# per=np.random.permutation(int(np.max(rfmcomps2))+1)\n",
    "%gui qt\n",
    "\n",
    "newselcolor=np.max(rfmcomps[HIGHLY_VISIBLE,:])\n",
    "\n",
    "import mayavi.mlab\n",
    "to_show=np.array(to_show).reshape(3,-1)\n",
    "print(to_show.shape)\n",
    "\n",
    "r=np.sqrt((to_show[0]-xc)**2+(to_show[1]-yc)**2)\n",
    "keep = (r>58) & (r<63)\n",
    "to_show=to_show[:,keep]\n",
    "to_show=to_show[:,rfmcomps[tuple(to_show)]==newselcolor]\n",
    "\n",
    "print(to_show.shape)\n",
    "# to_show=to_show[:,to_show[1]>106]\n",
    "\n",
    "color = np.clip((np.sum(vox_refl>0.3,axis=2)[tuple(to_show[:2])])/4,0,wide_threshold/4*2)\n",
    "color = countratio[tuple(to_show[:2])]\n",
    "\n",
    "# color=(HIT+MISS)[tuple(to_show)]\n",
    "# color=score[tuple(to_show[:2])]\n",
    "\n",
    "# color=per[rfmcomps[tuple(to_show)].astype(int)]\n",
    "mayavi.mlab.close(all=True)\n",
    "\n",
    "mayavi.mlab.points3d(to_show[0], -to_show[1], to_show[2]/4, color*100, colormap='spectral', mode= 'point')\n",
    "f = mayavi.mlab.gcf()\n",
    "camera = f.scene.camera\n",
    "\n",
    "camera.position=[xc,-yc,400]\n",
    "camera.view_up=[0,1,0]\n",
    "camera.focal_point = [xc,-yc,180]\n",
    "camera.view_angle=70\n",
    "\n",
    "# Need to roll mouse to rerender\n",
    "# 1. Enable legend\n",
    "# 2. set ticks to 11\n",
    "# 3. set legend actor to tick format %-#6.1g\n",
    "# 4. label %reflective\n",
    "# 5. set legend actor Vertical Title Separation to 5\n",
    "\n",
    "# Try:\n",
    "# Whiter background for color contrast\n",
    "# Red for walls\n",
    "# Blue for glass\n",
    "# "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Do GT work"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Stages and Inputs:\n",
    "1. Base RFM\n",
    "      1. Require: rays, Optional: is_ref_rays\n",
    "      2. % visible\n",
    "      3. countref\n",
    "      4. vox_refl\n",
    "      5. [not really done] update flood_cache\n",
    "      6. update high_viz_seeds\n",
    "      7. update refl_cache <- threshhold %visible \n",
    "2. Motion Removal\n",
    "      1. Requires: vox_refl, high_viz_seeds\n",
    "      2. conncomps <- before flood\n",
    "      3. floodcomps <- after flood\n",
    "      4. update refl_cache\n",
    "3. Reflection Removal\n",
    "      1. Requires: rays, refl_cache\n",
    "      2. is_ref_rays\n",
    "4. RFM #2\n",
    "5. Motion Removal #2\n",
    "\n",
    "\n",
    "\n",
    "Strategies\n",
    "[assume all reflection removal immediately followed by RFM]\n",
    "\n",
    "0. OCC GRID\n",
    "\n",
    "1. \n",
    "RFM\n",
    "\n",
    "2. \n",
    "RFM\n",
    "MOT\n",
    "\n",
    "3. \n",
    "RFM\n",
    "MOT\n",
    "REF\n",
    "\n",
    "4. \n",
    "RFM\n",
    "REF\n",
    "REF\n",
    "REF\n",
    "REF\n",
    "\n",
    "5. \n",
    "RFM\n",
    "MOT\n",
    "REF\n",
    "MOT\n",
    "\n",
    "6. \n",
    "RFM\n",
    "REF\n",
    "MOT\n",
    "REF\n",
    "MOT\n",
    "REF\n",
    "MOT\n",
    "REF\n",
    "MOT\n",
    "...\n",
    "\n",
    "\n",
    "      \n",
    "      "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "# load_gt()\n",
    "# convert to points\n",
    "# Kd tree for DNN\n",
    "gti = cv2.imread(\"bbb3_glass_eval_111716-gt.png\")\n",
    "\n",
    "\n",
    "# plt.imshow(gti)\n",
    "empty = np.min(gti,axis=2)[2::5,2::5].T\n",
    "plt.imshow((empty + (allkept>0)*500), cmap='gray')\n",
    "\n",
    "bounds=(np.max(gti,axis=2).T==255)&(np.min(gti,axis=2).T==0)\n",
    "gtglass = (gti[:,:,0]==255).T&(np.min(gti,axis=2).T==0)\n",
    "gtmetal = (gti[:,:,2]==255).T&(np.min(gti,axis=2).T==0)\n",
    "gtdiffuse = (gti[:,:,1]==255).T&(np.min(gti,axis=2).T==0)\n",
    "gtwall = np.nonzero(bounds)\n",
    "gtwall[0][:] = gtwall[0]/5\n",
    "gtwall[1][:] = gtwall[1]/5\n",
    "\n",
    "plt.figure()\n",
    "plt.imshow(np.dstack([gtmetal,gtdiffuse,gtglass])*1.0)\n",
    "\n",
    "assert(np.sum(gtmetal&gtdiffuse)==0)\n",
    "assert(np.sum(gtmetal&gtglass)==0)\n",
    "assert(np.sum(gtglass&gtdiffuse)==0)\n",
    "assert(np.all((gtmetal|gtglass|gtdiffuse)==bounds))\n",
    "\n",
    "\n",
    "\n",
    "gtrefbarrier = np.zeros_like(refl_cache, bool)\n",
    "gtrefbarrier[gtwall[0],gtwall[1]] = True\n",
    "\n",
    "wide_bar = cv2.dilate(((gtrefbarrier!=0)*255).astype('uint8'),np.ones((3,3),np.uint8))\n",
    "assert(not np.any((wide_bar==0)&(gtrefbarrier!=0)))\n",
    "print(np.sum(gtwall[0]))\n",
    "plt.figure()\n",
    "plt.imshow(gtrefbarrier.T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Get GT reflection classes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "lim=len(keeprays)\n",
    "\n",
    "\n",
    "# VERY EXPENSIVE: touched barrier, got near barrier, went way through barrier\n",
    "refl_cache[...] = gtrefbarrier\n",
    "keptrays, realclass = classify(outrays[:,:lim].copy(),pen_depth=0.3)\n",
    "\n",
    "\n",
    "refl_cache[...] = wide_bar\n",
    "keptrays, closenref = classify(outrays[:,:lim].copy(),pen_depth=0.0)\n",
    "\n",
    "refl_cache[...] = (empty==0)\n",
    "keptrays, inzone = classify(outrays[:,:lim].copy(),pen_depth=0.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "plt.close('all')\n",
    "plt.figure(figsize=(selected.shape[0]/my_dpi*sc, selected.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "\n",
    "closeref = (closenref==0)\n",
    "\n",
    "# something is definitely a reflection if it touches a barrier, goes well through it, and ends not near the barrier\n",
    "\n",
    "# Base state\n",
    "# plt.plot(px_ept[:lim][keeprays,0], px_ept[:lim][keeprays,1] , 'w,')\n",
    "\n",
    "# Reflection\n",
    "intept = px_ept.astype(int)\n",
    "inrange = np.clip(px_ept.T, loclip[:2], hiclip[:2]).astype(int)\n",
    "endfar = (wide_bar[inrange[0],inrange[1]]==0)&keeprays\n",
    "outer = endfar[:lim]&(realclass==0)\n",
    "defref = outer&keeprays\n",
    "\n",
    "plt.plot(px_ept[:lim][defref,0], px_ept[:lim][defref,1] , 'c,')\n",
    "\n",
    "# Motion\n",
    "zone=empty\n",
    "defnotref = inzone&(closeref==0)&keeprays[:lim]\n",
    "print(\"defnotref:\",np.sum(defnotref))\n",
    "# plt.imshow(((empty==0).T))\n",
    "# plt.imshow((gtrefbarrier.T))\n",
    "import matplotlib as mpl\n",
    "vimg=np.dstack([gtmetal.T,gtdiffuse.T,gtglass.T])*1.0\n",
    "# import cv\n",
    "for i in range(1):\n",
    "    strel=cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * 5 + 1, 2 * 5 + 1),\n",
    "                                       (5, 5))\n",
    "    vimg=cv2.dilate(vimg.astype(np.float32),strel)\n",
    "imgplot= plt.imshow(vimg*0,extent=(0,W,H,0))\n",
    "\n",
    "def fatten(img):\n",
    "    strel=cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2 * 5 + 1, 2 * 5 + 1),\n",
    "                                       (5, 5))\n",
    "    vimg=cv2.dilate(img.astype(np.float32),strel)\n",
    "    return vimg.astype(img.dtype)\n",
    "\n",
    "# imgplot= plt.imshow(np.clip(selected.T,0,30)*0,cmap='gray',vmin=-10, vmax=30)\n",
    "transform = mpl.transforms.Affine2D().translate(0.5, 0.5)\n",
    "imgplot.set_transform(transform + plt.gca().transData)\n",
    "\n",
    "plt.plot(px_ept[:lim][defnotref,0], px_ept[:lim][defnotref,1] , 'm,')\n",
    "\n",
    "# Wall core\n",
    "inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "keepcore = (bounds[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "# plt.plot(px_ept[:lim][keepcore,0], px_ept[:lim][keepcore,1] , 'y,')\n",
    "\n",
    "def easy_read_fig():\n",
    "    inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "    glasscore = (fatten(gtglass)[inrange5[0],inrange5[1]]!=0)&keeprays&(defref==0)\n",
    "    plt.plot(px_ept[:lim][glasscore,0], px_ept[:lim][glasscore,1] , 'b,',markersize=1)\n",
    "\n",
    "    inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "    metalcore = (fatten(gtmetal)[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "    plt.plot(px_ept[:lim][metalcore,0], px_ept[:lim][metalcore,1] , 'r,')\n",
    "\n",
    "    inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "    diffusecore = (fatten(gtdiffuse)[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "    plt.plot(px_ept[:lim][diffusecore,0], px_ept[:lim][diffusecore,1] , 'g,',markersize=1)\n",
    "easy_read_fig()\n",
    "\n",
    "inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "glasscore = (gtglass[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "# plt.plot(px_ept[:lim][glasscore,0], px_ept[:lim][glasscore,1] , 'b,',markersize=1)\n",
    "\n",
    "inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "metalcore = (gtmetal[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "# plt.plot(px_ept[:lim][metalcore,0], px_ept[:lim][metalcore,1] , 'r,')\n",
    "\n",
    "inrange5 = np.clip(px_ept.T*5, loclip[:2]*5, hiclip[:2]*5).astype(int)\n",
    "diffusecore = (gtdiffuse[inrange5[0],inrange5[1]]!=0)&keeprays\n",
    "# plt.plot(px_ept[:lim][diffusecore,0], px_ept[:lim][diffusecore,1] , 'g,',markersize=1)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "plt.ylim(1000,700)\n",
    "plt.xlim(520,710)\n",
    "assert(np.sum(diffusecore&endfar)==0)\n",
    "eval_set = np.zeros_like(keeprays)\n",
    "classes = [defref, defnotref, glasscore, metalcore, diffusecore]\n",
    "for i in range(len(classes)):\n",
    "    eval_set |= classes[i]\n",
    "    for j in range(len(classes)):\n",
    "        if i==j:\n",
    "            continue\n",
    "        print(i,j)\n",
    "        assert(not np.any(classes[i]&classes[j]))\n",
    "assert(np.all((glasscore| metalcore|diffusecore)==(keepcore)))\n",
    "\n",
    "print(\"Measurable verse:\", np.sum(np.vstack(classes)))\n",
    "print(\"TP refl:\", np.sum(defref&(notref==0)))\n",
    "\n",
    "plt.savefig('GT Program.png')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "\n",
    "\n",
    "plt.imshow(wide_bar.T,extent=(0,W,H,0))\n",
    "# # transform = mpl.transforms.Affine2D().translate(0.5, 0.5)\n",
    "# # imgplot.set_transform(transform + plt.gca().transData)\n",
    "\n",
    "\n",
    "# overlapped=defref&diffusecore\n",
    "\n",
    "plt.plot(px_ept[:lim][overlapped,0],px_ept[:lim][overlapped,1],'b.')\n",
    "\n",
    "wide_bar[700,864]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib widget\n",
    "plt.close('all')\n",
    "plt.figure(figsize=(selected.shape[0]/my_dpi*sc, selected.shape[1]/my_dpi*sc), dpi=my_dpi)\n",
    "\n",
    "imgplot= plt.imshow(np.clip(selected.T,0,30),cmap='gray_r',vmin=-5, vmax=5, extent=(0,W,H,0))\n",
    "plt.ylim(1000,700)\n",
    "plt.xlim(520,710)\n",
    "\n",
    "plt.savefig('Program Output.png')\n",
    "# plt.plot(px_ept[:lim][(notref==0),0], px_ept[:lim][(notref==0),1] , 'w,')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pred_ref = (notref == False)&eval_set\n",
    "pred_motion = (notref != False)&(allkept[inrange[0],inrange[1]]==0)&eval_set\n",
    "pred_static = (notref != False)&(allkept[inrange[0],inrange[1]]!=0)&eval_set\n",
    "\n",
    "print(\"Real ref:\", np.sum((pred_motion|pred_ref)&defref),\"(Pred Remove), \", np.sum(pred_static&defref), \"(Pred Keep)\")\n",
    "# print(\"total ref:\",np.sum(defref))\n",
    "print(\"Real motion:\", np.sum((pred_ref|pred_motion)&defnotref),\"(Pred Remove), \", np.sum(defnotref&pred_static), \"(Pred Keep)\")\n",
    "# print(\"total real motion:\",np.sum(defnotref))\n",
    "print()\n",
    "\n",
    "\n",
    "\n",
    "kept_solids = (allkept[inrange[0],inrange[1]]!=0)&keeprays&notref&eval_set\n",
    "removed_solids = (allkept[inrange[0],inrange[1]]==0)&keeprays&notref&eval_set\n",
    "removed_refl = (notref==0) & eval_set\n",
    "\n",
    "import inspect, re\n",
    "\n",
    "def varname(p):\n",
    "    for line in inspect.getframeinfo(inspect.currentframe().f_back)[3]:\n",
    "        m = re.search(r'\\bvarname\\s*\\(\\s*([A-Za-z_][A-Za-z0-9_]*)\\s*\\)', line)\n",
    "        if m:\n",
    "            return m.group(1)\n",
    "total=0\n",
    "# actual ['defref', 'defnotref', 'glasscore', 'metalcore', 'diffusecore'] -> \n",
    "for c,cn in zip(classes[2:],['glass', 'metal', 'diffuse']):\n",
    "    print('Actual', cn,'&', \n",
    "          np.sum(c&pred_static),'(Pred Keep) &',\n",
    "          np.sum((c&pred_motion)), '(Pred Remove)',\n",
    "          \"\"\"\\\\ \\hhline{~*\\items{|-}|}\"\"\")\n",
    "    \n",
    "# print(\"total attempted:\" , np.sum(keepcore)) # remainder is unclear reflection or wall\n",
    "\n",
    "assert(False)\n",
    "\n",
    "\n",
    "total_ref = np.sum(defref)\n",
    "print(\"Total Reflection\", total_ref)\n",
    "TP_reflection = np.sum((notref==0)*defref)\n",
    "print(\"TP Reflection\", TP_reflection)\n",
    "print(\"Reflection removed\", TP_reflection/total_ref)\n",
    "\n",
    "# total_not_ref = np.sum(defnotref\n",
    "FP_reflection = np.sum((notref==1)*defnotref)\n",
    "print(\"TP Reflection\", TP_reflection)\n",
    "print(\"Reflection removed\", TP_reflection/total_ref)"
   ]
  }
 ],
 "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
}