Applied_kalman_filter_1.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#files to import\n",
    "import numpy as np\n",
    "import cv2\n",
    "from collections import defaultdict\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Kalman equation 1: \n",
    "Contain the state space model of position and velocity in x and y coordinates that is row and column\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "#kalman filter\n",
    "def k_predict(x,P,FPS=30):\n",
    "    delta_t = 1\n",
    "    #x is state \n",
    "    #p is uncertainity covariance\n",
    "    F = np.array([[1,0,delta_t,0],[0,1,0,delta_t],[0,0,1,0],[0,0,0,1]])#state transistion matrix\n",
    "    x_pred = np.dot(F,x)\n",
    "    p_pred = np.dot(np.dot(F,P),F.T)\n",
    "    return x_pred,p_pred\n",
    "\n",
    "def k_update(z,x_pred,p_pred):\n",
    "    R = np.identity(4)#measurement uncertainity assume to be 1\n",
    "    I = np.identity(4) #identity matrix\n",
    "    H = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])#measurment function contain all the information velocity as well as position in x and y coord\n",
    "    y = z - np.dot(H,x_pred)#residul or error\n",
    "    s = np.dot(np.dot(H,p_pred),H.T)+R\n",
    "    k = np.dot(np.dot(p_pred,H.T),np.linalg.inv(s))#kalman gain\n",
    "    x_next = x_pred+np.dot(k,y) #corrected state\n",
    "    p_next = np.dot((I-np.dot(k,H)),p_pred) #corrected covariance\n",
    "    return x_next,p_next"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "#feature parameter for the shi tomasi algo\n",
    "feature_params = dict(maxCorners=50,\n",
    "                    qualityLevel=0.2,\n",
    "                    minDistance = 10,\n",
    "                    blockSize = 10)\n",
    "\n",
    "#parameters for lucasKanade\n",
    "lk_params = dict( winSize  = (15,15),\n",
    "                  maxLevel = 2,criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))\n",
    "\n",
    "#create the color\n",
    "#color  = np.random.randint(0,255,(300,3))\n",
    "\n",
    "check_id = 0 #it is the id given to each object\n",
    "main_id = 0\n",
    "object_ids = dict() #it is the dictionary which can contains the id and the coordinate\n",
    "disappeared = dict() #it is the dictionary which contains the number of time the object is dissappeared\n",
    "max_disappeared = 10 #till 10 frames the object will be followed up if it is not visible after 10 frames it will be remove\n",
    "#state and noise initialization for n corner points\n",
    "state = defaultdict(np.array)\n",
    "P_cov = defaultdict(np.array)\n",
    "\n",
    "max_points = 30\n",
    "points = defaultdict(list)\n",
    "points_kalman = defaultdict(list)\n",
    "\n",
    "##TODO\n",
    "#Edit the filename want to track\n",
    "cap = cv2.VideoCapture(\"filename\")\n",
    "ret, old_frame = cap.read()\n",
    "old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)\n",
    "#create the feature points\n",
    "fp = cv2.goodFeaturesToTrack(old_gray, mask = None, **feature_params)\n",
    "\n",
    "#state initialization\n",
    "for i,fppp in enumerate(fp):\n",
    "    (c,d) = fppp.ravel()\n",
    "    check_id+=1\n",
    "    object_ids[check_id] = object_ids.get(check_id,(c,d))\n",
    "    disappeared[check_id] = disappeared.get(check_id,0)\n",
    "    points[check_id].append((c,d))\n",
    "    points_kalman[check_id].append((c,d))\n",
    "    state[check_id] = np.array([c,d,0,0]).reshape(4,1) #[x,y,vx,vy]\n",
    "    P_cov[check_id] = np.array([[1,0,0,0],[0,1,0,0],[0,0,10,0],[0,0,0,10]])\n",
    "#print(fp)\n",
    "while(cap.isOpened()):\n",
    "    ret,frame = cap.read()\n",
    "    if ret==True:\n",
    "        gray_frame = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)\n",
    "        tp,st,err = cv2.calcOpticalFlowPyrLK(old_gray,gray_frame,fp,None,**lk_params)\n",
    "        good_new = tp[st==1]\n",
    "        good_old = fp[st==1]\n",
    "        image = frame.copy()\n",
    "        image1 = image.copy()\n",
    "        image2 = image.copy()\n",
    "\n",
    "        for i,(old,new) in enumerate(zip(good_old,good_new)):\n",
    "            a,b = old.ravel().astype(int)\n",
    "            c,d = new.ravel().astype(int)\n",
    "            z = np.array([c,d,c-a,d-b]).reshape(4,1)\n",
    "            try:\n",
    "                main_id = [key for key,values in object_ids.items() if values[0]==a and values[1]==b][0]\n",
    "            except Exception as e:\n",
    "                main_id=-1\n",
    "                pass\n",
    "\n",
    "            if main_id<0:\n",
    "                check_id+=1\n",
    "                object_ids[check_id] = object_ids.get(check_id,(c,d))\n",
    "                disappeared[check_id] = disappeared.get(check_id,0)\n",
    "                points[check_id].append((c,d))\n",
    "                main_id=check_id\n",
    "            else:\n",
    "                object_ids[main_id]=(c,d)\n",
    "                points[main_id].append((c,d))\n",
    "                if len(points[main_id])>max_points:\n",
    "                    points[main_id].pop(0)\n",
    "            for newp,point in enumerate(points[main_id]):\n",
    "                if newp>=len(points[main_id])-2:\n",
    "                    break\n",
    "                (rpx,rpy) = point\n",
    "                (rnx,rny) = points[main_id][newp+1]\n",
    "                image = cv2.line(frame,(rpx,rpy),(rnx,rny),(255,0,0),2)\n",
    "            image1 = cv2.circle(image1,(c,d),2,(255,0,0),2)\n",
    "            image1 = cv2.putText(image1,\"x:\"+str(c),(c-2,d-2),cv2.FONT_HERSHEY_COMPLEX,0.5,(255,0,0),1)\n",
    "            #for iterate in range(10):\n",
    "            x_predict,p_predict = k_predict(state[main_id],P_cov[main_id])\n",
    "            x_next,p_next = k_update(z,x_predict,p_predict)\n",
    "            pfpx = int(x_predict[0][0])\n",
    "            pfpy = int(x_predict[1][0])\n",
    "            pfpvx = int(x_predict[2][0])\n",
    "            pfpvy = int(x_predict[3][0])\n",
    "            #pfpx = int(x_next[0][0])\n",
    "            #pfpy = int(x_next[1][0])\n",
    "            \n",
    "            image2 = cv2.circle(image2,(pfpx,pfpy),2,(0,0,255),2)\n",
    "            image2 = cv2.putText(image2,\"x:\"+str(pfpx),(pfpx-2,pfpy-2),cv2.FONT_HERSHEY_COMPLEX,0.5,(0,0,255),1)\n",
    "            state[main_id] = x_next\n",
    "            P_cov[main_id] = p_next\n",
    "        #new_fp = cv2.goodFeaturesToTrack(gray_frame, mask = None, **feature_params)\n",
    "        #for nfp in new_fp:\n",
    "        #    (nfpx,nfpy) = nfp.ravel()\n",
    "        #    image = cv2.circle(image,(nfpx,nfpy),2,(255,0,0),2)\n",
    "        fp = good_new.reshape(-1,1,2)\n",
    "        old_gray = gray_frame.copy()\n",
    "        cv2.imshow('frame',frame)\n",
    "        #cv2.imshow('main',image)\n",
    "        cv2.imshow('original',image1)\n",
    "        cv2.imshow('kalman',image2)\n",
    "        k = cv2.waitKey(30) & 0xff\n",
    "        if k==27:\n",
    "            break\n",
    "    else:\n",
    "        break\n",
    "cv2.destroyAllWindows()\n",
    "cap.release()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}