PinholeCameraCalib_OpenVSLAM.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pinhole camera calibration for OpenVSLAM"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "import sys\n",
    "import os\n",
    "from os.path import join\n",
    "import yaml\n",
    "import glob\n",
    "from functools import partial\n",
    "\n",
    "from enum import Enum\n",
    "class Camera(Enum):\n",
    "    OpenCV = 1 # OpenCV\n",
    "    PySpin = 2 # Spinnaker\n",
    "    \n",
    "class Pattern(Enum):\n",
    "    Chessboard = 1\n",
    "    Circle = 2\n",
    "\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Capture checkboard images\n",
    "Checkborad pdf file is available at \n",
    " - https://raw.githubusercontent.com/opencv/opencv/master/doc/pattern.png\n",
    " - https://raw.githubusercontent.com/opencv/opencv/master/doc/acircles_pattern.png"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# User setting\n",
    "\n",
    "# Input camera\n",
    "cam_kind = Camera.OpenCV\n",
    "# cam_kind = Camera.PySpin\n",
    "\n",
    "# Pattern\n",
    "pattern_kind = Pattern.Chessboard\n",
    "# pattern_kind = Pattern.Circle\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Find pattern\n",
    "subpix_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)\n",
    "if pattern_kind == Pattern.Chessboard:\n",
    "    pattern_size = (6,9)\n",
    "    detect_flag = cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_NORMALIZE_IMAGE # cv2.CALIB_CB_FAST_CHECK+\n",
    "    findCorners = partial(cv2.findChessboardCorners, patternSize=pattern_size, flags=detect_flag)\n",
    "elif pattern_kind == Pattern.Circle:\n",
    "    pattern_size = (4, 11)\n",
    "    detect_flag = cv2.CALIB_CB_ASYMMETRIC_GRID+cv2.CALIB_CB_CLUSTERING+cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_NORMALIZE_IMAGE\n",
    "    findCorners = partial(cv2.findCirclesGrid, patternSize=pattern_size, flags=detect_flag)\n",
    "\n",
    "# Save images if corners are detected\n",
    "checkBeforeInsert = True\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cam_id = 0\n",
    "\n",
    "if cam_kind == Camera.PySpin:\n",
    "    sys.path.insert(0, './SpinnakerVideoCapture/python')\n",
    "    from PySpinCap import PySpinManager\n",
    "    # start manager\n",
    "    manager = PySpinManager()\n",
    "    cap = manager.get_camera(cam_id)\n",
    "if cam_kind == Camera.OpenCV:\n",
    "    cap = cv2.VideoCapture(cam_id)\n",
    "    fps = cap.get(cv2.CAP_PROP_FPS)\n",
    "    print('Framerate:',fps)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print('#####################')\n",
    "print('### start capturing')\n",
    "elps = []\n",
    "save_imgs = []\n",
    "# cv2.namedWindow('show img', cv2.WINDOW_KEEPRATIO)\n",
    "while True:\n",
    "    start = time.time()\n",
    "\n",
    "    ret, img = cap.read()\n",
    "    cv2.imshow('show img', img)\n",
    "#     show = cv2.resize(img, None, fx=0.5, fy=0.5)\n",
    "#     cv2.imshow('show img', show)\n",
    "    key = cv2.waitKey(20)\n",
    "    elps.append((time.time() - start))\n",
    "\n",
    "    if key == ord('s'):\n",
    "        cv2.waitKey(300)\n",
    "        \n",
    "        ret = True\n",
    "        if checkBeforeInsert:\n",
    "            gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "            # Find the chess board corners\n",
    "            ret, corners = findCorners(gray)\n",
    "        if ret:\n",
    "            save_imgs.append(img)\n",
    "            print('save image:', len(save_imgs))\n",
    "        else:\n",
    "            print(\"couldn't find pattern\")\n",
    "    elif key == 27:\n",
    "        break\n",
    "\n",
    "    if len(elps) == 100:\n",
    "        print('- FPS:{}'.format(len(elps) / sum(elps)))\n",
    "        elps = []\n",
    "\n",
    "cv2.destroyAllWindows()\n",
    "print('### finish capturing')\n",
    "print('#####################')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Release everything\n",
    "cap.release()\n",
    "if cam_kind == Camera.PySpin:\n",
    "    manager.release()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "save_img_folder = './img{}'.format(time.strftime(\"%Y%m%d-%H%M\"))\n",
    "try:\n",
    "    os.mkdir(save_img_folder)\n",
    "except OSError as exc:\n",
    "    print(exc)\n",
    "print(save_img_folder)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Save images\n",
    "for i,it in enumerate(save_imgs):\n",
    "    print('save image : {:03}'.format(i))\n",
    "    cv2.imwrite(join(save_img_folder, '{:03}.png'.format(i)), it)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Calib image\n",
    "\n",
    "## Select pattern type first"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# load_folder = './img20191027-2136/'\n",
    "# load_folder = './img20191027-2312/'\n",
    "load_folder = save_img_folder\n",
    "print('load folder:', load_folder)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-09-05T07:21:35.349704Z",
     "start_time": "2019-09-05T07:21:35.140878Z"
    }
   },
   "outputs": [],
   "source": [
    "import glob\n",
    "import numpy as np\n",
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "%matplotlib inline\n",
    "\n",
    "images = []\n",
    "for fname in sorted(glob.glob('{}/*.png'.format(load_folder))):\n",
    "    img = cv2.imread(fname)\n",
    "    images.append(img)\n",
    "    print(fname)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "subpix_criteria = (cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)\n",
    "imgsize = None\n",
    "if pattern_kind == Pattern.Chessboard:\n",
    "    pattern_size = (6,9)\n",
    "    objp = np.zeros((1, pattern_size[0]*pattern_size[1], 3), np.float32)\n",
    "    objp[0,:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2)\n",
    "    detect_flag = cv2.CALIB_CB_ADAPTIVE_THRESH+cv2.CALIB_CB_NORMALIZE_IMAGE # cv2.CALIB_CB_FAST_CHECK+\n",
    "    findCorners = partial(cv2.findChessboardCorners, patternSize=pattern_size, flags=detect_flag)\n",
    "    \n",
    "elif pattern_kind == Pattern.Circle:\n",
    "    pattern_size = (4, 11)\n",
    "    objp = []\n",
    "    for i in range(pattern_size[1]):\n",
    "        for j in range(pattern_size[0]):\n",
    "            objp.append([2*j+i%2, i ,0])\n",
    "    objp = np.array(objp, np.float32)[np.newaxis]\n",
    "    detect_flag = cv2.CALIB_CB_ASYMMETRIC_GRID+cv2.CALIB_CB_CLUSTERING\n",
    "    findCorners = partial(cv2.findCirclesGrid, patternSize=pattern_size, flags=detect_flag)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-09-05T07:21:36.393050Z",
     "start_time": "2019-09-05T07:21:36.388729Z"
    }
   },
   "outputs": [],
   "source": [
    "# check first image\n",
    "if True:\n",
    "    img = images[2].copy()\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "    # Find the chess board corners\n",
    "    ret, corners = findCorners(gray)\n",
    "\n",
    "    if ret:\n",
    "        cv2.cornerSubPix(gray,corners,(3,3),(-1,-1),subpix_criteria)\n",
    "        cv2.drawChessboardCorners(img, pattern_size, corners, ret) \n",
    "        x_max, y_max = np.max(corners, axis=0)[0]\n",
    "        x_min, y_min = np.min(corners, axis=0)[0]\n",
    "        offset = 50\n",
    "        x_max = int(x_max)+offset\n",
    "        x_min = int(x_min)-offset\n",
    "        y_max = int(y_max)+offset\n",
    "        y_min = int(y_min)-offset\n",
    "        fig, ax = plt.subplots(1, 2, figsize=(15,8))\n",
    "        ax[0].imshow(img[y_min:y_max,x_min:x_max,::-1])\n",
    "        ax[1].imshow(img[:,:,::-1])\n",
    "    else:\n",
    "        print(\"couldn't find corner\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-09-05T07:21:38.645110Z",
     "start_time": "2019-09-05T07:21:37.869287Z"
    }
   },
   "outputs": [],
   "source": [
    "save_cornerimg = True\n",
    "\n",
    "objpoints = [] # 3d point in real world space\n",
    "imgpoints = [] # 2d points in image plane.\n",
    "for i, img in enumerate(images):\n",
    "    if imgsize == None:\n",
    "        imgsize = img.shape[:2]\n",
    "    else:\n",
    "        assert imgsize == img.shape[:2], \"All images must share the same size.\"\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "    # Find the chess board corners\n",
    "    ret, corners = findCorners(gray)\n",
    "    # If found, add object points, image points (after refining them)\n",
    "    if ret == True:\n",
    "        print('found!{}: imgpoints id:{}'.format(i, len(objpoints)))\n",
    "        objpoints.append(objp)\n",
    "        cv2.cornerSubPix(gray,corners,(3,3),(-1,-1),subpix_criteria)\n",
    "        imgpoints.append(corners)\n",
    "        if save_cornerimg:\n",
    "            show = img.copy()\n",
    "            cv2.drawChessboardCorners(show, pattern_size, corners, ret) \n",
    "            fname = join(load_folder, 'corner{}.jpg'.format(len(imgpoints)-1))\n",
    "            cv2.imwrite(fname, show)\n",
    "    else:\n",
    "        print(\"couldn't find corner\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"#### CALIB START #####\")\n",
    "N_OK = len(imgpoints)\n",
    "print(\"Found \" + str(N_OK) + \" valid images for calibration\")\n",
    "K = np.zeros((3, 3))\n",
    "D = np.zeros((5, 1))\n",
    "rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]\n",
    "tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]\n",
    "rms, K, D, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, imgsize[::-1], K, D, rvecs, tvecs)\n",
    "print('final result')\n",
    "print(\"K=np.array(\" + str(K.tolist()) + \")\")\n",
    "print(\"D=np.array(\" + str(D.tolist()) + \")\")\n",
    "print(\"RMS:\", rms)\n",
    "print(\"#### CALIB END #####\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Camera.setup: \"monocular\"\n",
    "Camera.model: \"perspective\"\n",
    "\n",
    "Camera.fx: 458.654\n",
    "Camera.fy: 457.296\n",
    "Camera.cx: 367.215\n",
    "Camera.cy: 248.375\n",
    "\n",
    "Camera.k1: -0.28340811\n",
    "Camera.k2: 0.07395907\n",
    "Camera.p1: 0.00019359\n",
    "Camera.p2: 1.76187114e-05\n",
    "Camera.k3: 0.0\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-09-05T07:26:40.738945Z",
     "start_time": "2019-09-05T07:26:40.727403Z"
    }
   },
   "outputs": [],
   "source": [
    "def printForOpenVSLAM(K, D, img, fps=0, load_folder=load_folder):\n",
    "    data = {}\n",
    "    data['fx'] = K[0][0]\n",
    "    data['fy'] = K[1][1]\n",
    "    data['cx'] = K[0][2]\n",
    "    data['cy'] = K[1][2]\n",
    "\n",
    "    data['k1'] = D[0][0]\n",
    "    data['k2'] = D[1][0]\n",
    "    data['p1'] = D[2][0]\n",
    "    data['p2'] = D[3][0]\n",
    "    data['k3'] = D[4][0]\n",
    "    \n",
    "    data['fps'] = fps\n",
    "    data['cols'] = img.shape[1]\n",
    "    data['rows'] = img.shape[0]\n",
    "    \n",
    "    print_array=['Camera.setup: \"monocular\"',\n",
    "                 'Camera.model: \"perspective\"', '']\n",
    "    for it in ['fx', 'fy', 'cx', 'cy']:\n",
    "        print_array.append('Camera.{}: {}'.format(it, data[it]))\n",
    "    print_array.append('')\n",
    "    for it in ['k1', 'k2', 'p1', 'p2', 'k3']:\n",
    "        print_array.append('Camera.{}: {}'.format(it, data[it]))\n",
    "    print_array.append('')\n",
    "    for it in ['fps', 'cols', 'rows']:\n",
    "        print_array.append('Camera.{}: {}'.format(it, data[it]))\n",
    "\n",
    "    with open(join(load_folder,'calib_pinhole_results.txt'), mode='w') as f:\n",
    "        for it in print_array:\n",
    "            print(it)\n",
    "            print(it, file=f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2019-09-05T07:26:40.864212Z",
     "start_time": "2019-09-05T07:26:40.859656Z"
    }
   },
   "outputs": [],
   "source": [
    "printForOpenVSLAM(K, D, img, fps=30, load_folder=load_folder)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#writes array to .yml file\n",
    "yml_file = join(load_folder,'calib_pinhole_results.yml')\n",
    "fs_write = cv2.FileStorage(yml_file, cv2.FILE_STORAGE_WRITE)\n",
    "fs_write.write('K', K)\n",
    "fs_write.write('D', D)\n",
    "fs_write.write('width', img.shape[1])\n",
    "fs_write.write('height', img.shape[0])\n",
    "fs_write.release()\n",
    "\n",
    "#READ\n",
    "fs_read = cv2.FileStorage(yml_file, cv2.FILE_STORAGE_READ)\n",
    "fs_read.getNode('K').mat()    \n",
    "fs_read.getNode('D').mat()\n",
    "fs_read.getNode('width').real()    \n",
    "fs_read.getNode('height').real()"
   ]
  },
  {
   "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.3"
  },
  "toc": {
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}