TakeshiHMM-Gazebo-ICP.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "from sklearn.cluster import KMeans\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "from sklearn.cluster import  MiniBatchKMeans\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from joblib import dump, load\n",
    "import sys\n",
    "%autosave 0 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "class HMM (object):\n",
    "             def __init__(self,A,B,PI):\n",
    "                 self.A=A\n",
    "                 self.B=B\n",
    "                 self.PI=PI   \n",
    "def viterbi(obs,Modelo1,PI):\n",
    "    \n",
    "    delta=np.zeros((len(obs)+1,len(Modelo1.A)))\n",
    "    phi=np.zeros((len(obs)+1,len(A)))+666\n",
    "    path =np.zeros(len(obs)+1)\n",
    "    T=len(obs)\n",
    "    Modelo1.PI = PI\n",
    "    delta[0,:]= Modelo1.PI * Modelo1.B[:,obs[0]]\n",
    "    phi[0,:]=666\n",
    "    for t in range(len(obs)):\n",
    "        for j in range(delta.shape[1]):\n",
    "\n",
    "            delta [t+1,j]=np.max(delta[t] * A[:,j]) * B[j,obs[t]]\n",
    "            phi[t+1,j]= np.argmax(delta[t] * A[:,j])\n",
    "    path[T]=int(np.argmax(delta[T,:]))\n",
    "    for i in np.arange(T-1,0,-1):\n",
    "        #print (i,phi[i+1,int(path[i+1])])\n",
    "        path[i]=phi[i+1,int(path[i+1])]\n",
    "    return(int(path))\n",
    "def cuantizar_xy(xy, cc):\n",
    "    xycuant=cc\n",
    "    out=np.power(xycuant-xy,2).sum(axis=1).argmin()\n",
    "    return out\n",
    "    \n",
    "def path_to_xy(path,ccxy):\n",
    "    estimated= pd.DataFrame(path.astype(int).T)\n",
    "    estimated.columns=['Path_vit']\n",
    "    estimated['xcuant'] = estimated['Path_vit'].apply(lambda x: ccxy[x,0])\n",
    "    estimated['ycuant'] = estimated['Path_vit'].apply(lambda x: ccxy[x,1])                                \n",
    "    return (estimated)\n",
    "def quantized(xyth,ccxyth):\n",
    "    xythcuant=np.argmin(np.linalg.norm(xyth-ccxyth,axis=1))\n",
    "    x,y=ccxyth[xythcuant,:2]\n",
    "    return ((x,y),(xythcuant))\n",
    "def viterbi(obs,Modelo1,PI):\n",
    "    A, B= Modelo1.A , Modelo1.B\n",
    "    \n",
    "    delta=np.zeros((len(obs)+1,len(Modelo1.A)))\n",
    "    phi=np.zeros((len(obs)+1,len(A)))+666\n",
    "    path =np.zeros(len(obs)+1)\n",
    "    T=len(obs)\n",
    "    Modelo1.PI = PI\n",
    "    delta[0,:]= Modelo1.PI * Modelo1.B[:,obs[0]]\n",
    "    phi[0,:]=666\n",
    "    for t in range(len(obs)):\n",
    "        for j in range(delta.shape[1]):\n",
    "\n",
    "            delta [t+1,j]=np.max(delta[t] * A[:,j]) * B[j,obs[t]]\n",
    "            phi[t+1,j]= np.argmax(delta[t] * A[:,j])\n",
    "    path[T]=int(np.argmax(delta[T,:]))\n",
    "    for i in np.arange(T-1,0,-1):\n",
    "        #print (i,phi[i+1,int(path[i+1])])\n",
    "        path[i]=phi[i+1,int(path[i+1])]\n",
    "    return(path)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Markov_A_2_grafo(A,ccxyth):\n",
    "    dists=np.zeros(A.shape)\n",
    "    for i in range(A.shape[0]):\n",
    "        for j in range (A.shape[1]):\n",
    "            if A[i,j]!=0 :\n",
    "                dists[i,j]= np.linalg.norm(ccxyth[i]-ccxyth[j])    \n",
    "    \n",
    "    \n",
    "    con = np.where(dists==0,np.inf,dists)\n",
    "    graphe2=grafo(ccxyth,con)\n",
    "    return graphe2\n",
    "\n",
    "\n",
    "class node(object):\n",
    "    def __init__(self,x,y):\n",
    "        self.x=x\n",
    "        self.y=y\n",
    "        \n",
    "        \n",
    "class grafo (object):\n",
    "             def __init__(self,nodos,conec):\n",
    "                 self.nodos=nodos\n",
    "                 self.conec=conec        \n",
    "\n",
    "def dijkstra(nodoinicial,nodofinal,graphe):\n",
    "    \n",
    "\n",
    "    numnodos= len(graphe.nodos)\n",
    "    con = graphe.conec\n",
    "    D= np.ones(numnodos)*np.inf\n",
    "    Prv= np.ones(numnodos)*np.inf\n",
    "    V= np.zeros(numnodos)\n",
    "    a = nodoinicial\n",
    "    D[a]=0\n",
    "    Prv[a]=0\n",
    "    Prv[np.where(con[a]!=np.inf)]=a\n",
    "    V[a]=1\n",
    "    Dacc=D[a]\n",
    "    ########\n",
    "    D=np.minimum(D,con[a]+D[a])\n",
    "    cont=0\n",
    "    sucess=False\n",
    "    while(sucess==False):\n",
    "        a = np.argmin(D+np.where (V==1,np.inf, V))\n",
    "        Dacc=D[a]\n",
    "        Prv[np.where(D>(con[a]+Dacc) )]=a\n",
    "        V[a]=1\n",
    "        D=np.minimum(D,con[a]+Dacc)\n",
    "        if (a== nodofinal):\n",
    "            print(\"RUTA CALCULADA \")\n",
    "            sucess=True\n",
    "    rutainv=[]\n",
    "    rutainv.append(nodofinal)\n",
    "    while(rutainv[-1]!=nodoinicial):\n",
    "        prv=Prv[int(rutainv[-1])]\n",
    "        rutainv.append(prv)\n",
    "\n",
    "    ruta=[]\n",
    "    for n in reversed(rutainv):\n",
    "        ruta.append(n)\n",
    "    return(ruta)\n",
    "\n",
    "def plot_cc_arrow(cc):\n",
    "    fig=plt.figure(figsize=(10,10))\n",
    "    for c in cc:\n",
    "        ang=c[2]*180/math.pi\n",
    "        \n",
    "\n",
    "        plt.plot(c[0], c[1], marker=(3, 1,ang ), markersize=10,c='green', linestyle='None')\n",
    "        plt.plot(c[0]-(.01*np.cos(c[2]+math.pi/2)), c[1]-(.01*np.sin(c[2]+math.pi/2)), marker=(1, 1,ang), markersize=25,c='green', linestyle='--')\n",
    "\n",
    "    #plt.xlim([0,4])\n",
    "    #plt.ylim([0,4])\n",
    "\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "both=False\n",
    "#data=pd.read_csv('dataset_candidatura_wr/lecs_odom(goodman).txt')\n",
    "data=pd.read_csv('/home/roboworks/Codes/hsrb_robocup_dspl_docker/src/hmm_navigation/scripts/lecs_odom.txt')\n",
    "#data=data2\n",
    "#data=data.iloc[:30000,:]\n",
    "\n",
    "cols=['x','y','theta']\n",
    "n_reads=data.shape[1]-3\n",
    "n_reads\n",
    "etiquetas=[]\n",
    "for i in range (n_reads):\n",
    "    etiquetas.append('R'+str(i))\n",
    "\n",
    "        \n",
    "for lab in cols:\n",
    "    etiquetas.append(lab)\n",
    "data.columns=etiquetas\n",
    "\n",
    "    \n",
    "\n",
    "\"\"\"if (only_nice!=True):\n",
    "    print(\"NOT ONLY NICE\")\n",
    "    data2=pd.read_csv('lecturasconodometria.txt')\n",
    "    data2.columns=etiquetas\n",
    "    data3= pd.concat((data,data2))\n",
    "    data=data3\n",
    "\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "\n",
    "####\n",
    "#index = data[data.y<-1].index\n",
    "#data=data.drop(index)\n",
    "data.describe()\n",
    "\n",
    "\n",
    "\n",
    "#####\n",
    "\n",
    "xyth=data[['x','y','theta']]\n",
    "\n",
    "lecs=data.iloc[:,0:n_reads]\n",
    "lecs=np.clip(lecs,0,5)\n",
    "data.iloc[:,0:n_reads]=lecs\n",
    "\n",
    "data.describe()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print( 'There are ',data.isnull().sum().sum(),'nans','removing')\n",
    "data.fillna(0,inplace=True)\n",
    "print( 'There are ',data.isnull().sum().sum(),'nans','left')\n",
    "lecs=data.iloc[:,0:n_reads]\n",
    "lecs=np.clip(lecs,0,10)\n",
    "data.iloc[:,0:n_reads]=lecs\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "get_new_ccs=True\n",
    "Q_aff=True\n",
    "if get_new_ccs:\n",
    "    \n",
    "    K_corpus=256\n",
    "    N=20\n",
    "    \n",
    "    kmedias=MiniBatchKMeans(init='k-means++', n_clusters=K_corpus, batch_size=1000,\n",
    "                      n_init=10, max_no_improvement=10, verbose=0)\n",
    "    kmedias.fit(lecs)\n",
    "    ccvk=kmedias.cluster_centers_\n",
    "    mbk = MiniBatchKMeans(init='k-means++', n_clusters=N, batch_size=1000,\n",
    "                      n_init=10, max_no_improvement=10, verbose=0)\n",
    "    mbk.fit(xyth)\n",
    "    cc=mbk.cluster_centers_\n",
    "    ccxyth=pd.DataFrame(cc)\n",
    "    ccxyth['norm']=np.linalg.norm(cc,axis=1)\n",
    "    #sort by proximity to origin\n",
    "    cc=ccxyth.sort_values('norm').iloc[:,:3].values\n",
    "    \n",
    "else:\n",
    "    ccxyth,ccvk=np.load('ccxyth.npy'),np.load('ccvk.npy')\n",
    "    print(\"CUANTIZANDO SIN RECALCULAR CC\")\n",
    "aus=[]\n",
    "auvks=[]\n",
    "for i,lec in zip(xyth.values, lecs.values):\n",
    "    _, au= quantized(i , cc)\n",
    "    aus.append(au)\n",
    "\n",
    "    auvk= np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()\n",
    "    auvks.append(auvk)\n",
    "data['Y']=aus\n",
    "data['Vk']=auvks\n",
    "\n",
    "\n",
    "if (Q_aff==True):\n",
    "    clf=load('aff_prop_class.joblib') ##PYTHON VERSION 2 \n",
    "    data['Vk_aff']=clf.predict(lecs)\n",
    "\n",
    "\n",
    "if (Q_aff!=True):\n",
    "    data['Vk_aff']=data['Vk']\n",
    "\n",
    "obs,est= data['Vk'].values,data['Y'].values\n",
    "obs_aff= data['Vk_aff'].values\n",
    "np.save('obs.npy',obs)\n",
    "np.save('est.npy',est)\n",
    "np.save ('ccxyth.npy',cc)\n",
    "np.save ('ccvk.npy',ccvk)\n",
    "data.to_csv('pddata.csv')    \n",
    "print('new obs/ est almacenados')\n",
    "data['Y'].describe()   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "new_xyth=False\n",
    "if new_xyth:\n",
    "\n",
    "    xyth_symbols=data[['x','y','theta','Vk','Vk_aff']]\n",
    "    mbk.fit(xyth_symbols)\n",
    "    ccxyth_symbols=mbk.cluster_centers_\n",
    "    ccxyth=ccxyth_symbols[:,:3]\n",
    "    data.Y=mbk.labels_\n",
    "    est= data['Y'].values\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "data.to_csv('fast_load.csv')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "save=True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "####BAUM WELCH APPROX###############???!?!?!\n",
    "if save:\n",
    "    N=est.max()+1 \n",
    "    K_corpus= obs.max()+1\n",
    "    TRANS=np.zeros((N,N))\n",
    "    for i in range (N):\n",
    "        fromTO=[]\n",
    "\n",
    "\n",
    "        indices=np.asarray(np.where(est==i)).ravel()\n",
    "\n",
    "\n",
    "        for indice in indices:\n",
    "\n",
    "            if (int(indice)==len(est)-1):\n",
    "\n",
    "                indice=indice-1\n",
    "\n",
    "            fromTO.append(est[indice+1])\n",
    "        fromTO.append(0)\n",
    "        fromTO.append(N-1)\n",
    "        aux=np.asarray(np.bincount(fromTO    ))\n",
    "        aux[0]=aux[0]-1\n",
    "        aux[N-1]=aux[N-1]-1\n",
    "\n",
    "        TRANS[i,:]=aux\n",
    "    TRANS=TRANS/TRANS.sum(axis=1)\n",
    "    #####################################3\n",
    "    EMIS= np.zeros((N,K_corpus))\n",
    "    for Vk in range (K_corpus):\n",
    "\n",
    "        indices=np.asarray(np.where(obs==Vk)).ravel()\n",
    "        estconvk= est[indices]\n",
    "\n",
    "        numestenvk=np.bincount(estconvk)\n",
    "        if (N-len(numestenvk)>0):\n",
    "            numestenvk=np.append(numestenvk, np.zeros(N-len(numestenvk)))\n",
    "        EMIS[:,Vk]=(numestenvk/np.bincount(est)+.00001)\n",
    "\n",
    "    A,B=TRANS,EMIS\n",
    "    PI=np.ones(N)/N\n",
    "    Modelo1=HMM(A,B,PI)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "    #np.linalg.norm(Modelo1.A-Modelo2.A)\n",
    "    np.save('A.npy',Modelo1.A)\n",
    "    np.save('B.npy',Modelo1.B)\n",
    "    np.save('PI.npy',Modelo1.PI)\n",
    "    print(\"MODEL SAVED\")\n",
    "else:\n",
    "    A,B,PI = np.load('A.npy'),np.load('B.npy'),np.load('PI.npy')\n",
    "    Modelo1=HMM(A,B,PI)\n",
    "B.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "obs2=data.Vk_aff.values\n",
    "if save:\n",
    "    ####BAUM WELCH APPROX  AFF PROP###############???!?!?!\n",
    "    \n",
    "    N=est.max()+1 \n",
    "    K_corpus= obs2.max()+1\n",
    "    TRANS=np.zeros((N,N))\n",
    "    for i in range (N):\n",
    "        fromTO=[]\n",
    "\n",
    "\n",
    "        indices=np.asarray(np.where(est==i)).ravel()\n",
    "\n",
    "\n",
    "        for indice in indices:\n",
    "\n",
    "            if (int(indice)==len(est)-1):\n",
    "\n",
    "                indice=indice-1\n",
    "\n",
    "            fromTO.append(est[indice+1])\n",
    "        fromTO.append(0)\n",
    "        fromTO.append(N-1)\n",
    "        aux=np.asarray(np.bincount(fromTO    ))\n",
    "        aux[0]=aux[0]-1\n",
    "        aux[N-1]=aux[N-1]-1\n",
    "\n",
    "        TRANS[i,:]=aux\n",
    "    TRANS=TRANS/TRANS.sum(axis=1)\n",
    "    #####################################3\n",
    "    EMIS= np.zeros((N,K_corpus))\n",
    "    for Vk in range (K_corpus):\n",
    "\n",
    "        indices=np.asarray(np.where(obs2==Vk)).ravel()\n",
    "        estconvk= est[indices]\n",
    "\n",
    "        numestenvk=np.bincount(estconvk)\n",
    "        if (N-len(numestenvk)>0):\n",
    "            numestenvk=np.append(numestenvk, np.zeros(N-len(numestenvk)))\n",
    "        EMIS[:,Vk]=(numestenvk/np.bincount(est))\n",
    "\n",
    "    A,B=TRANS,EMIS\n",
    "    PI=np.ones(N)/N\n",
    "\n",
    "    Modelo2= HMM(A,B,PI)\n",
    "    Modelo2.B=B\n",
    "    Modelo2.A=A\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "    #np.linalg.norm(Modelo1.A-Modelo2.A)\n",
    "    np.save('A2.npy',Modelo2.A)\n",
    "    np.save('B2.npy',Modelo2.B)\n",
    "    np.save('PI2.npy',Modelo2.PI)\n",
    "    print(\"MODEL SAVED\")\n",
    "else:\n",
    "    A,B,PI = np.load('A2.npy'),np.load('B2.npy'),np.load('PI2.npy')\n",
    "    Modelo2=HMM(A,B,PI)\n",
    "B.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def accuracy(yhat,ytrue):\n",
    "    weights=[.5,.3,.1,.05,.05]\n",
    "    if len (yhat)>=5:\n",
    "        yhat=yhat[-5:]\n",
    "        ytrue=ytrue[-5:]\n",
    "        accuracy=np.dot((yhat==ytrue),weights)\n",
    "        return accuracy\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "buf_vit=150\n",
    "indice= int(np.random.rand()*(len(obs)-2*buf_vit))\n",
    "for i in range(1):\n",
    "    \n",
    "   \n",
    "    obs_windowed= obs[indice:indice+buf_vit]\n",
    "    obs2_windowed= obs2[indice:indice+buf_vit]\n",
    "    est_windowed= est[indice:indice+buf_vit]\n",
    "    odomreal= data[['x','y']].iloc[indice:indice+buf_vit]\n",
    "    #print(data[['x','y']].iloc[indice:indice+buf_vit],est_windowed)\n",
    "\n",
    "    print(indice)\n",
    "    #PATH from Model 1 KMEANS CC's obs\n",
    "    path= viterbi(obs_windowed,Modelo1,Modelo1.PI)\n",
    "    \n",
    "    ##PATH FROM MODEL 2 WICH NEEDS AFF PORP QUANT OBS\n",
    "    path2= viterbi(obs2_windowed,Modelo2,Modelo2.PI)\n",
    "    yhat=np.zeros(len(path)-1)\n",
    "    yhat2=np.zeros(len(path2)-1)\n",
    "    ytrue=np.zeros(len(path)-1)\n",
    "    for i in range(len(path)-1):\n",
    "        yhat[i]=int(path[i])\n",
    "        yhat2[i]=int(path2[i])\n",
    "        ytrue[i]=int(est_windowed[i])\n",
    "    print(accuracy(ytrue,yhat), accuracy(ytrue,yhat2))\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "len(obs_windowed)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "paths= viterbi(obs2_windowed,Modelo2,Modelo2.PI)  #Modelo 2 uses obs2\n",
    "path=paths[int(-len(paths)/2):]\n",
    "path_bigdelay = paths[:int(-len(paths)/2)]\n",
    "cords= path_to_xy(path,cc)\n",
    "cords_bigdelay = path_to_xy(path_bigdelay,cc)\n",
    "cords1=path_to_xy(est_windowed.T[int(-len(paths)/2):],cc)\n",
    "cords1delay=path_to_xy(est_windowed.T[:int(-len(paths)/2)],cc)\n",
    "\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Test Run Buf Vit=150', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.00313)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "cords3=path_to_xy(np.arange(0,Modelo1.A.shape[0]),cc)\n",
    "ax1.scatter(x= cords3.xcuant, y=cords3.ycuant ,marker='.',s=40,c='g')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def plot_cc_arrow(cc):\n",
    "    fig=plt.figure(figsize=(10,10))\n",
    "    for c in cc:\n",
    "        ang=c[2]*180/math.pi\n",
    "        \n",
    "\n",
    "        plt.plot(c[0], c[1], marker=(3, 1,ang + 90 ), markersize=10,c='green', linestyle='None')\n",
    "        plt.plot(c[0]-(.01*np.cos(c[2])), c[1]-(.01*np.sin(c[2])), marker=(1, 1,ang+ 90), markersize=25,c='green', linestyle='--')\n",
    "        #plt.plot(c[0]-(.01*np.cos(c[2]+.5*math.pi)), c[1]-(.01*np.sin(c[2]+.5*math.pi)), marker=(1, 1,ang), markersize=25,c='blue', linestyle='--')\n",
    "\n",
    "    #plt.xlim([0,4])\n",
    "    #plt.ylim([0,4])\n",
    "\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig=plt.figure(figsize=(10,10))\n",
    "plot_cc_arrow(cc)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc=ccxyth.sort_values('norm').iloc[:,:3].values\n",
    "paths= viterbi(obs2_windowed,Modelo2,Modelo2.PI)  #Modelo 2 uses obs2\n",
    "path=paths[int(-len(paths)/2):]\n",
    "path_bigdelay = paths[:int(-len(paths)/2)]\n",
    "cords= path_to_xy(path,cc)\n",
    "cords_bigdelay = path_to_xy(path_bigdelay,cc)\n",
    "cords1=path_to_xy(est_windowed.T[int(-len(paths)/2):],cc)\n",
    "cords1delay=path_to_xy(est_windowed.T[:int(-len(paths)/2)],cc)\n",
    "\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Test Run Buf Vit=150', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.00313)\n",
    "\n",
    "ax1.scatter(x=cords.xcuant, y=cords.ycuant, marker='o',c='r',s=52,alpha=.5 , )\n",
    "ax1.scatter(x=cords_bigdelay.xcuant, y=cords_bigdelay.ycuant, marker='+',c='m',s=52,alpha=.1 )\n",
    "ax1.scatter(x=cords1.xcuant, y=cords1.ycuant, marker='o',c='b' ,alpha=1)\n",
    "ax1.scatter(x=cords1delay.xcuant, y=cords1delay.ycuant, marker='+',c='c' ,alpha=.1)\n",
    "\n",
    "\n",
    "\n",
    "ax1.scatter(x= odomreal.x[int(-len(paths)/2):], y=odomreal.y[int(-len(paths)/2):],marker='.',s=13,c='y')\n",
    "ax1.scatter(x= odomreal.x[:int(-len(paths)/2)], y=odomreal.y[:int(-len(paths)/2)],marker='.',s=13,c='m')\n",
    "cords3=path_to_xy(np.arange(0,Modelo1.A.shape[0]),cc)\n",
    "ax1.scatter(x= cords3.xcuant, y=cords3.ycuant ,marker='.',s=40,c='g')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "\n",
    "buf_vit=60\n",
    "for i in range(1):\n",
    "    \n",
    "    \n",
    "    obs_windowed= obs[indice:indice+buf_vit]\n",
    "    obs2_windowed= obs2[indice:indice+buf_vit]\n",
    "    est_windowed= est[indice:indice+buf_vit]\n",
    "    odomreal= data[['x','y']].iloc[indice:indice+buf_vit]\n",
    "    #print(data[['x','y']].iloc[indice:indice+buf_vit],est_windowed)\n",
    "\n",
    "    print(indice)\n",
    "    path= viterbi(obs_windowed,Modelo1,Modelo1.PI)\n",
    "    path2= viterbi(obs2_windowed,Modelo2,Modelo2.PI)\n",
    "    yhat=np.zeros(len(path)-1)\n",
    "    yhat2=np.zeros(len(path2)-1)\n",
    "    ytrue=np.zeros(len(path)-1)\n",
    "    for i in range(len(path)-1):\n",
    "        yhat[i]=int(path[i])\n",
    "        yhat2[i]=int(path2[i])\n",
    "        ytrue[i]=int(est_windowed[i])\n",
    "    print(accuracy(ytrue,yhat), accuracy(ytrue,yhat2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "paths= viterbi(obs2_windowed,Modelo2,Modelo2.PI)\n",
    "path=paths[int(-len(paths)/2):]\n",
    "path_bigdelay = paths[:int(-len(paths)/2)]\n",
    "cords= path_to_xy(path,cc)\n",
    "cords_bigdelay = path_to_xy(path_bigdelay,cc)\n",
    "cords1=path_to_xy(est_windowed.T[int(-len(paths)/2):],cc)\n",
    "cords1delay=path_to_xy(est_windowed.T[:int(-len(paths)/2)],cc)\n",
    "\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Test Run Buf Vit=30', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.0313)\n",
    "\n",
    "ax1.scatter(x=cords.xcuant, y=cords.ycuant, marker='o',c='r',s=52,alpha=.5 )\n",
    "ax1.scatter(x=cords_bigdelay.xcuant, y=cords_bigdelay.ycuant, marker='+',c='r',s=52,alpha=.1 )\n",
    "ax1.scatter(x=cords1.xcuant, y=cords1.ycuant, marker='o',c='c' ,alpha=.5)\n",
    "ax1.scatter(x=cords1delay.xcuant, y=cords1delay.ycuant, marker='+',c='c' ,alpha=.1)\n",
    "\n",
    "\n",
    "\n",
    "ax1.scatter(x= odomreal.x[int(-len(paths)/2):], y=odomreal.y[int(-len(paths)/2):],marker='.',s=13,c='y')\n",
    "ax1.scatter(x= odomreal.x[:int(-len(paths)/2)], y=odomreal.y[:int(-len(paths)/2)],marker='.',s=13,c='m')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Training Set Pose', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.3)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Training Set KMEANS', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Vk)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Training Set Vk = AFF PROP', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Vk_aff)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ccxyth=cc\n",
    "graphe= Markov_A_2_grafo(A,ccxyth)\n",
    "ruta=dijkstra(1,6,graphe)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def Markov_A_2_grafo(A,ccxyth):\n",
    "    dists=np.zeros(A.shape)\n",
    "    for i in range(A.shape[0]):\n",
    "        for j in range (A.shape[1]):\n",
    "            if A[i,j]!=0 :\n",
    "                dists[i,j]= np.linalg.norm(ccxyth[i]-ccxyth[j])    \n",
    "    \n",
    "    \n",
    "    con = np.where(dists==0,np.inf,dists)\n",
    "    graphe2=grafo(ccxyth,con)\n",
    "    return graphe2\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "graphe= Markov_A_2_grafo(A,ccxyth)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#ruta.append(nodofinal)\n",
    "path=np.array(ruta)\n",
    "path\n",
    "cords_path= path_to_xy(path,cc)\n",
    "#cords_path= path_to_xy(path[:int(len(path)/2)],cc)\n",
    "\n",
    "cords_path2= path_to_xy(path,cc)\n",
    "\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.0313)\n",
    "\n",
    "ax1.scatter(x=cords_path.xcuant, y=cords_path.ycuant, marker='o',c='r',s=52,alpha=.5 )\n",
    "ax1.scatter(x=cords_path2.xcuant, y=cords_path2.ycuant, marker='o',c='g',s=52 )\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def dijkstra(nodoinicial,nodofinal,graphe):\n",
    "    if (nodofinal > graphe.nodos.shape[0]):\n",
    "        print ('Wrong dimensions for requested node')\n",
    "        return None\n",
    "\n",
    "    numnodos= len(graphe.nodos)\n",
    "    con = graphe.conec\n",
    "    D= np.ones(numnodos)*np.inf\n",
    "    Prv= np.ones(numnodos)*np.inf\n",
    "    V= np.zeros(numnodos)\n",
    "    a = nodoinicial\n",
    "    D[a]=0\n",
    "    Prv[a]=0\n",
    "    Prv[np.where(con[a]!=np.inf)]=a\n",
    "    V[a]=1\n",
    "    Dacc=D[a]\n",
    "    ########\n",
    "    D=np.minimum(D,con[a]+D[a])\n",
    "    cont=0\n",
    "    sucess=False\n",
    "    while(sucess==False):\n",
    "        a = np.argmin(D+np.where (V==1,np.inf, V))\n",
    "        Dacc=D[a]\n",
    "        Prv[np.where(D>(con[a]+Dacc) )]=a\n",
    "        V[a]=1\n",
    "        D=np.minimum(D,con[a]+Dacc)\n",
    "        if (a== nodofinal):\n",
    "            print(\"RUTA CALCULADA \")\n",
    "            sucess=True\n",
    "    rutainv=[]\n",
    "    rutainv.append(nodofinal)\n",
    "    while(rutainv[-1]!=nodoinicial):\n",
    "        prv=Prv[int(rutainv[-1])]\n",
    "        rutainv.append(prv)\n",
    "\n",
    "    ruta=[]\n",
    "    for n in reversed(rutainv):\n",
    "        ruta.append(n)\n",
    "    return(ruta)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path=np.array(ruta)\n",
    "\n",
    "\n",
    "cords_path= path_to_xy(path,cc)\n",
    "#cords_path= path_to_xy(path[:int(len(path)/2)],cc)\n",
    "#cords_path2= path_to_xy(path[int(len(path)/2):],cc)\n",
    "\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= data.x, y=data.y,marker='+',c=data.Y,alpha=.0313)\n",
    "\n",
    "ax1.scatter(x=cords_path.xcuant, y=cords_path.ycuant, marker='o',c='r',s=52,alpha=.5 )\n",
    "#ax1.scatter(x=cords_path2.xcuant, y=cords_path2.ycuant, marker='o',c='g',s=52,alpha=.5 )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#lec=np.load('lec.npy')\n",
    "aux=pd.DataFrame(data.Vk.value_counts())\n",
    "\n",
    "\n",
    "\n",
    "lec_deseada=aux.index[0]   ###CHOOSING MOST FREQUENT OBSERVATION SYMBOL\n",
    "\n",
    "lec=data[data.Vk==lec_deseada].sample(n=1,axis=0).iloc[:,:-6].values.ravel()\n",
    "\n",
    "\n",
    "start_ang=-240/2*np.pi/180\n",
    "stop_ang= 240/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(lec))\n",
    "lec.shape,lec_deseada"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cordsy, cordsx= np.cos(angs) *  lec ,  np.sin(angs) *  lec\n",
    "CORDS=pd.DataFrame((cordsx,cordsy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "print(np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin())\n",
    "lec_leida= np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()\n",
    "vec_obs=ccvk[np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()]\n",
    "cordsy, cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "CORDS['xx'],CORDS['yy']=cordsx,cordsy\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between real read and cuantized', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x=CORDS.xx,y=CORDS.yy,marker='*')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "lecs=data[data.Vk==lec_leida]\n",
    "lecs.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "auxy , auxx =np.array([0,0]),np.array([0,0])\n",
    "start_ang=-250/2*np.pi/180\n",
    "stop_ang= 250/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(vec_obs))\n",
    "\n",
    "\n",
    "lecs=data[data.Vk==lec_leida].sample(n=10,axis=0).iloc[:,:-6]\n",
    "\n",
    "for  vec_obs in lecs.values.tolist():\n",
    "    cordsy,cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "    auxy,auxx=np.concatenate((auxy,cordsy)),np.concatenate((auxx,cordsx))\n",
    "print(lec_leida)    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "CORDS=pd.DataFrame((auxx,auxy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "vec_obs=ccvk[np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()]\n",
    "cordsy, cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "\n",
    "CORDSVK=pd.DataFrame()\n",
    "CORDSVK['xx'],CORDSVK['yy']=cordsx,cordsy\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between real10 random reads and their cuantized versions', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x=CORDSVK.xx,y=CORDSVK.yy,marker='*')\n",
    "print(lec_leida)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#lec=np.load('lec.npy')\n",
    "aux=pd.DataFrame(data.Vk.value_counts())\n",
    "\n",
    "\n",
    " \n",
    "lec_deseada=aux.index[4]   ###CHOOSING 5th MOST FREQUENT OBSERVATION SYMBOL\n",
    "\n",
    "lec=data[data.Vk==lec_deseada].sample(n=1,axis=0).iloc[:,:-6].values.ravel()\n",
    "\n",
    "\n",
    "start_ang=-240/2*np.pi/180\n",
    "stop_ang= 240/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(lec))\n",
    "lec.shape,lec_deseada"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cordsy, cordsx= np.cos(angs) *  lec ,  np.sin(angs) *  lec\n",
    "CORDS=pd.DataFrame((cordsx,cordsy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "print(np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin())\n",
    "lec_leida= np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()\n",
    "vec_obs=ccvk[np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()]\n",
    "cordsy, cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "CORDS['xx'],CORDS['yy']=cordsx,cordsy\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between real and quantized reads KMEANS', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x=CORDS.xx,y=CORDS.yy,marker='*')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "auxy , auxx =np.array([0,0]),np.array([0,0])\n",
    "start_ang=-250/2*np.pi/180\n",
    "stop_ang= 250/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(vec_obs))\n",
    "\n",
    "\n",
    "lecs=data[data.Vk==lec_leida].sample(n=10,axis=0 , replace=True).iloc[:,:-6]\n",
    "\n",
    "for  vec_obs in lecs.values.tolist():\n",
    "    cordsy,cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "    auxy,auxx=np.concatenate((auxy,cordsy)),np.concatenate((auxx,cordsx))\n",
    "print(lec_leida)    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "CORDS=pd.DataFrame((auxx,auxy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "vec_obs=ccvk[np.power(lec.T-ccvk,2).sum(axis=1,keepdims=True).argmin()]\n",
    "cordsy, cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "\n",
    "CORDSVK=pd.DataFrame()\n",
    "CORDSVK['xx'],CORDSVK['yy']=cordsx,cordsy\n",
    "\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between10 random reads and their cuantized version ' , fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x=CORDSVK.xx,y=CORDSVK.yy,marker='*')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "############################################################################################\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#lec=np.load('lec.npy')\n",
    "aux=pd.DataFrame(data.Vk_aff.value_counts())\n",
    "\n",
    "\n",
    "\n",
    "lec_deseada=aux.index[0]   ###CHOOSING MOST FREQUENT OBSERVATION SYMBOL\n",
    "\n",
    "lec=data[data.Vk_aff==lec_deseada].sample(n=1,axis=0).iloc[:,:-6].values.ravel()\n",
    "\n",
    "\n",
    "start_ang=-240/2*np.pi/180\n",
    "stop_ang= 240/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(lec))\n",
    "lec.shape,lec_deseada"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cordsy, cordsx= np.cos(angs) *  lec ,  np.sin(angs) *  lec\n",
    "CORDS=pd.DataFrame((cordsx,cordsy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "print(\"COMPARING AFF PROP READINGS NO centroid\")\n",
    "\n",
    "lec_leida= (int)( clf.predict(lec.reshape(1, -1)))\n",
    "print(lec_leida)\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "\n",
    "CORDS_ORIG = pd.DataFrame((cordsx,cordsy))\n",
    "CORDS_ORIG=CORDS_ORIG.T\n",
    "CORDS_ORIG.columns=['x','y']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "auxy , auxx =np.array([0,0]),np.array([0,0])\n",
    "start_ang=-250/2*np.pi/180\n",
    "stop_ang= 250/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(vec_obs))\n",
    "\n",
    "\n",
    "lecs=data[data.Vk_aff==lec_leida].sample(n=20,axis=0).iloc[:,:-6]\n",
    "\n",
    "for  vec_obs in lecs.values.tolist():\n",
    "    cordsy,cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "    auxy,auxx=np.concatenate((auxy,cordsy)),np.concatenate((auxx,cordsx))\n",
    "print(lec_leida)    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "CORDS=pd.DataFrame((auxx,auxy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between real 20 random reads and their cuantized versions AFF_PROP', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x= CORDS_ORIG.x, y=CORDS_ORIG.y,marker='+')\n",
    "\n",
    "print(lec_leida)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#lec=np.load('lec.npy')\n",
    "aux=pd.DataFrame(data.Vk_aff.value_counts())\n",
    "\n",
    "\n",
    "\n",
    "lec_deseada=aux.index[3]   ###CHOOSING 3rd MOST FREQUENT OBSERVATION SYMBOL\n",
    "\n",
    "lec=data[data.Vk_aff==lec_deseada].sample(n=1,axis=0).iloc[:,:-6].values.ravel()\n",
    "\n",
    "\n",
    "start_ang=-240/2*np.pi/180\n",
    "stop_ang= 240/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(lec))\n",
    "lec.shape,lec_deseada"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cordsy, cordsx= np.cos(angs) *  lec ,  np.sin(angs) *  lec\n",
    "CORDS=pd.DataFrame((cordsx,cordsy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "print(\"COMPARING AFF PROP READINGS NO centroid\")\n",
    "\n",
    "lec_leida= (int)( clf.predict(lec.reshape(1, -1)))\n",
    "print(lec_leida)\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "CORDS_ORIG = pd.DataFrame((cordsx,cordsy))\n",
    "CORDS_ORIG=CORDS_ORIG.T\n",
    "CORDS_ORIG.columns=['x','y']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "auxy , auxx =np.array([0,0]),np.array([0,0])\n",
    "start_ang=-250/2*np.pi/180\n",
    "stop_ang= 250/2*np.pi/180\n",
    "angs=np.linspace(start_ang,stop_ang,num=len(vec_obs))\n",
    "\n",
    "\n",
    "lecs=data[data.Vk_aff==lec_leida].sample(n=20,axis=0).iloc[:,:-6]\n",
    "\n",
    "for  vec_obs in lecs.values.tolist():\n",
    "    cordsy,cordsx= np.cos(angs) *  vec_obs ,  np.sin(angs) *  vec_obs\n",
    "    auxy,auxx=np.concatenate((auxy,cordsy)),np.concatenate((auxx,cordsx))\n",
    "print(lec_leida)    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "CORDS=pd.DataFrame((auxx,auxy))\n",
    "CORDS=CORDS.T\n",
    "CORDS.columns=['x','y']\n",
    "\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Comparison Between real 20 random reads and their cuantized versions AFF_PROP', fontsize=16)\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+')\n",
    "ax1.scatter(x= CORDS_ORIG.x, y=CORDS_ORIG.y,marker='+')\n",
    "\n",
    "print(lec_leida)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data.Vk.hist()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data.Vk_aff.hist()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aux=pd.DataFrame(data.Vk.value_counts())\n",
    "xy=data[['x','y','Vk','Y']][data.Vk==(aux.index[0]  ) ]\n",
    "CORDS=xy\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+', c=CORDS.Y),xy.describe()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aux=pd.DataFrame(data.Vk.value_counts())\n",
    "xy=data[['x','y','theta','Vk','Y']][data.Vk==aux.index[0]]\n",
    "xy= xy.append(data[['x','y','theta','Vk','Y']][data.Vk==aux.index[1]])\n",
    "xy= xy.append(data[['x','y','theta','Vk','Y']][data.Vk==aux.index[2]])\n",
    "\n",
    "\n",
    "CORDS=xy\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y, c=CORDS.Vk,marker='+',cmap='Set1'),xy.describe()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aux=pd.DataFrame(data.Vk_aff.value_counts())\n",
    "xy=data[['x','y','Vk_aff','Y']][data.Vk_aff==(aux.index[0]  ) ]\n",
    "CORDS=xy\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y,marker='+', c=CORDS.Y),xy.describe()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aux=pd.DataFrame(data.Vk_aff.value_counts())\n",
    "xy=data[['x','y','theta','Vk_aff','Y']][data.Vk_aff==aux.index[0]]\n",
    "xy= xy.append(data[['x','y','theta','Vk_aff','Y']][data.Vk_aff==aux.index[1]])\n",
    "xy= xy.append(data[['x','y','theta','Vk_aff','Y']][data.Vk_aff==aux.index[2]])\n",
    "\n",
    "\n",
    "CORDS=xy\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "\n",
    "ax1 = fig.add_subplot(111)\n",
    "ax1.scatter(x= CORDS.x, y=CORDS.y, c=CORDS.Vk_aff,marker='+',cmap='Set1'),xy.describe()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aux=pd.DataFrame(data.Vk.value_counts())\n",
    "cents=[]\n",
    "for i in aux.index[:3]:\n",
    "    print (data[data.Vk==i][['x','y','theta']].describe())\n",
    "    cents.append(data[data.Vk==i][['x','y','theta']].mean(axis=0))\n",
    "\n",
    "aux=pd.DataFrame(data.Vk_aff.value_counts())\n",
    "cents_aff=[]\n",
    "for i in aux.index[:3]:\n",
    "    print (data[data.Vk_aff==i][['x','y','theta']].describe())\n",
    "    cents_aff.append(data[data.Vk_aff==i][['x','y','theta']].mean(axis=0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cents_aff=np.asarray(cents_aff)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cents_aff"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cents"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_lec(lec):\n",
    "    cordsy, cordsx= np.cos(angs) *  lec ,  np.sin(angs) *  lec\n",
    "    CORDS=pd.DataFrame((cordsx,cordsy))\n",
    "    CORDS=CORDS.T\n",
    "    CORDS.columns=['x','y']\n",
    "    plt.scatter(CORDS.x , CORDS.y)\n",
    "    return CORDS.T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "lec= data.iloc[1000,0:721].values\n",
    "lec1= data.iloc[1111,0:721].values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy=plot_lec(lec1).values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy_ref=plot_lec(lec).values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_data(data_1, data_2, label_1, label_2, markersize_1=8, markersize_2=8):\n",
    "    fig = plt.figure(figsize=(10, 6))\n",
    "    ax = fig.add_subplot(111)\n",
    "    ax.axis('equal')\n",
    "    if data_1 is not None:\n",
    "        x_p, y_p = data_1\n",
    "        ax.plot(x_p, y_p, color='#336699', markersize=markersize_1, marker='o', linestyle=\":\", label=label_1)\n",
    "    if data_2 is not None:\n",
    "        x_q, y_q = data_2\n",
    "        ax.plot(x_q, y_q, color='orangered', markersize=markersize_2, marker='o', linestyle=\":\", label=label_2)\n",
    "    ax.legend()\n",
    "    return ax\n",
    "\n",
    "def plot_values(values, label):\n",
    "    fig = plt.figure(figsize=(10, 4))\n",
    "    ax = fig.add_subplot(111)\n",
    "    ax.plot(values, label=label)\n",
    "    ax.legend()\n",
    "    ax.grid(True)\n",
    "    plt.show()\n",
    "    \n",
    "def animate_results(P_values, Q, corresp_values, xlim, ylim):\n",
    "    \"\"\"A function used to animate the iterative processes we use.\"\"\"\n",
    "    fig = plt.figure(figsize=(10, 6))\n",
    "    anim_ax = fig.add_subplot(111)\n",
    "    anim_ax.set(xlim=xlim, ylim=ylim)\n",
    "    anim_ax.set_aspect('equal')\n",
    "    plt.close()\n",
    "    x_q, y_q = Q\n",
    "    # draw initial correspondeces\n",
    "    corresp_lines = []\n",
    "    for i, j in correspondences:\n",
    "        corresp_lines.append(anim_ax.plot([], [], 'grey')[0])\n",
    "    # Prepare Q data.\n",
    "    Q_line, = anim_ax.plot(x_q, y_q, 'o', color='orangered')\n",
    "    # prepare empty line for moved data\n",
    "    P_line, = anim_ax.plot([], [], 'o', color='#336699')\n",
    "\n",
    "    def animate(i):\n",
    "        P_inc = P_values[i]\n",
    "        x_p, y_p = P_inc\n",
    "        P_line.set_data(x_p, y_p)\n",
    "        draw_inc_corresp(P_inc, Q, corresp_values[i])\n",
    "        return (P_line,)\n",
    "    \n",
    "    def draw_inc_corresp(points_from, points_to, correspondences):\n",
    "        for corr_idx, (i, j) in enumerate(correspondences):\n",
    "            x = [points_from[0, i], points_to[0, j]]\n",
    "            y = [points_from[1, i], points_to[1, j]]\n",
    "            corresp_lines[corr_idx].set_data(x, y)\n",
    "    \n",
    "    anim = animation.FuncAnimation(fig, animate,\n",
    "                                   frames=len(P_values), \n",
    "                                   interval=500, \n",
    "                                   blit=True)\n",
    "    return HTML(anim.to_jshtml())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "plot_data(xy, xy_ref, \"new read:P \", \"Q: reference\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "plot_data(xy, xy_ref, \"P: moved data\", \"Q: true data\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_correspondence_indices(P, Q):\n",
    "    \"\"\"For each point in P find closest one in Q.\"\"\"\n",
    "    p_size = P.shape[1]\n",
    "    q_size = Q.shape[1]\n",
    "    correspondences = []\n",
    "    for i in range(p_size):\n",
    "        p_point = P[:, i]\n",
    "        min_dist = sys.maxsize\n",
    "        chosen_idx = -1\n",
    "        for j in range(q_size):\n",
    "            q_point = Q[:, j]\n",
    "            dist = np.linalg.norm(q_point - p_point)\n",
    "            if dist < min_dist:\n",
    "                min_dist = dist\n",
    "                chosen_idx = j\n",
    "        correspondences.append((i, chosen_idx))\n",
    "    return correspondences\n",
    "\n",
    "def draw_correspondeces(P, Q, correspondences, ax):\n",
    "    label_added = False\n",
    "    for i, j in correspondences:\n",
    "        x = [P[0, i], Q[0, j]]\n",
    "        y = [P[1, i], Q[1, j]]\n",
    "        if not label_added:\n",
    "            ax.plot(x, y, color='grey', label='correpondences')\n",
    "            label_added = True\n",
    "        else:\n",
    "            ax.plot(x, y, color='grey')\n",
    "    ax.legend()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def center_data(data, exclude_indices=[]):\n",
    "    reduced_data = np.delete(data, exclude_indices, axis=1)\n",
    "    center = np.array([reduced_data.mean(axis=1)]).T\n",
    "    return center, data - center\n",
    "\n",
    "center_of_P, P_centered = center_data(xy)\n",
    "center_of_Q, Q_centered = center_data(xy_ref)\n",
    "ax = plot_data(P_centered, Q_centered,\n",
    "               label_1='Moved data centered',\n",
    "               label_2='True data centered')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(P_centered, Q_centered)\n",
    "ax = plot_data(P_centered, Q_centered,\n",
    "               label_1='P centered',\n",
    "               label_2='Q centered')\n",
    "draw_correspondeces(P_centered, Q_centered, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_cross_covariance(P, Q, correspondences, kernel=lambda diff: 1.0):\n",
    "    cov = np.zeros((2, 2))\n",
    "    exclude_indices = []\n",
    "    for i, j in correspondences:\n",
    "        p_point = P[:, [i]]\n",
    "        q_point = Q[:, [j]]\n",
    "        weight = kernel(p_point - q_point)\n",
    "        if weight < 0.01: exclude_indices.append(i)\n",
    "        cov += weight * q_point.dot(p_point.T)\n",
    "    return cov, exclude_indices\n",
    "\n",
    "cov, _ = compute_cross_covariance(P_centered, Q_centered, correspondences)\n",
    "print(cov)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_cross_covariance(P, Q, correspondences):\n",
    "    cov = np.zeros((2, 2))\n",
    "    exclude_indices = []\n",
    "    for i, j in correspondences:\n",
    "        p_point = P[:, [i]]\n",
    "        q_point = Q[:, [j]]\n",
    "        \n",
    "        cov +=  q_point.dot(p_point.T)\n",
    "        \n",
    "    return cov, exclude_indices\n",
    "\n",
    "cov, _ = compute_cross_covariance(P_centered, Q_centered, correspondences)\n",
    "print(cov)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "U, S, V_T = np.linalg.svd(cov)\n",
    "print(S)\n",
    "R_found = U.dot(V_T)\n",
    "t_found = center_of_Q - R_found.dot(center_of_P)\n",
    "print(\"R_found =\\n\", R_found)\n",
    "print(\"t_found =\\n\", t_found)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(t_found)\n",
    "print(R_found)\n",
    "P_corrected = R_found.dot(xy) + t_found\n",
    "ax = plot_data(P_corrected, xy_ref, label_1='P corrected', label_2='Q')\n",
    "plt.show()\n",
    "print(\"Squared diff: (P_corrected - Q) = \", np.linalg.norm(P_corrected - xy_ref))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "plot_data(xy, xy_ref, \"P: moved data\", \"Q: true data\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy=P_corrected"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def icp_svd(P, Q, iterations=10, kernel=lambda diff: 1.0):\n",
    "    \"\"\"Perform ICP using SVD.\"\"\"\n",
    "    center_of_Q, Q_centered = center_data(Q)\n",
    "    norm_values = []\n",
    "    P_values = [P.copy()]\n",
    "    P_copy = P.copy()\n",
    "    corresp_values = []\n",
    "    exclude_indices = []\n",
    "    for i in range(iterations):\n",
    "        center_of_P, P_centered = center_data(P_copy, exclude_indices=exclude_indices)\n",
    "        correspondences = get_correspondence_indices(P_centered, Q_centered)\n",
    "        corresp_values.append(correspondences)\n",
    "        norm_values.append(np.linalg.norm(P_centered - Q_centered))\n",
    "        cov, exclude_indices = compute_cross_covariance(P_centered, Q_centered, correspondences)\n",
    "        U, S, V_T = np.linalg.svd(cov)\n",
    "        R = U.dot(V_T)  \n",
    "        t = center_of_Q - R.dot(center_of_P)  \n",
    "        P_copy = R.dot(P_copy) + t\n",
    "        P_values.append(P_copy)\n",
    "    corresp_values.append(corresp_values[-1])\n",
    "    return P_values, norm_values, corresp_values\n",
    "\n",
    "P_values, norm_values, corresp_values = icp_svd(xy, xy_ref)\n",
    "plot_values(norm_values, label=\"Squared diff P->Q\")\n",
    "ax = plot_data(P_values[-1], xy_ref, label_1='P final', label_2='Q', markersize_1=15)\n",
    "plt.show()\n",
    "print(norm_values)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(xy, xy_ref)\n",
    "ax = plot_data(xy, xy_ref, \"Moved data\", \"True data\")\n",
    "draw_correspondeces(xy, xy_ref, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dR(theta):\n",
    "    return np.array([[-np.sin(theta), -np.cos(theta)],\n",
    "                     [np.cos(theta),  -np.sin(theta)]])\n",
    "\n",
    "def R(theta):\n",
    "    return np.array([[np.cos(theta), -np.sin(theta)],\n",
    "                     [np.sin(theta),  np.cos(theta)]]).reshape((2,2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def jacobian(x, p_point):\n",
    "    theta = x[2]\n",
    "    J = np.zeros((2, 3))\n",
    "    J[0:2, 0:2] = np.identity(2)\n",
    "    J[0:2, [2]] = dR(0).dot(p_point)\n",
    "    return J\n",
    "\n",
    "def error(x, p_point, q_point):\n",
    "    rotation = R(x[2])\n",
    "    translation = x[0:2]\n",
    "    prediction = rotation.dot(p_point) + translation\n",
    "    return prediction - q_point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def prepare_system(x, P, Q, correspondences, kernel=lambda distance: 1.0):\n",
    "    H = np.zeros((3, 3))\n",
    "    g = np.zeros((3, 1))\n",
    "    chi = 0\n",
    "    for i, j in correspondences:\n",
    "        p_point = P[:, [i]]\n",
    "        q_point = Q[:, [j]]\n",
    "        e = error(x, p_point, q_point)\n",
    "        weight = kernel(e) # Please ignore this weight until you reach the end of the notebook.\n",
    "        J = jacobian(x, p_point)\n",
    "        H += weight * J.T.dot(J)\n",
    "        g += weight * J.T.dot(e)\n",
    "        chi += e.T * e\n",
    "    return H, g, chi\n",
    "\n",
    "def icp_least_squares(P, Q, iterations=30, kernel=lambda distance: 1.0):\n",
    "    x = np.zeros((3, 1))\n",
    "    chi_values = []\n",
    "    x_values = [x.copy()]  # Initial value for transformation.\n",
    "    P_values = [P.copy()]\n",
    "    P_copy = P.copy()\n",
    "    corresp_values = []\n",
    "    for i in range(iterations):\n",
    "        rot = R(x[2])\n",
    "        t = x[0:2]\n",
    "        correspondences = get_correspondence_indices(P_copy, Q)\n",
    "        corresp_values.append(correspondences)\n",
    "        H, g, chi = prepare_system(x, P, Q, correspondences, kernel)\n",
    "        dx = np.linalg.lstsq(H, -g, rcond=None)[0]\n",
    "        x += dx\n",
    "        x[2] = np.arctan2(np.sin(x[2]), np.cos(x[2])) # normalize angle\n",
    "        chi_values.append(chi.item(0))\n",
    "        x_values.append(x.copy())\n",
    "        rot = R(x[2])\n",
    "        t = x[0:2]\n",
    "        P_copy = rot.dot(P.copy()) + t\n",
    "        P_values.append(P_copy)\n",
    "    corresp_values.append(corresp_values[-1])\n",
    "    return P_values, chi_values, corresp_values\n",
    "\n",
    "P_values, chi_values, corresp_values = icp_least_squares(xy, xy_ref)\n",
    "plot_values(chi_values, label=\"chi^2\")\n",
    "print(chi_values)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(xy, xy_ref)\n",
    "ax = plot_data(xy, xy_ref, \"Moved data\", \"True data\")\n",
    "draw_correspondeces(xy, xy_ref, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(P_values[-1], xy_ref)\n",
    "ax = plot_data(P_values[-1], xy_ref, \"Moved data\", \"True data\")\n",
    "draw_correspondeces(P_values[-1], xy_ref, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_normals(points, step=1):\n",
    "    normals = [np.array([[0, 0]])]\n",
    "    normals_at_points = []\n",
    "    for i in range(step, points.shape[1] - step):\n",
    "        prev_point = points[:, i - step]\n",
    "        next_point = points[:, i + step]\n",
    "        curr_point = points[:, i]\n",
    "        dx = next_point[0] - prev_point[0] \n",
    "        dy = next_point[1] - prev_point[1]\n",
    "        normal = np.array([[0, 0],[-dy, dx]])\n",
    "        normal = normal / np.linalg.norm(normal)\n",
    "        normals.append(normal[[1], :])  \n",
    "        normals_at_points.append(normal + curr_point)\n",
    "    normals.append(np.array([[0, 0]]))\n",
    "    return normals, normals_at_points\n",
    "\n",
    "def plot_normals(normals, ax):\n",
    "    label_added = False\n",
    "    for normal in normals:\n",
    "        if not label_added:\n",
    "            ax.plot(normal[:,0], normal[:,1], color='grey', label='normals')\n",
    "            label_added = True\n",
    "        else:\n",
    "            ax.plot(normal[:,0], normal[:,1], color='grey')\n",
    "    ax.legend()\n",
    "    return ax\n",
    "\n",
    "Q_normals, Q_normals_to_draw = compute_normals(xy_ref)\n",
    "ax = plot_data(None, xy_ref, None, 'Q')\n",
    "ax = plot_normals(Q_normals_to_draw, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import init_printing, symbols, Matrix, cos as s_cos, sin as s_sin, diff\n",
    "init_printing(use_unicode = True)\n",
    "\n",
    "def RotationMatrix(angle):\n",
    "    return Matrix([[s_cos(angle) , -s_sin(angle)], [s_sin(angle), s_cos(angle)]])\n",
    "\n",
    "x, y, theta, n_x, n_y, p_x, p_y = symbols('x, y, \\\\theta, n_x, n_y, p_x, p_y')\n",
    "t = Matrix([[x], [y]])\n",
    "X = Matrix([x,y,theta])\n",
    "n = Matrix([[n_x],[n_y]])\n",
    "p = Matrix([[p_x], [p_y]])\n",
    "\n",
    "error_point = RotationMatrix(theta) * p + t\n",
    "error_normal = n.dot(RotationMatrix(theta) * p + t)\n",
    "\n",
    "display()\n",
    "J_point = diff(error_point, X).reshape(3,2).transpose()\n",
    "J_normal = diff(error_normal, X).reshape(3,1).transpose()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import init_printing, symbols, Matrix, cos as s_cos, sin as s_sin, diff\n",
    "init_printing(use_unicode = True)\n",
    "\n",
    "def RotationMatrix(angle):\n",
    "    return Matrix([[s_cos(angle) , -s_sin(angle)], [s_sin(angle), s_cos(angle)]])\n",
    "\n",
    "x, y, theta, n_x, n_y, p_x, p_y = symbols('x, y, \\\\theta, n_x, n_y, p_x, p_y')\n",
    "t = Matrix([[x], [y]])\n",
    "X = Matrix([x,y,theta])\n",
    "n = Matrix([[n_x],[n_y]])\n",
    "p = Matrix([[p_x], [p_y]])\n",
    "\n",
    "error_point = RotationMatrix(theta) * p + t\n",
    "error_normal = n.dot(RotationMatrix(theta) * p + t)\n",
    "\n",
    "display()\n",
    "J_point = diff(error_point, X).reshape(3,2).transpose()\n",
    "J_normal = diff(error_normal, X).reshape(3,1).transpose()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def prepare_system_normals(x, P, Q, correspondences, Q_normals):\n",
    "    H = np.zeros((3, 3))\n",
    "    g = np.zeros((3, 1))\n",
    "    chi = 0\n",
    "    for i, j in correspondences:\n",
    "        p_point = P[:, [i]]\n",
    "        q_point = Q[:, [j]]\n",
    "        normal = Q_normals[j]\n",
    "        e = normal.dot(error(x, p_point, q_point))\n",
    "        J = normal.dot(jacobian(x, p_point))\n",
    "        H += J.T.dot(J)\n",
    "        g += J.T.dot(e)\n",
    "        chi += e.T * e\n",
    "    return H, g, chi\n",
    "\n",
    "def icp_normal(P, Q, Q_normals, iterations=4):\n",
    "    x = np.zeros((3, 1))\n",
    "    chi_values = []\n",
    "    x_values = [x.copy()]  # Initial value for transformation.\n",
    "    P_values = [P.copy()]\n",
    "    P_latest = P.copy()\n",
    "    corresp_values = []\n",
    "    for i in range(iterations):\n",
    "        rot = R(x[2])\n",
    "        t = x[0:2]\n",
    "        correspondences = get_correspondence_indices(P_latest, Q)\n",
    "        corresp_values.append(correspondences)\n",
    "        H, g, chi = prepare_system_normals(x, P, Q, correspondences, Q_normals)\n",
    "        dx = np.linalg.lstsq(H, -g, rcond=None)[0]\n",
    "        x += dx\n",
    "        x[2] = np.arctan2(np.sin(x[2]), np.cos(x[2])) # normalize angle\n",
    "        chi_values.append(chi.item(0)) # add error to list of errors\n",
    "        x_values.append(x.copy())\n",
    "        rot = R(x[2])\n",
    "        t = x[0:2]\n",
    "        P_latest = rot.dot(P.copy()) + t\n",
    "        P_values.append(P_latest)\n",
    "    corresp_values.append(corresp_values[-1])\n",
    "    return P_values, chi_values, corresp_values\n",
    "\n",
    "P_values, chi_values, corresp_values = icp_normal(xy, xy_ref, Q_normals)\n",
    "plot_values(chi_values, label=\"chi^2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "P_values[-1].shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(P_values[-1], xy_ref)\n",
    "ax = plot_data(P_values[-1], xy_ref, \"Moved data\", \"True data\")\n",
    "draw_correspondeces(P_values[-1], xy_ref, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "P_values, chi_values, corresp_values = icp_normal(xy, xy_ref, Q_normals)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "def plot_cc_arrow(cc):\n",
    "    for c in cc:\n",
    "        ang=c[2]*180/math.pi\n",
    "        plt.plot(c[0], c[1], marker=(3, 1,ang+.5*math.pi ), markersize=10,c='green', linestyle='None')\n",
    "        plt.plot(c[0]-(.01*np.cos(c[2]+math.pi/2)), c[1]-(.01*np.sin(c[2]+math.pi/2)), marker=(1, 1,ang+.5*math.pi), markersize=25,c='green', linestyle='--')\n",
    "\n",
    "    #plt.xlim([0,4])\n",
    "    #plt.ylim([0,4])\n",
    "\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_cc_arrow(cc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc=ccxyth\n",
    "\n",
    "\n",
    "\n",
    "x=np.linspace(0,.4)\n",
    "fig = plt.figure(figsize=(10,10))\n",
    "fig.suptitle('Centroids POSE', fontsize=16)\n",
    "euclD=np.zeros((N,N,1))\n",
    "ss= np.arange(len(A))\n",
    "for s1 in ss:\n",
    "    for s2 in ss:\n",
    "        if (s1!=s2)and (A[s1,s2]!=0):# and (np.linalg.norm(ccxyth[s1,:2]-ccxyth[s2,:2])<1)   :#and(s1==s or s2==s)\n",
    "            xvalues=[ccxyth[s1,0],ccxyth[s2,0]]\n",
    "            yvalues=[ccxyth[s1,1],ccxyth[s2,1]]\n",
    "            euclD[s1,s2]=np.linalg.norm(ccxyth[s1,:2]-ccxyth[s2,:2])\n",
    "\n",
    "            ax1 = fig.add_subplot(111)\n",
    "\n",
    "            plt.plot(xvalues,yvalues ,alpha=.4 ,c='b' )\n",
    "         \n",
    "            scatter= ax1.scatter(x= ccxyth[s1,0], y=ccxyth[s1,1],c='b' ,marker='.',s=100)\n",
    "            scatter= ax1.scatter(x= ccxyth[s2,0], y=ccxyth[s2,1],c='b' ,marker='.',s=100)\n",
    "            #scatter= ax1.scatter(x= trans[s1,s2,0], y=trans[s1,s2,1],c='b' ,marker='|',s=140)\n",
    "for c in cc:\n",
    "        ang=c[2]*180/math.pi\n",
    "        plt.plot(c[0], c[1], marker=(3, 1,ang - 90 ), markersize=20,c='green', linestyle='None')\n",
    "        plt.plot(c[0]-(.01*np.cos(c[2])), c[1]-(.01*np.sin(c[2])), marker=(1, 1,ang- 90), markersize=50,c='green', linestyle='--')\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.cos(cc[1,2]), np.sin(cc[1,2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc[1,2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "import math\n",
    "math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc=ccxyth.sort_values('norm').iloc[:,:3].values\n",
    "cc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc=np.load('ccxyth.npy')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "obs2=np.asarray([29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 29, 42, 42, 42, 42, 42, 42, 42, 23, 48, 48, 48, 48, 48, 48])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Modelo2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "viterbi(obs2[-20:-10],Modelo2,PI)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils_hmm import forw_alg\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "forw_alg(obs2[5:10],Modelo2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "obs2[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Modelo2.B.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Modelo2.B[4,:]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "buf_vit=150\n",
    "indice= int(np.random.rand()*(len(obs)-2*buf_vit))\n",
    "for i in range(1):\n",
    "    \n",
    "   \n",
    "    obs_windowed= obs[indice:indice+buf_vit]\n",
    "    obs2_windowed= obs_aff[indice:indice+buf_vit]\n",
    "    est_windowed= est[indice:indice+buf_vit]\n",
    "    odomreal= data[['x','y']].iloc[indice:indice+buf_vit]\n",
    "    #print(data[['x','y']].iloc[indice:indice+buf_vit],est_windowed)\n",
    "\n",
    "    print(indice)\n",
    "    #PATH from Model 1 KMEANS CC's obs\n",
    "    path= viterbi(obs_windowed,Modelo1,Modelo1.PI)\n",
    "    \n",
    "    ##PATH FROM MODEL 2 WICH NEEDS AFF PORP QUANT OBS\n",
    "    path2= viterbi(obs2_windowed,Modelo2,Modelo2.PI)\n",
    "    yhat=np.zeros(len(path)-1)\n",
    "    yhat2=np.zeros(len(path2)-1)\n",
    "    ytrue=np.zeros(len(path)-1)\n",
    "    for i in range(len(path)-1):\n",
    "        yhat[i]=int(path[i])\n",
    "        yhat2[i]=int(path2[i])\n",
    "        ytrue[i]=int(est_windowed[i])\n",
    "    print(accuracy(ytrue,yhat), accuracy(ytrue,yhat2))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "path, est_windowed, path2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_particles=10\n",
    "particles=np.zeros((3,num_particles))\n",
    "particles[:,0]=cc[np.argmax(Modelo2.B[:,obs2_windowed[0]])]\n",
    "particles[:,1]=cc[np.argmax(Modelo1.B[:,obs_windowed[0]])]\n",
    "particles[:,2]=cc[np.argmax(Modelo1.B[:,obs_windowed[-1]])]\n",
    "particles[:,3]=cc[np.argmax(Modelo2.B[:,obs2_windowed[-1]])]\n",
    "particles[:,4]=cc[int(path2[0])]\n",
    "particles[:,5]=cc[int(path2[-1])]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img = plt.imread(\"map.png\")\n",
    "fig, ax = plt.subplots(1,1,figsize=(10,10))\n",
    "x = range(300)\n",
    "ax.imshow(img, extent=[-0.3, 3.5,-1, 5])\n",
    "ax.scatter(x=particles[:,0],y=particles[:,1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#next Step\n",
    "indice+= buf_vit \n",
    "for i in range(1):\n",
    "    \n",
    "   \n",
    "    obs_windowed= obs[indice:indice+buf_vit]\n",
    "    obs2_windowed= obs_aff[indice:indice+buf_vit]\n",
    "    est_windowed= est[indice:indice+buf_vit]\n",
    "    odomreal= data[['x','y']].iloc[indice:indice+buf_vit]\n",
    "    #print(data[['x','y']].iloc[indice:indice+buf_vit],est_windowed)\n",
    "\n",
    "    print(indice)\n",
    "    #PATH from Model 1 KMEANS CC's obs\n",
    "    path= viterbi(obs_windowed,Modelo1,Modelo1.PI) #############################NO MANTAIN MODE\n",
    "    \n",
    "    ##PATH FROM MODEL 2 WICH NEEDS AFF PORP QUANT OBS\n",
    "    path2= viterbi(obs2_windowed,Modelo2,Modelo2.PI)###########################NO MANTAIN MODE \n",
    "    yhat=np.zeros(len(path)-1)\n",
    "    yhat2=np.zeros(len(path2)-1)\n",
    "    ytrue=np.zeros(len(path)-1)\n",
    "    for i in range(len(path)-1):\n",
    "        yhat[i]=int(path[i])\n",
    "        yhat2[i]=int(path2[i])\n",
    "        ytrue[i]=int(est_windowed[i])\n",
    "    print(accuracy(ytrue,yhat), accuracy(ytrue,yhat2))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "num_particles1=10\n",
    "particles1=np.zeros((3,num_particles1))\n",
    "particles1[:,0]=cc[np.argmax(Modelo2.B[:,obs2_windowed[0]])]\n",
    "particles1[:,1]=cc[np.argmax(Modelo1.B[:,obs_windowed[0]])]\n",
    "particles1[:,2]=cc[np.argmax(Modelo1.B[:,obs_windowed[-1]])]\n",
    "particles1[:,3]=cc[np.argmax(Modelo2.B[:,obs2_windowed[-1]])]\n",
    "particles1[:,4]=cc[int(path2[0])]\n",
    "particles1[:,5]=cc[int(path2[-1])]\n",
    "particles1[:,6]=cc[np.argmax(Modelo2.B[:,obs2_windowed[70]])]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "particles=np.concatenate((particles,particles1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img = plt.imread(\"map.png\")\n",
    "fig, ax = plt.subplots(1,1,figsize=(10,10))\n",
    "x = range(300)\n",
    "ax.imshow(img, extent=[-0.3, 3.5,-1, 5])\n",
    "ax.scatter(x=particles[:,0],y=particles[:,1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy_ref=plot_lec(ccvk[np.argmin(np.linalg.norm((lec-ccvk),axis=1))]).values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy=plot_lec(lec).values"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_normals, Q_normals_to_draw = compute_normals(xy_ref)\n",
    "ax = plot_data(None, xy_ref, None, 'Q')\n",
    "ax = plot_normals(Q_normals_to_draw, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "P_values, chi_values, corresp_values = icp_normal(xy, xy_ref, Q_normals)\n",
    "plot_values(chi_values, label=\"chi^2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "correspondences = get_correspondence_indices(P_values[-1], xy_ref)\n",
    "ax = plot_data(P_values[-1], xy_ref, \"Moved data\", \"True data\")\n",
    "draw_correspondeces(P_values[-1], xy_ref, correspondences, ax)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "xy_ref[0].shape"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "particles=np.zeros((3,len(ccxyth)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "particles=ccxyth[:,:2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img = plt.imread(\"map.png\")\n",
    "fig, ax = plt.subplots(1,1,figsize=(10,10))\n",
    "x = range(300)\n",
    "ax.imshow(img, extent=[-0.3, 3.5,-1, 5])\n",
    "ax.scatter(x=particles[:,0],y=particles[:,1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img = plt.imread(\"map.png\")\n",
    "fig, ax = plt.subplots(1,1,figsize=(10,10))\n",
    "x = range(300)\n",
    "#ax.imshow(img, extent=[-1, 3, -1.5, 2.5])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "tf_odapi",
   "language": "python",
   "name": "tf_odapi"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}