Tracking_hungarian.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Pixiedust database opened successfully\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "        <div style=\"margin:10px\">\n",
       "            <a href=\"https://github.com/ibm-watson-data-lab/pixiedust\" target=\"_new\">\n",
       "                <img src=\"https://github.com/ibm-watson-data-lab/pixiedust/raw/master/docs/_static/pd_icon32.png\" style=\"float:left;margin-right:10px\"/>\n",
       "            </a>\n",
       "            <span>Pixiedust version 1.1.18</span>\n",
       "        </div>\n",
       "        "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import os\n",
    "import sys\n",
    "import random\n",
    "import math\n",
    "from copy import copy, deepcopy\n",
    "import numpy as np\n",
    "import skimage.io\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2\n",
    "import scipy\n",
    "import pandas as pd\n",
    "import pixiedust\n",
    "from pathlib import Path\n",
    "# load numpy array from npy file\n",
    "from numpy import load\n",
    "from scipy.stats import multivariate_normal\n",
    "from scipy.optimize import linear_sum_assignment\n",
    "import heapq\n",
    "# save numpy array as npy file\n",
    "from numpy import asarray\n",
    "from numpy import save"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "# HERE IS WHERE I CHANGE VIDEO LOCATION AND TRACKER TYPE FOR SAVING AND LOADING DATA:\n",
    "# Specific video location\n",
    "Video = \"Short_Garden\"\n",
    "# Tracking type\n",
    "Tracker = \"_hung\"\n",
    "# NOW EVERYTHING ELSE GETS DONE AUTOMATICALLY\n",
    "\n",
    "# Root directory of the project\n",
    "ROOT_DIR = os.path.abspath(\"C:/Users/ddefr/OneDrive/Documents/Skripsie/Mask_RCNN\")\n",
    "\n",
    "# Directory for all videos\n",
    "all_vids = Path(\"C:/Users/ddefr/OneDrive/Documents/skripsie_videos\")\n",
    "\n",
    "#Specific video dir\n",
    "video_dir = all_vids / Video\n",
    "\n",
    "# Measurements to load:\n",
    "load_measurement = video_dir / \"measurements.npy\"\n",
    "\n",
    "# Result save locations:\n",
    "mu_save = os.path.join(video_dir,'mu{}.npy'.format(Tracker))\n",
    "cov_save = os.path.join(video_dir,'cov{}.npy'.format(Tracker))\n",
    "mu_obs_save = os.path.join(video_dir,'mu_obs{}.npy'.format(Tracker))\n",
    "cov_obs_save = os.path.join(video_dir,'cov_obs{}.npy'.format(Tracker))\n",
    "number_of_detections_save = os.path.join(video_dir,'number_of_detections{}.npy'.format(Tracker))\n",
    "\n",
    "# Import Mask RCNN\n",
    "sys.path.append(ROOT_DIR)  # To find local version of the library\n",
    "\n",
    "from mrcnn import utils\n",
    "import mrcnn.model as modellib\n",
    "from mrcnn import visualize\n",
    "# Import COCO config\n",
    "sys.path.append(os.path.join(ROOT_DIR, \"samples/coco/\"))  # To find local version\n",
    "import coco\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "pixiedust": {
     "displayParams": {}
    }
   },
   "outputs": [],
   "source": [
    "# load array\n",
    "measurement = load(load_measurement, allow_pickle=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Kalman Gain:\n",
    "def Kalman_Gain(X_cov_0, Y_transition, Ny_cov):\n",
    "    if np.all(X_cov_0 != None):\n",
    "        Kg = X_cov_0.dot(Y_transition.T).dot(np.linalg.inv(np.dot(Y_transition,X_cov_0).dot(Y_transition.T) + Ny_cov))\n",
    "    else:\n",
    "        Kg = None\n",
    "    return(Kg)\n",
    "\n",
    "#joint distribution:\n",
    "def joint_dis(cov1, mu1, Y_transition, Ny_cov):\n",
    "    if np.all(cov1 != None) and np.all(mu1 != None):\n",
    "        cov_new = np.array([[cov1, cov1.dot(Y_transition.T)],[Y_transition.dot(cov1.T), Y_transition.dot(cov1).dot(Y_transition.T) + Ny_cov]])\n",
    "        mu_new = np.array([[mu1],[Y_transition.dot(mu1)]])\n",
    "    else:\n",
    "        cov_new = None\n",
    "        mu_new = None\n",
    "    return(cov_new,mu_new)\n",
    "\n",
    "#observe evidence\n",
    "def observe(X_cov_0, X_mu_0, Y_transition, Ny_cov, y, Kg):\n",
    "    if np.all(X_cov_0 != None) and np.all(X_mu_0 != None):\n",
    "        X_cov_obs = X_cov_0 - Kg.dot(Y_transition).dot(X_cov_0)\n",
    "        X_mu_obs = X_mu_0 + Kg.dot(y - (Y_transition.dot(X_mu_0)))\n",
    "    else:\n",
    "        X_cov_obs = None\n",
    "        X_mu_obs = None\n",
    "    return(X_cov_obs, X_mu_obs)\n",
    "\n",
    "#Prediction\n",
    "def prediction(cov_obs, mu_obs, Dyn_T, Nx):\n",
    "    if np.all(cov_obs != None) and np.all(mu_obs != None):\n",
    "        cov_predict = Dyn_T.dot(cov_obs).dot(Dyn_T.T) + Nx\n",
    "        mu_predict = Dyn_T.dot(mu_obs)\n",
    "    else:\n",
    "        cov_predict = None\n",
    "        mu_predict = None\n",
    "    return(cov_predict, mu_predict)\n",
    "\n",
    "def prediction_2(cov_obs, mu_obs, Dyn_T, Dyn_T_2, Nx,vx,vy):\n",
    "    if np.all(cov_obs != None) and np.all(mu_obs != None):\n",
    "        mu_obs[2] = vx\n",
    "        mu_obs[3] = vy\n",
    "        cov_predict = Dyn_T_2.dot(cov_obs).dot(Dyn_T_2.T) + Nx\n",
    "        mu_predict = Dyn_T.dot(mu_obs)\n",
    "    else:\n",
    "        cov_predict = None\n",
    "        mu_predict = None\n",
    "    return(cov_predict, mu_predict)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Multivariate functions\n",
    "def multi_gauss_pdf(mu, covariance, x, dim):\n",
    "    if np.all(mu != None) and np.all(covariance != None) and np.all(x != None):\n",
    "        x_m = x - mu\n",
    "        ans = (1./(np.sqrt(((2*np.pi)**dim)*np.linalg.det(covariance))))*np.exp(-0.5*(np.linalg.solve(covariance,x_m).T.dot(x_m)))\n",
    "        x_0 = mu - mu\n",
    "        c_norm = (1./(np.sqrt(((2*np.pi)**dim)*np.linalg.det(covariance))))*np.exp(-0.5*(np.linalg.solve(covariance,x_0).T.dot(x_0)))\n",
    "        ans_norm = ans/c_norm\n",
    "    else:\n",
    "        ans = None\n",
    "        ans_norm = None\n",
    "    return (ans_norm)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "#parametrization\n",
    "#Measurement info\n",
    "Ny_cov = np.array([[250, 0, 0, 0],[0, 250, 0, 0],[0, 0, 100, 0],[0, 0, 0, 100]]) #Measurement noise\n",
    "Y_transition = np.array([[1,0,0,0],[0,1,0,0],[0,0,0,0],[0,0,0,0]]) #Measurement Transition Matrix\n",
    "\n",
    "#predict info\n",
    "Nx = np.array([[50, 0, 0, 0],[0, 50, 0, 0],[0, 0, 25, 0],[0, 0, 0, 25]]) #Prediction noise\n",
    "Dyn_T = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]]) #System Dynamics\n",
    "Dyn_T_2 = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) #System Dynamics\n",
    "Gx = np.array([[5, 0, 0, 0],[0, 5, 0, 0],[0, 0, 50, 0],[0, 0, 0, 50]]) #gating"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "pixiedust": {
     "displayParams": {}
    },
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "init = 0\n",
    "\n",
    "for n in range(len(measurement)):      \n",
    "    if (init == 0)&(np.all(measurement[n]!=None)):\n",
    "        #Setup Priors\n",
    "        n_objects = 1500\n",
    "\n",
    "        mu = np.zeros([len(measurement)+1, n_objects,4])\n",
    "        mu_obs = np.zeros([len(measurement), n_objects,4])\n",
    "        cov = np.zeros([len(measurement)+1, n_objects,4,4])\n",
    "        cov_obs = np.zeros([len(measurement), n_objects,4,4])\n",
    "        for m in range(n_objects):\n",
    "            mu[n+1][m] = np.array([None,None,None,None]).T\n",
    "            mu_obs[n][m] = np.array([None,None,None,None]).T\n",
    "            cov[n+1][m] = np.array([[None,None,None,None],[None,None,None,None],[None,None,None,None],[None,None,None,None]])\n",
    "            cov_obs[n][m] = np.array([[None,None,None,None],[None,None,None,None],[None,None,None,None],[None,None,None,None]])\n",
    "    \n",
    "            if m in range(len(measurement[0])):\n",
    "                n_obj = m\n",
    "                mu[n+1][n_obj] = np.array([measurement[n][m][0],measurement[n][m][1],0,0]).T\n",
    "                mu_obs[n][n_obj] = np.array([measurement[n][m][0],measurement[n][m][1],0,0]).T\n",
    "                cov[n+1][n_obj] = np.array([[25, 0, 0, 0],[0, 25, 0, 0],[0, 0, 5, 0],[0, 0, 0, 5]])\n",
    "                cov_obs[n][n_obj] = np.array([[25, 0, 0, 0],[0, 25, 0, 0],[0, 0, 5, 0],[0, 0, 0, 5]])\n",
    "        \n",
    "        no_detect = np.zeros([n_objects])\n",
    "        number_of_detections = np.zeros([n_objects])\n",
    "        init = 1\n",
    "    elif(init == 1):\n",
    "        if np.any(pd.isnull(measurement[n])):\n",
    "            for o in range(n_obj + 1):\n",
    "                cov_obs[n][o] = cov[n][o]\n",
    "                mu_obs[n][o] = mu[n][o]\n",
    "                cov[n+1][o], mu[n+1][o] = prediction(cov_obs[n][o], mu_obs[n][o], Dyn_T, Nx)\n",
    "                no_detect[o] = 1 + no_detect[o]\n",
    "            o = 0\n",
    "        else:\n",
    "            a = np.zeros([n_obj + 1, len(measurement[n])+ n_obj + 1])\n",
    "            cov_gate = []\n",
    "            mu_gate = []\n",
    "            \n",
    "            for g in range (n_obj + 1):\n",
    "                if np.any(pd.isnull(mu_obs[n-1][g])) or np.any(pd.isnull(cov[n][g])):   #test to see if that track still has values            \n",
    "                    for q in range(len(measurement[n]) + n_obj):\n",
    "                        a[g,q] = 0\n",
    "                    a[g,len(measurement[n])] = 0\n",
    "                else:\n",
    "                    Kg_i = Kalman_Gain(cov[n][g], Y_transition, Ny_cov)\n",
    "                    cov_gate = np.array([[cov[n][g][0][0], 0],[0, cov[n][g][1][1]]])\n",
    "                    mu_gate = np.array([mu[n][g][0],mu[n][g][1]]).T\n",
    "                    mu_3_std = np.array([mu_gate[0] - 3*np.sqrt(cov_gate[0][0]),mu_gate[1] - 3*np.sqrt(cov_gate[1][1])]).T\n",
    "                    for q in range(len(measurement[n]) + n_obj +1):\n",
    "                        if q < len(measurement[n]):\n",
    "                            y = np.array([measurement[n][q][0],measurement[n][q][1]]).T #Measurement\n",
    "                        else:\n",
    "                            y = mu_3_std\n",
    "                        a[g,q] = multi_gauss_pdf(mu_gate, cov_gate, y, 2)            \n",
    "\n",
    "            # GET BEST ASSIGNMENTS:\n",
    "            cost_matrix = np.ones(a.shape) - a\n",
    "            assignments = linear_sum_assignment(np.array(cost_matrix))\n",
    "            for o in range(len(assignments[0])):\n",
    "                if assignments[1][o] < len(measurement[n]):\n",
    "                    Kg = Kalman_Gain(cov[n][o], Y_transition, Ny_cov)\n",
    "                    y_e = np.array([measurement[n][assignments[1][o]][0],measurement[n][assignments[1][o]][1],0,0]).T #Measurement\n",
    "                    cov_obs[n][o], mu_obs[n][o] = observe(cov[n][o], mu[n][o], Y_transition, Ny_cov, y_e, Kg)\n",
    "                    cov[n+1][o], mu[n+1][o] = prediction(cov_obs[n][o], mu_obs[n][o], Dyn_T, Nx)\n",
    "                    no_detect[o] = 0\n",
    "                    number_of_detections = number_of_detections + 1\n",
    "                else:\n",
    "                    if no_detect[o] > 10:\n",
    "                        cov[n+1][o], mu[n+1][o] = None,None\n",
    "                        mu_obs[n][o], cov_obs[n][o] =None, None\n",
    "                    else:\n",
    "                        cov_obs[n][o] = cov[n][o]\n",
    "                        mu_obs[n][o] = mu[n][o]\n",
    "                        cov[n+1][o], mu[n+1][o] = prediction(cov_obs[n][o], mu_obs[n][o], Dyn_T, Nx)\n",
    "                        no_detect[o] = 1 + no_detect[o]\n",
    "\n",
    "            #STARTING NEW TRACKS:\n",
    "            m = 0\n",
    "            for m in range(len(measurement[n])):\n",
    "                if m not in assignments[1] and (n_obj + 1) < n_objects:\n",
    "                    n_obj = n_obj + 1\n",
    "                    mu[n+1][n_obj] = np.array([measurement[n][m][0],measurement[n][m][1],0,0]).T\n",
    "                    mu_obs[n][n_obj] = np.array([measurement[n][m][0],measurement[n][m][1],0,0]).T\n",
    "                    cov[n+1][n_obj] = np.array([[25, 0, 0, 0],[0, 25, 0, 0],[0, 0, 5, 0],[0, 0, 0, 5]])\n",
    "                    cov_obs[n][n_obj] = np.array([[25, 0, 0, 0],[0, 25, 0, 0],[0, 0, 5, 0],[0, 0, 0, 5]])\n",
    "                    \n",
    "# save to npy file\n",
    "save(mu_save, mu)\n",
    "save(cov_save, cov)\n",
    "save(cov_obs_save, cov_obs)\n",
    "save(mu_obs_save, mu_obs)\n",
    "save(number_of_detections_save, number_of_detections)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.6.10 64-bit ('pgmpractice': conda)",
   "language": "python",
   "name": "python361064bitpgmpracticeconda5b31ac614eac4629951ba8226450065a"
  },
  "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.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}