head-pose-estimator.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "#import dlib\n",
    "import cv2\n",
    "import numpy as np\n",
    "import argparse"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['data/images\\\\1_joey-friends.jpg', 'data/images\\\\joye.jpg', 'data/images\\\\left.jpg', 'data/images\\\\left01.jpg', 'data/images\\\\left02.jpg', 'data/images\\\\left03.jpg', 'data/images\\\\left04.jpg', 'data/images\\\\left05.jpg', 'data/images\\\\left06.jpg', 'data/images\\\\left07.jpg', 'data/images\\\\left08.jpg', 'data/images\\\\left09.jpg', 'data/images\\\\left11.jpg', 'data/images\\\\left12.jpg', 'data/images\\\\left13.jpg', 'data/images\\\\left14.jpg']\n",
      "[[841.03841995   0.         299.82147851]\n",
      " [  0.         795.35092229 311.11137452]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-0.74385783  1.22763835 -0.06298022  0.02514577 -4.96593631]]\n",
      "[[532.31551799   0.         339.8746808 ]\n",
      " [  0.         533.20299198 229.95766405]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-2.74923328e-01 -1.16425935e-01  1.97353060e-03 -2.80934499e-04\n",
      "   6.99029347e-01]]\n",
      "[[534.66009875   0.         338.04563279]\n",
      " [  0.         535.23103875 233.17885002]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-0.29859772  0.15622958  0.00161868 -0.00059438 -0.04155775]]\n",
      "[[532.84697556   0.         338.36938031]\n",
      " [  0.         532.96420007 232.3225661 ]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-0.29156285  0.10805925  0.00205759 -0.00092031  0.08343395]]\n",
      "[[534.34144579   0.         339.15527836]\n",
      " [  0.         534.68425882 233.84359493]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-2.88320983e-01  5.41079685e-02  1.73501622e-03 -2.61333895e-04\n",
      "   2.04110465e-01]]\n",
      "[[534.23975673   0.         339.48785767]\n",
      " [  0.         534.48885733 233.1201801 ]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-0.29800288  0.16066574  0.00158846 -0.00033622 -0.06800946]]\n",
      "[[534.09050351   0.         339.75719693]\n",
      " [  0.         534.3178381  233.13618843]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-3.03020486e-01  1.76149345e-01  1.51645329e-03 -4.80550013e-05\n",
      "  -8.53855132e-02]]\n",
      "[[534.17982188   0.         341.22392645]\n",
      " [  0.         534.42712122 233.96164532]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-3.05041222e-01  1.88567156e-01  1.55700506e-03  5.80965865e-05\n",
      "  -1.13923108e-01]]\n",
      "[[534.21305012   0.         341.94076136]\n",
      " [  0.         534.3422263  234.20697574]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-3.00225860e-01  1.48224136e-01  1.49033658e-03  6.62245606e-05\n",
      "  -2.78325469e-02]]\n",
      "[[534.75798783   0.         341.4801265 ]\n",
      " [  0.         534.96955545 233.64983894]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-2.96548154e-01  1.13868006e-01  1.44476302e-03  2.94839956e-04\n",
      "   4.73668193e-02]]\n",
      "[[534.07088364   0.         341.53407554]\n",
      " [  0.         534.11914595 232.94565259]\n",
      " [  0.           0.           1.        ]]\n",
      "[[-2.92971637e-01  1.07706962e-01  1.31038376e-03 -3.11018780e-05\n",
      "   4.34798110e-02]]\n"
     ]
    }
   ],
   "source": [
    "# https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html\n",
    "\n",
    "import glob\n",
    "\n",
    "# termination criteria\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "\n",
    "# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)\n",
    "objp = np.zeros((6 * 7, 3), np.float32)\n",
    "objp[:, :2] = np.mgrid[0: 7, 0: 6].T.reshape(-1, 2)\n",
    "\n",
    "# Arrays to store object points and image points from all the images.\n",
    "objpoints = []  # 3d point in real world space\n",
    "imgpoints = []  # 2d points in image plane.\n",
    "\n",
    "images = glob.glob('data/images/*.jpg')\n",
    "print(images)\n",
    "\n",
    "for image in images:\n",
    "    img = cv2.imread(image)\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)\n",
    "\n",
    "    if ret:\n",
    "        objpoints.append(objp)\n",
    "\n",
    "        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)\n",
    "        imgpoints.append(corners2)\n",
    "\n",
    "        img = cv2.drawChessboardCorners(img, (7, 6), corners2, ret)\n",
    "        cv2.imshow('img', img)\n",
    "        cv2.waitKey(500)\n",
    "\n",
    "        ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)\n",
    "        print(mtx)\n",
    "        print(dist)\n",
    "\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "detector = dlib.get_frontal_face_detector()\n",
    "predictor = dlib.shape_predictor(\"shape_predictor_68_face_landmarks.dat\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ref3DModel():\n",
    "    modelPoints = [[0.0,0.0,0.0],\n",
    "                   [0.0,-330.0,-65.0],\n",
    "                   [-255.0,170.0,-135.0],\n",
    "                   [225.0,170.0,-135.0],\n",
    "                   [-150.0,-150.0,-125.0],\n",
    "                   [150.0,-150.0,-125.0]]\n",
    "    return np.array(modelPoints,dtype=np.float64)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ref2DImagePoints(shape):\n",
    "    imagePoints = [[shape.part(30).x,shape.part(30).y],\n",
    "                   [shape.part(8).x,shape.part(8).y],\n",
    "                   [shape.part(36).x,shape.part(36).y],\n",
    "                   [shape.part(45).x,shape.part(45).y],\n",
    "                   [shape.part(48).x,shape.part(48).y],\n",
    "                   [shape.part(54).x,shape.part(54).y]]\n",
    "    return np.array(imagePoints,dtype=np.float64)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# def CameraMatrix(fl,center):\n",
    "#     cameraMatrix = [[fl,1,center[0]],\n",
    "#                     [0,fl,centre[1]],\n",
    "#                     [0,0,1]]\n",
    "#     return np.array(cameraMatrix,dtype=np.float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def drawPolyline(img,shapes,start,end,isClosed=False):\n",
    "    points = []\n",
    "    for i in range(start,end+1):\n",
    "        point = [shapes.part(i).x,shapes.part(i).y]\n",
    "        points.append(point)\n",
    "    points = np.array(points,dtype=np.float32)\n",
    "    cv2.polylines(img,np.int32([points]),isClosed,(255,80,0),thickness=1,lineType=cv2.LINE_8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw(img,shapes):\n",
    "    drawPolyline(img, shapes, 0, 16)\n",
    "    drawPolyline(img, shapes, 17, 21)\n",
    "    drawPolyline(img, shapes, 22, 26)\n",
    "    drawPolyline(img, shapes, 27, 30)\n",
    "    drawPolyline(img, shapes, 30, 35, True)\n",
    "    drawPolyline(img, shapes, 36, 41, True)\n",
    "    drawPolyline(img, shapes, 42, 47, True)\n",
    "    drawPolyline(img, shapes, 48, 59, True)\n",
    "    drawPolyline(img, shapes, 60, 67, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "cap = cv2.VideoCapture(0)\n",
    "\n",
    "while True:\n",
    "    GAZE = \"Face Not Found\"\n",
    "    ret,img = cap.read()\n",
    "    \n",
    "    faces = detector(cv2.cvtColor(img,cv2.COLOR_BGR2RGB),0)\n",
    "    face3Dmodel = ref3DModel()\n",
    "    \n",
    "    for face in faces:\n",
    "        shape = predictor(cv2.cvtColor(img,cv2.COLOR_BGR2RGB),face)\n",
    "        \n",
    "        draw(img,shape)\n",
    "        \n",
    "        refImgPts = ref2DImagePoints(shape)\n",
    "        \n",
    "        height,width,channels = img.shape\n",
    "#         focalLength = args.focal * width\n",
    "#         cameraMatrix = cameraMatrix(focalLength,(height/2,width/2))\n",
    "        \n",
    "        mdists = np.zeros((4,1),dtype=np.float64)\n",
    "        \n",
    "        success,rotationVector,translationVector = cv2.solvePnP(face3Dmodel,refImgPts,mtx,mdists)\n",
    "        \n",
    "        noseEndPoints3D = np.array([[0,0,1000.0]],dtype=np.float64)\n",
    "        noseEndPoint2D,jacobian = cv2.projectPoints(noseEndPoints3D, rotationVector, translationVector, mtx, mdists)\n",
    "        \n",
    "        rmat, jac = cv2.Rodrigues(rotationVector)\n",
    "        angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)\n",
    "        \n",
    "        if angles[1] < -15:\n",
    "            GAZE = \"Looking: Left\"\n",
    "        elif angles[1] > 15:\n",
    "            GAZE = \"Looking: Right\"\n",
    "        else:\n",
    "            GAZE = \"Forward\"\n",
    "            \n",
    "    cv2.putText(img,GAZE,(20,20),cv2.FONT_HERSHEY_SIMPLEX,1,(0,255,80),2)\n",
    "    cv2.imshow(\"Head Pose\",img)\n",
    "    \n",
    "    key = cv2.waitKey(10) & 0xFF\n",
    "    if key == 27:\n",
    "        break\n",
    "        \n",
    "cap.release()\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "cap.release()\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.10.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}