contact-edge.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import time\n",
    "\n",
    "import cv2\n",
    "import numpy as np\n",
    "from scipy import ndimage\n",
    "import src.utils as utils\n",
    "from src.getDensity import getFilteredDensity\n",
    "from src.tracker import Tracker\n",
    "from datetime import datetime\n",
    "import freud\n",
    "from pynhhd import nHHD\n",
    "import math\n",
    "import scipy, scipy.fftpack\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def poisson_reconstruct(grady, gradx, boundarysrc):\n",
    "\t# Thanks to Dr. Ramesh Raskar for providing the original matlab code from which this is derived\n",
    "\t# Dr. Raskar's version is available here: http://web.media.mit.edu/~raskar/photo/code.pdf\n",
    "\n",
    "\t# Laplacian\n",
    "\tgyy = grady[1:,:-1] - grady[:-1,:-1]\n",
    "\tgxx = gradx[:-1,1:] - gradx[:-1,:-1]\n",
    "\tf = np.zeros(boundarysrc.shape)\n",
    "\tf[:-1,1:] += gxx\n",
    "\tf[1:,:-1] += gyy\n",
    "\n",
    "\t# Boundary image\n",
    "\tboundary = boundarysrc.copy()\n",
    "\tboundary[1:-1,1:-1] = 0\n",
    "\n",
    "\t# Subtract boundary contribution\n",
    "\tf_bp = -4*boundary[1:-1,1:-1] + boundary[1:-1,2:] + boundary[1:-1,0:-2] + boundary[2:,1:-1] + boundary[0:-2,1:-1]\n",
    "\tf = f[1:-1,1:-1] - f_bp\n",
    "\n",
    "\t# Discrete Sine Transform\n",
    "\ttt = scipy.fftpack.dst(f, norm='ortho')\n",
    "\tfsin = scipy.fftpack.dst(tt.T, norm='ortho').T\n",
    "\n",
    "\t# Eigenvalues\n",
    "\t(x,y) = np.meshgrid(range(1,f.shape[1]+1), range(1,f.shape[0]+1), copy=True)\n",
    "\tdenom = (2*np.cos(math.pi*x/(f.shape[1]+2))-2) + (2*np.cos(math.pi*y/(f.shape[0]+2)) - 2)\n",
    "\n",
    "\tf = fsin/denom\n",
    "\n",
    "\t# Inverse Discrete Sine Transform\n",
    "\ttt = scipy.fftpack.idst(f, norm='ortho')\n",
    "\timg_tt = scipy.fftpack.idst(tt.T, norm='ortho').T\n",
    "\n",
    "\t# New center + old boundary\n",
    "\tresult = boundary\n",
    "\tresult[1:-1,1:-1] = img_tt\n",
    "\n",
    "\treturn result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def match_depth(flow_x, flow_y):\n",
    "    \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Young(flow, a, b, delta, E_1=2.2):\n",
    "    \n",
    "    w_1 = match_depth(flow[:,:,1],flow[:,:,0])\n",
    "    \n",
    "    E_2 = b*(w_1*E_1)/(a-w_1)+ delta\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "loop closed at frame  0e queue length:  2\n",
      "loop closed at frame  0e queue length:  2\n",
      "loop closed at frame  0e queue length:  1\n",
      "loop closed at frame  0e queue length:  2\n",
      "20.04 FPS, current frame queue length:  1\r"
     ]
    }
   ],
   "source": [
    "if __name__ == \"__main__\":\n",
    "    tracker = Tracker(adaptive=True,\n",
    "                      cuda=False)  # cuda=True if using opencv cuda\n",
    "    #cap = cv2.VideoCapture(0, cv2.CAP_V4L2)\n",
    "    top, bottom, left, right = (0, 720, 200, 980) # ROI\n",
    "    \"\"\"gstream_str = 'nvarguscamerasrc sensor-id=0 tnr-mode=2 ee-mode=2 aeantibanding=2 !'\\\n",
    "        ' video/x-raw(memory:NVMM), width=1280, height=720, format=(string)NV12, framerate=(fraction)60/1 !'\\\n",
    "            ' nvvidconv top={top} bottom={bottom} left={left} right={right} !'\\\n",
    "                ' video/x-raw, format=(string)GRAY8, width={width}, height={height}! appsink max-buffers=1 drop=true' \\\n",
    "                    .format(width=str(right-left), height=str(bottom-top), top=top, bottom=bottom, left=left, right=right)\n",
    "    cap = cv2.VideoCapture(gstream_str, cv2.CAP_GSTREAMER)\"\"\"\n",
    "    cap = cv2.VideoCapture(0)\n",
    "    frame_num = 0.0\n",
    "    start_time = time.time()\n",
    "    capture = False\n",
    "    count = 0\n",
    "    while True: \n",
    "        success, img = cap.read(  )\n",
    "        if capture:\n",
    "            frame_num += 1.0\n",
    "            img_cut = img[20:-20, 100:-100]\n",
    "\n",
    "            # img = cv2.cvtColor(img, cv2.COLOR_YUV2GRAY_I420)\n",
    "            img_gray = cv2.cvtColor(img_cut, cv2.COLOR_BGR2GRAY)\n",
    "            # img = img[72:400, 175:480].copy()  \n",
    "            # img = cv2.resize(img, (img.shape[1]//2,img.shape[0]//2))\n",
    "            flow = tracker.track(img_gray) \n",
    "\n",
    "            boundarysrc = np.zeros_like(flow[:,:,0])\n",
    "            \"\"\"\n",
    "            results = poisson_reconstruct(flow[:,:,1],flow[:,:,0],boundarysrc)\n",
    "            \n",
    "            Sn, St, d = utils.Helmholtz(flow)\n",
    "            #rawdata_Sn.append(Sn)\n",
    "            #rawdata_St.append(St)\n",
    "  \n",
    "            Fn = -0.714368+6.911549*Sn\n",
    "            if Fn<0:\n",
    "               Fn = 0\n",
    "            print(Fn)\n",
    "            \"\"\"\n",
    "            \n",
    "            vis = utils.flowvis(img_cut,flow)\n",
    "            density, threshold = getFilteredDensity(flow, use_cuda=True)\n",
    "            density = (density + 1.0) / 1.0\n",
    "            density = (density * 255.0).astype('uint8')\n",
    "            #print(\"density max:\",density.max(),\"density min:\", density.min())\n",
    "            threshold, contact_boundary = utils.getContactBoundary(density)\n",
    "            density[np.where(density < threshold)] = 0\n",
    "            center_of_mass = ndimage.measurements.center_of_mass(density)\n",
    "            density = cv2.applyColorMap(density, cv2.COLORMAP_HOT)\n",
    "            cv2.circle(density,   \n",
    "                       (int(center_of_mass[1]), int(center_of_mass[0])), 5,\n",
    "                       (255, 0, 0), 2)\n",
    "            #arrows = utils.put_optical_flow_arrows_on_image(\n",
    "                #np.zeros_like(density), flow.download()[15:-15, 15:-15, :])\n",
    "            arrows = utils.put_optical_flow_arrows_on_image(\n",
    "                np.zeros_like(density), flow)\n",
    "            #img_cut = img_cut[15:-15,15:-15]\n",
    "            results = utils.show_multi_imgs(1, [img_cut, contact_boundary, arrows, density],(2,2))\n",
    "            cv2.imshow('contact-edge',results)\n",
    "            #print(time.time()-start_time)\n",
    "            # cv2.imshow('img', img)\n",
    "            # cv2.imshow('boundary', contact_boundary)\n",
    "            # cv2.imshow('arrows', arrows)  \n",
    "            # cv2.imshow('density', density)\n",
    "        k = cv2.waitKey(1)\n",
    "        if k & 0xFF == ord('q'):\n",
    "            # cv2.imwrite('d3.jpg', img)\n",
    "            break\n",
    "        elif k & 0xFF == ord('s'):\n",
    "             file_name = datetime.now().strftime(\"%m-%d-%H-%M-%S\")\n",
    "             print(file_name, \" pic saved\")\n",
    "             cv2.imwrite(file_name+'.jpg', img_cut)\n",
    "        elif k & 0xFF == ord('r'):\n",
    "            tracker.reset()\n",
    "            print(\"Tracker reset\")\n",
    "        elif k & 0xFF == ord('f'):\n",
    "            file_name ='flow-'+str(count)\n",
    "            print(file_name,\" flow saved\")\n",
    "            np.save(\"./flowdata/\"+file_name+'.npy',flow)\n",
    "            count += 1\n",
    "        if time.time() - start_time > 1.0:\n",
    "            capture = True\n",
    "            fps = frame_num / (time.time() - start_time)\n",
    "\n",
    "            print(\"{:.2f}\".format(fps),\n",
    "                  \"FPS, current frame queue length: \",\n",
    "                  tracker.getFrameQueueLength(),\n",
    "                  end='\\r')\n",
    "    \n",
    "            frame_num = 0.0\n",
    "            start_time = time.time()\n",
    "            \n",
    "    cap.release()\n",
    "    cv2.destroyAllWindows()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "pytorch1.10",
   "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.13"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}