PS3.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def readPoints(filename):\n",
    "    points = []\n",
    "    with open(filename, 'r') as f:\n",
    "        for line in f.readlines():\n",
    "            points.append([float(n) for n in line.split()])\n",
    "    return np.asarray(points)\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "points3d = readPoints('input/pts3d.txt')\n",
    "points3d_norm = readPoints('input/pts3d-norm.txt')\n",
    "points2d = readPoints('input/pts2d-pic_a.txt')\n",
    "points2d_norm = readPoints('input/pts2d-norm-pic_a.txt')\n",
    "points2d_b = readPoints('input/pts2d-pic_b.txt')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def svdSolver(pts2d, pts3d):\n",
    "    n_points = pts2d.shape[0]\n",
    "    \n",
    "    a_matrix = np.zeros((n_points * 2, 12))\n",
    "    \n",
    "    u, v = pts2d[:, 0], pts2d[:, 1]\n",
    "    x, y, z = pts3d[:, 0], pts3d[:, 1], pts3d[:, 2]\n",
    "    \n",
    "    ones = np.ones(n_points)\n",
    "    zeros = np.zeros(n_points)\n",
    "    \n",
    "    a_matrix[::2, :] = np.column_stack((x, y, z, ones, zeros, zeros, zeros, zeros, -u*x, -u*y, -u*z, -u))\n",
    "    a_matrix[1::2, :] = np.column_stack((zeros, zeros, zeros, zeros, x, y, z, ones, -v*x, -v*y, -v*z, -v))\n",
    "    \n",
    "    _,_,V = np.linalg.svd(a_matrix, full_matrices=True)\n",
    "    M = V.T[:, -1]\n",
    "    M = M.reshape((3, 4))\n",
    "    \n",
    "    pts3d_homo = np.row_stack((pts3d.T, ones))\n",
    "    pts2d_homo = M.dot(pts3d_homo)\n",
    "    pts2d_proj = pts2d_homo / pts2d_homo[2, :]\n",
    "    \n",
    "    res = np.sqrt(np.sum((pts2d - pts2d_proj.T[:, :2]) ** 2))\n",
    "    return M, res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M, res = svdSolver(points2d_norm, points3d_norm)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def computeResidual(pts2d, pts3d, M):\n",
    "    ones = np.ones(len(pts2d))\n",
    "    pts3d_homo = np.row_stack((pts3d.T, ones))\n",
    "    pts2d_homo = M.dot(pts3d_homo)\n",
    "    pts2d_proj = pts2d_homo / pts2d_homo[2, :]\n",
    "    \n",
    "    res = np.sqrt(np.sum((pts2d - pts2d_proj.T[:, :2]) ** 2))\n",
    "    return res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def leastSquareSolver(pts2d, pts3d):\n",
    "    num_points = pts2d.shape[0]\n",
    "    a_matrix = np.zeros((num_points * 2, 11))\n",
    "    u, v = pts2d[:, 0], pts2d[:, 1]\n",
    "    x, y, z = pts3d[:, 0], pts3d[:, 1], pts3d[:, 2]\n",
    "    \n",
    "    ones = np.ones(num_points)\n",
    "    zeros = np.zeros(num_points)\n",
    "    a_matrix[::2, :] = np.column_stack((x, y, z, ones, zeros, zeros, zeros, zeros, -u*x, -u*y, -u*z))\n",
    "    a_matrix[1::2, :] = np.column_stack((zeros, zeros, zeros, zeros, x, y, z, ones, -v*x, -v*y, -v*z))\n",
    "    \n",
    "    b = np.zeros(2 * num_points)\n",
    "    b[::2] = u\n",
    "    b[1::2] = v\n",
    "\n",
    "    M,res,_,_ = np.linalg.lstsq(a_matrix, b)\n",
    "    M = np.append(M, 1)\n",
    "    M = M.reshape((3,4))\n",
    "    return M, res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M, res = leastSquareSolver(points2d_norm, points3d_norm)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def randomSolver(pts2d, pts3d, samples=8, iterations=10, solver=leastSquareSolver):\n",
    "    \n",
    "    indices = np.arange(len(pts2d))\n",
    "    best_res = np.inf\n",
    "    bestM = None\n",
    "    for i in range(iterations):\n",
    "        np.random.shuffle(indices)\n",
    "        train_indices = indices[:samples]\n",
    "        val_indices = indices[samples: samples + 4]\n",
    "        \n",
    "        M, _ = solver(pts2d[train_indices], pts3d[train_indices])\n",
    "        res = computeResidual(pts2d[val_indices], pts3d[val_indices], M)\n",
    "        if res < best_res:\n",
    "            bestM = M\n",
    "            best_res = res\n",
    "    return bestM, best_res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M, res = randomSolver(points2d, points3d, 16, 50)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Mnormlized, res = randomSolver(points2d_norm, points3d_norm, 16, 50)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = Mnormlized[:,:3]\n",
    "m4 = Mnormlized[:, 3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "-np.linalg.inv(Q).dot(m4)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Fundamental Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def leastSquareFSolver(ptsLeft, ptsRight):\n",
    "    num_points = ptsLeft.shape[0]\n",
    "    a_matrix = np.zeros((num_points, 8))\n",
    "    u, v = ptsLeft[:, 0], ptsLeft[:, 1]\n",
    "    ur, vr = ptsRight[:, 0], ptsRight[:, 1]\n",
    "    \n",
    "    ones = np.ones(num_points)\n",
    "    a_matrix = np.column_stack((ur*u, ur*v, ur, vr*u, vr*v, vr, u, v))\n",
    "    \n",
    "    b = -ones\n",
    "\n",
    "    F,res,_,_ = np.linalg.lstsq(a_matrix, b)\n",
    "    F = np.append(F, 1)\n",
    "    F = F.reshape((3,3))\n",
    "    return F, res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "F, res = leastSquareFSolver(points2d_b, points2d)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "F"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calculateFdash(F):\n",
    "    U,D,V = np.linalg.svd(F, full_matrices=True)\n",
    "    D[np.argmin(D)] = 0\n",
    "    return np.dot(U, np.dot(np.diag(D), V))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Fdash = calculateFdash(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Fdash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calculateEpipolarLine(imgA, pointsA, pointsB, F):\n",
    "    rows, cols, _ = imgA.shape\n",
    "    pUL = np.asarray([0,0,1])\n",
    "    pBL = np.asarray([rows-1, 0,1])\n",
    "    \n",
    "    pUR = np.asarray([0, cols-1, 1])\n",
    "    pBR = np.asarray([rows-1, cols-1, 1])\n",
    "    \n",
    "    lL = np.cross(pUL, pBL)\n",
    "    lR = np.cross(pUR, pBR)\n",
    "    \n",
    "    lines = []\n",
    "    for pA, pB in zip(pointsA, pointsB):\n",
    "        la = F.dot(np.append(pB, 1).T)\n",
    "        p1 = np.cross(la, lL)\n",
    "        p2 = np.cross(la, lR)\n",
    "        \n",
    "        p1 = (p1[:2] / p1[2]).astype(np.int)\n",
    "        p2 = (p2[:2] / p2[2]).astype(np.int)\n",
    "        lines.append((p1, p2))\n",
    "    return lines  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def drawEpipolarLines(fn, point, others, Fdash):\n",
    "    img = cv2.imread(fn)\n",
    "    lines = calculateEpipolarLine(img, point, others, Fdash)\n",
    "\n",
    "    for p1, p2 in lines:\n",
    "        cv2.line(img, (int(p1[0]), int(p1[1])), (int(p2[0]), int(p2[1])), color=(0, 0, 0))\n",
    "    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)\n",
    "    plt.imshow(img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "drawEpipolarLines('input/pic_a.jpg', points2d, points2d_b, Fdash)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "drawEpipolarLines('input/pic_b.jpg', points2d_b, points2d, Fdash.T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}