sfm.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Structure from Motion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The Projective space\n",
    "\n",
    "Points in $ \\mathbb{P}^2 $ are represented as\n",
    "\n",
    "$$ \\mathbf{x} = \\begin{bmatrix}\n",
    "x & y & w\n",
    "\\end{bmatrix}^T\n",
    "$$\n",
    "\n",
    "The equivalent point in $ \\mathbb{R}^2 $ is\n",
    "\n",
    "$$ \\mathbf{x} = \\begin{bmatrix}\n",
    "x / w & y / w\n",
    "\\end{bmatrix}^T\n",
    "$$\n",
    "\n",
    "For points with $w \\neq 0$. Points with $w=0$ is called points as infinity."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lines are represented as\n",
    "\n",
    "$$\n",
    "\\mathbf{x}^T\\mathbf{l} = 0\n",
    "$$\n",
    "\n",
    "Where \n",
    "\n",
    "$$ \\mathbf{l} = \\begin{bmatrix}\n",
    "a & b & c\n",
    "\\end{bmatrix}^T\n",
    "$$\n",
    "\n",
    "For example the horizontal line $x=17$ is expressed as $1x + 0y - 17 = 0$ or $\\mathbf{x}^T\\begin{bmatrix} 1 & 0 & -17\\end{bmatrix}^T = 0$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Null space"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finding any non-zero vector $\\mathbf{x}$ satifying the equation\n",
    "\n",
    "$$\n",
    "A\\mathbf{x} = \\mathbf{0}\n",
    "$$\n",
    "\n",
    "The nullspace is only determined up to a scale."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 88,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([], shape=(2, 0), dtype=float64)"
      ]
     },
     "execution_count": 88,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import numpy as np\n",
    "from numpy.linalg import svd\n",
    "\n",
    "def nullspace(A, atol=1e-13, rtol=0):\n",
    "    A = np.atleast_2d(A)\n",
    "    u, s, vh = svd(A)\n",
    "    tol = max(atol, rtol * s[0])\n",
    "    nnz = (s >= tol).sum()\n",
    "    ns = vh[nnz:].conj().T\n",
    "    return ns\n",
    "\n",
    "A = np.array([[1, 0],\n",
    "              [0, 1]])\n",
    "nullspace(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 101,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.]\n",
      " [1.]]\n",
      "[[0.]\n",
      " [0.]]\n",
      "[[0.]\n",
      " [0.]]\n"
     ]
    }
   ],
   "source": [
    "A = np.array([[1, 0],\n",
    "              [0, 0]])\n",
    "n = nullspace(A)\n",
    "print(n)\n",
    "print(A @ n)\n",
    "k = 17\n",
    "print(A @ (k * n))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Projective camera"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A projective camera $\\texttt{P}$ transforms a world point $\\mathbf{X}$ into a image point $\\mathbf{x}.$\n",
    "\n",
    "$$\\mathbf{x} = \\texttt{P}\\mathbf{X}$$\n",
    "\n",
    "Where $\\texttt{P}$ is a 3x4 matrix and the world point $\\mathbf{X}$ is a 4-vector while the imgage point $\\mathbf{x}$ is a 3-vector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[5. 7. 1.]\n"
     ]
    }
   ],
   "source": [
    "P = np.array([\n",
    "    [20,  0, 0, -10],\n",
    "    [ 0, 20, 0, -10],\n",
    "    [ 0,  0, 1, 0],\n",
    "])\n",
    "X = [3, 4, 10, 1]\n",
    "x = P @ X\n",
    "print(x / x[2])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The camera matrix $\\texttt{P}$ can be decomposed into the calibration matrix $\\texttt{K}$ and a rotation and translation pair like so\n",
    "\n",
    "$$\n",
    "\\texttt{P} = \\texttt{K}\n",
    "\\begin{bmatrix}\n",
    "\\texttt{R} & \\mathbf{t}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Sometimes the camera centre $\\tilde{C}$ is needed expicitly\n",
    "\n",
    "$$\n",
    "\\mathbf{t} = -\\texttt{R}\\tilde{C}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 10.   0.  50. 100.]\n",
      " [  0.  10.  50. 100.]\n",
      " [  0.   0.   1.   0.]]\n"
     ]
    }
   ],
   "source": [
    "K = np.array([\n",
    "    [10, 0, 50],\n",
    "    [0, 10, 50],\n",
    "    [0, 0, 1]\n",
    "])\n",
    "R = np.eye(3)\n",
    "t = np.array([10, 10, 0])\n",
    "P = K @ np.hstack([R, t[:, None]])\n",
    "print(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Calibration matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 90,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "33ed20b6ca3d4ad7acefc0bc5aaae21a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(IntSlider(value=525, description='f', max=1000, min=50), FloatSlider(value=0.0, descript…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "d5b3e4accc474a95ac52b4b804592ac7",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "RoughCanvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from ipywidgets import interact\n",
    "from ipycplot import Plot3D, rectangle\n",
    "import numgl\n",
    "from numpy.linalg import inv\n",
    "\n",
    "width, height = 320, 200\n",
    "\n",
    "n = 10\n",
    "x, y = np.meshgrid(np.linspace(0, width, n), np.linspace(0, height, n))\n",
    "w = np.ones(x.shape)\n",
    "# points in the image plane\n",
    "p1 = np.stack([x, y, w], axis=-1).reshape((-1, 3))\n",
    "\n",
    "plot_f = Plot3D()\n",
    "plot_f.scale = np.array([200, 200, 1])\n",
    "target = np.array([width/2, height/2, 0])\n",
    "plot_f.camera.lookat(target=target)\n",
    "def update_focal(f, ay, px, py):\n",
    "    K = np.array([\n",
    "        [f, 0, px],\n",
    "        [0, f, py],\n",
    "        [0, 0, 1],\n",
    "    ])\n",
    "    p2 = p1 + numgl.normalized(inv(K) @ p1.T, axis=0).T * 50\n",
    "    lines = np.hstack([p1, p2]).reshape(-1, 3)\n",
    "    \n",
    "    r = 600\n",
    "    eye = np.array([r * np.sin(ay), 0, r * np.cos(ay)])\n",
    "    plot_f.camera.lookat(eye=eye + target)\n",
    "    \n",
    "    plot_f.canvas.clear()\n",
    "    plot_f.lines(lines)\n",
    "    plot_f.lines(rectangle(width, height))\n",
    "\n",
    "interact(update_focal, f=(50, 1000), ay=(-np.pi/2, np.pi/2), px=(0, width), py=(0, height))\n",
    "plot_f.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Epipolar Geometry\n",
    "The geometry of two cameras depicting the same scene. \n",
    "* The _epipolar point_ is the image of the camera center of the other image. It may lie outside the visible image.\n",
    "* The _epipolar line_ is the line between the two camera centers.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "ef0f03ad0f3c45cb8c6fe23cd61524bd",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.0, description='ay', max=1.5707963267948966, min=-1.5707963267948966…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "fa29e4cc1c22460a8bbb2cd45d923272",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "RoughCanvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import numpy as np\n",
    "import numgl\n",
    "from ipywidgets import interact\n",
    "from ipycplot import Plot3D, rectangle, transform\n",
    "\n",
    "\n",
    "width, height = 320, 200\n",
    "\n",
    "# world points\n",
    "n = 10\n",
    "X = np.random.normal(size=(n, 3), scale=100)\n",
    "\n",
    "def rt(pose):\n",
    "    R, C = pose[0:3, 0:3], pose[0:3, 3][:, None]\n",
    "    t = -R.T @ C\n",
    "    return R, t\n",
    "\n",
    "plot_overview = Plot3D()\n",
    "plot_overview.scale = np.array([80, 80, 1])\n",
    "\n",
    "f = 350\n",
    "K = np.array([\n",
    "    [f, 0, width/2],\n",
    "    [0, f, height/2],\n",
    "    [0, 0, 1],\n",
    "])\n",
    "\n",
    "left_pose =  numgl.translate((-190,    0, 500)) @ numgl.roty(2.8)\n",
    "right_pose = numgl.translate(( 330, -100, 500)) @ numgl.roty(3.7)\n",
    "P1 = K @ np.hstack(rt(left_pose))\n",
    "P2 = K @ np.hstack(rt(right_pose))\n",
    "\n",
    "def mask_points(x, width, height):\n",
    "    mask = (x[:, 0] >= 0) * (x[:, 0] < width) * (x[:, 1] >= 0) * (x[:, 1] < height)\n",
    "    return x[mask]\n",
    "\n",
    "def update(ay):\n",
    "    center = np.array([0, 150, 0])\n",
    "    target = center\n",
    "    r = 1100\n",
    "    eye = center + np.array([r * np.sin(ay), 0, r * np.cos(ay)]) + np.array([0, -300, 0])\n",
    "    plot_overview.camera.lookat(eye=eye, target=target)\n",
    "\n",
    "    plot_overview.canvas.clear()\n",
    "\n",
    "    # plot world points\n",
    "    plot_overview.circles(X)    \n",
    "    \n",
    "    # project points in left camera\n",
    "    x1 = transform(P1, X)  # transform world points using camera\n",
    "    x1 = mask_points(x1, width, height)  # mask out points outside screen\n",
    "    x1 = np.hstack([x1, np.zeros((x1.shape[0], 1))])  # add z=0 coordinate\n",
    "    plot_overview.circles(transform(left_pose, x1))  # plot projected points for camera\n",
    "    plot_overview.lines(transform(left_pose, rectangle(width, height)))\n",
    "\n",
    "    # project points in right camera\n",
    "    x2 = transform(P2, X)  # transform world points using camera\n",
    "    x2 = mask_points(x2, width, height)  # mask out points outside screen\n",
    "    x2 = np.hstack([x2, np.zeros((x2.shape[0], 1))])  # add z=0 coordinate\n",
    "    plot_overview.circles(transform(right_pose, x2))  # plot projected points for camera\n",
    "    plot_overview.lines(transform(right_pose, rectangle(width, height)))\n",
    "    \n",
    "interact(update, ay=(-np.pi/2, np.pi/2))\n",
    "\n",
    "plot_overview.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The Fundamental Matrix\n",
    "The fundamental matrix maps points in the left image to lines in the right image.\n",
    "\n",
    "$$\n",
    "\\mathbf{l'} = \\texttt{F}\\mathbf{x}\n",
    "$$\n",
    "\n",
    "A point in the left image $\\mathbf{x}$ corresponding to the same world point $\\mathbf{X}$ will lie on the epipolar line $\\mathbf{l'}$ in the right image, so we have\n",
    "\n",
    "$$\n",
    "\\mathbf{x'}^T\\texttt{F}\\mathbf{x} = 0\n",
    "$$\n",
    "\n",
    "It can be directly computed from two cameras like so\n",
    "\n",
    "$$\n",
    "\\texttt{F} =\n",
    "\\begin{bmatrix}\n",
    "\\mathbf{e}'\n",
    "\\end{bmatrix}_\\times \n",
    "\\texttt{P}'\\texttt{P}^+\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1.46447971e-01 -4.81326969e-01  1.12239257e+02]\n",
      " [-2.81864098e-01 -2.55593651e-14  3.38446758e+02]\n",
      " [ 1.09432008e+01 -1.82613145e+02 -3.72723058e+04]]\n"
     ]
    }
   ],
   "source": [
    "from numpy.linalg import pinv\n",
    "\n",
    "def skew(x):\n",
    "    return np.array([\n",
    "        [    0, -x[2],  x[1]],\n",
    "        [ x[2],     0, -x[0]],\n",
    "        [-x[1],  x[0],    0]])\n",
    "\n",
    "def from_cameras(P1, P2):\n",
    "    C1 = nullspace(P1).flatten()  # camera center\n",
    "    e2 = P2 @ C1  # epipole\n",
    "    return skew(e2) @ P2 @ pinv(P1)\n",
    "\n",
    "F = from_cameras(P1, P2)\n",
    "print(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 9.93997891e-01]\n",
      " [-1.09396108e-01]\n",
      " [ 8.27818002e-04]]\n",
      "[[ 0.84917653]\n",
      " [-0.52810436]\n",
      " [-0.00223824]]\n"
     ]
    }
   ],
   "source": [
    "print(nullspace(F))\n",
    "print(nullspace(F.T))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "execution_count": 103,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def rank(A, tol=1e-13):\n",
    "    _, s, _ = svd(A)\n",
    "    return np.sum(s > tol)\n",
    "\n",
    "rank(F)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "3129d5f4a17f4dd4b380e8b6d997685a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "RoughCanvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from numpy import cross\n",
    "from ipycplot import Plot2D, homogenize, dehomogenize\n",
    "def from_implicit(lines, width, height):\n",
    "    \"\"\"Create a line segment from the intersection of an implicit line with a rectangle\"\"\"\n",
    "    result = np.empty((0, 2))\n",
    "    for l in lines:\n",
    "        left = np.array([1, 0, 0])  # x = 0\n",
    "        right = np.array([1, 0, -(width - 1)])  # x = width\n",
    "        top = np.array([0, 1, 0])  # y = 0\n",
    "        bottom = np.array([0, 1, -(height - 1)])  # y = height\n",
    "        intersections = np.array([\n",
    "            cross(l, left), cross(l, right), cross(l, top), cross(l, bottom),\n",
    "        ])\n",
    "        masked = mask_points(dehomogenize(intersections.T).T, width, height)\n",
    "        result = np.append(result, masked, axis=0)\n",
    "    return result\n",
    "\n",
    "n = 7\n",
    "x1 = np.hstack([np.random.rand(n, 2) * np.array([width, height]), np.ones((n, 1))])\n",
    "\n",
    "plot = Plot2D()\n",
    "plot.lines(from_implicit((F @ x1.T).T, width, height))\n",
    "plot.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Fundamental Matrix from correspondences\n",
    "Using $\\mathbf{x'}^T\\texttt{F}\\mathbf{x} = 0$ we can compute $\\texttt{F}$ from image correspondences alone. For a single correspondence $\\mathbf{x} \\leftrightarrow \\mathbf{x}'$ where $\\mathbf{x} = \\left [ x \\,  y \\, 1  \\right ]^T$ and $\\mathbf{x}' = \\left [ x '\\,  y' \\, 1  \\right ]^T$ we can expand the inner products like so\n",
    "\n",
    "$$\n",
    "x'xf_{11} + x'yf_{12} + xf_{13} + y'xf_{21} + y'yf_{22} + y'f_{23} + xf_{31} + yf_{32} + f_{33} = 0\n",
    "$$\n",
    "\n",
    "If we introduce a vector $ \\mathbf{f} = \\left [ f_{11}, f_{12}, f_{13}, f_{21}, f_{22}, f_{23}, f_{31}, f_{32}, f_{33} \\right ]^T $ it can be written as \n",
    "\n",
    "$$\n",
    "\\left [ x'x, x'y, x', y'x, y'y, y', x, y, 1  \\right ] \\mathbf{f} = 0\n",
    "$$\n",
    "\n",
    "Given several image correspondences we can stack these like so\n",
    "$$\n",
    "A\\mathbf{f} =\n",
    "\\begin{bmatrix}\n",
    "x_1'x_1 & x_1'y_1 & x_1' & y_1'x_1 & y_1'y_1 & y_1' & x_1 & y_1 & 1 \\\\\n",
    "\\vdots & \\vdots & \\vdots & \\vdots & \\vdots & \\vdots & \\vdots & \\vdots & \\vdots \\\\\n",
    "x_n'x_n & x_n'y_n & x_n' & y_n'x_n & y_n'y_n & y_n' & x_n & y_n & 1 \\\\\n",
    "\\end{bmatrix} \\mathbf{f}  = \\mathbf{0}\n",
    "$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1.08456011e-05 -3.56459723e-05  8.31218213e-03]\n",
      " [-2.08742092e-05 -1.86500262e-19  2.50645913e-02]\n",
      " [ 8.10428371e-04 -1.35239110e-02 -2.76030155e+00]]\n"
     ]
    }
   ],
   "source": [
    "from numpy.linalg import norm\n",
    "#from ipycplot import transform\n",
    "\n",
    "def normalizing_transform(p):\n",
    "    \"\"\"Isotropic point normalization. Returns a transform that transform to mean 0, 0 and mean norm=sqrt(2)\"\"\"\n",
    "    d = p.shape[1]\n",
    "    c = np.mean(p, axis=0)\n",
    "    s = np.mean(norm(p - c[None:], axis=-1))\n",
    "    T = np.diag(np.append(np.repeat(s / np.sqrt(2), d), 1))\n",
    "    T[:d, d] = c\n",
    "    return inv(T)\n",
    "\n",
    "x1 = transform(P1, X)  # transform world points using camera\n",
    "x2 = transform(P2, X)  # transform world points using camera\n",
    "\n",
    "# compute F\n",
    "def from_correspondences(x1, x2):\n",
    "    # compute normalizing transforms\n",
    "    T1 = normalizing_transform(x1)\n",
    "    T2 = normalizing_transform(x2)\n",
    "    nx1 = transform(T1, x1)\n",
    "    nx2 = transform(T2, x2)\n",
    "    A = np.vstack([(x2*x1, x2*y1, x2, y2*x1, y2*y1, y2, x1, y1, 1) for (x1, y1), (x2, y2) in zip(nx1, nx2)])\n",
    "\n",
    "    #f = nullspace(A)\n",
    "    u, s, vh = svd(A)\n",
    "    f = vh[-1].T\n",
    "    \n",
    "    F = f.reshape((3, 3))    \n",
    "    \n",
    "    # force rank 2\n",
    "    u, s, vh = svd(F)\n",
    "    s[-1] = 0\n",
    "    F = u @ np.diag(s) @ vh\n",
    "    # denormalize F matrix\n",
    "    return T2.T @ F @ T1\n",
    "\n",
    "F = from_correspondences(x1, x2)\n",
    "print(F)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Triangulation\n",
    "\n",
    "From the projection of world points $\\mathbf{X}$ we have\n",
    "$$\n",
    "\\mathbf{x} = \\texttt{P}\\mathbf{X} \\\\\n",
    "\\mathbf{x}' = \\texttt{P}'\\mathbf{X}\n",
    "$$\n",
    "\n",
    "The scale factor can be eliminated by using cross product, e.g. for the left image $\\mathbf{x} \\times (\\texttt{P}\\mathbf{X}) = \\mathbf{0}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[114.91400159  79.79591855]\n",
      " [191.96413533 104.21852799]\n",
      " [141.80631941  55.32463702]\n",
      " [160.07772027  21.93005932]\n",
      " [138.50489646 -25.3398641 ]\n",
      " [123.92059699  92.69832262]\n",
      " [228.36339019  35.69695923]\n",
      " [123.33437131   2.85075625]\n",
      " [140.80726638  54.07659304]\n",
      " [141.51867874 108.31534851]]\n",
      "[[114.91400159  79.79591855]\n",
      " [191.96413533 104.21852799]\n",
      " [141.80631941  55.32463702]\n",
      " [160.07772027  21.93005932]\n",
      " [138.50489646 -25.3398641 ]\n",
      " [123.92059699  92.69832262]\n",
      " [228.36339019  35.69695923]\n",
      " [123.33437131   2.85075625]\n",
      " [140.80726638  54.07659304]\n",
      " [141.51867874 108.31534851]]\n"
     ]
    }
   ],
   "source": [
    "def triangulate(P1, P2, x1, x2):\n",
    "    Xs = []\n",
    "    for (x1, y1), (x2, y2) in zip(x1, x2):\n",
    "        A = np.array([\n",
    "            x1 * P1[2, :] - P1[0, :],\n",
    "            y1 * P1[2, :] - P1[1, :],\n",
    "            x2 * P2[2, :] - P2[0, :],\n",
    "            y2 * P2[2, :] - P2[1, :],\n",
    "        ])\n",
    "        u, s, vh = svd(A)\n",
    "        Xs.append(vh[-1])\n",
    "    return np.array(Xs)\n",
    "        \n",
    "Xr = triangulate(P1, P2, x1, x2)  # triangulate points\n",
    "print(dehomogenize(P1 @ Xr.T).T)  # project back into image\n",
    "print(x1)  # original image points"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extracting cameras"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 9.22311834e+01, -3.29248775e-13,  3.73622013e+02,\n",
       "        -2.02047684e+02],\n",
       "       [-7.83326910e+01,  3.50000000e+02,  6.21609968e+01,\n",
       "         1.25653807e+02],\n",
       "       [-7.83326910e-01, -8.28012465e-16,  6.21609968e-01,\n",
       "         5.32552022e-01]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from numpy.linalg import det\n",
    "\n",
    "def depth(P, X):\n",
    "    \"\"\"Computes the depth of a world point X given a camera P\"\"\"\n",
    "    M = P[:3, :3]\n",
    "    w = (P @ X).flatten()[2]\n",
    "    W = X.flatten()[3]\n",
    "    return np.sign(det(M)) * w / (W * norm(M[-1]))\n",
    "\n",
    "def is_infront(P, X):\n",
    "    return depth(P, X) > 0\n",
    "\n",
    "def extract_cameras(K1, K2, F, x1_test, x2_test):\n",
    "    W = np.array([\n",
    "        [0, -1, 0],\n",
    "        [1,  0, 0],\n",
    "        [0,  0, 1],\n",
    "    ])  # orthonogal (W.T@W = W@W.T = I)\n",
    "    Z = np.array([\n",
    "        [ 0, 1, 0],\n",
    "        [-1, 0, 0],\n",
    "        [ 0, 0, 0],\n",
    "    ])  # skew-symetric (-Z = Z.T)\n",
    "    E = K2.T @ F @ K1\n",
    "    U, s, Vh = svd(E)\n",
    "    u3 = U.T[-1]\n",
    "    \n",
    "    alternatives = [\n",
    "        (U @ W @ Vh, u3),\n",
    "        (U @ W @ Vh, -u3),\n",
    "        (U @ W.T @ Vh, u3),\n",
    "        (U @ W.T @ Vh, -u3),\n",
    "    ]\n",
    "\n",
    "    P1 = K1 @ np.hstack([np.eye(3), np.zeros((3, 1))])\n",
    "    for R, t in alternatives:\n",
    "        P2 = K2 @ np.hstack([R, t[:, None]])\n",
    "        X_test = triangulate(P1, P2, x1_test[None, :], x2_test[None, :]).flatten()\n",
    "        if is_infront(P1, X_test) and is_infront(P2, X_test):\n",
    "            return P1, P2\n",
    "\n",
    "P1, P2 = extract_cameras(K, K, F, x1[3], x2[3])\n",
    "P2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing the Calibration matrix\n",
    "Given a set of corresponding image and world points $\\mathbf{x}_i \\leftrightarrow \\mathbf{X}_i $ compute the projective camera $\\texttt{P}$. We know the world point is projected as $\\mathbf{x} = \\texttt{P}\\mathbf{X}$. Again using the identity $ \\mathbf{v} \\times \\mathbf{v} = \\mathbf{0} $ we can write.\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\mathbf{0}^T & -w_i\\mathbf{X}_i^T & y_i\\mathbf{X}_i^T  \\\\ \n",
    "w_i\\mathbf{X}_i^T & \\mathbf{0}^T & -x_i\\mathbf{X}_i^T  \\\\ \n",
    "y_i\\mathbf{X}_i^T & x_i\\mathbf{X}_i^T & \\mathbf{0}^T\n",
    "\\end{bmatrix} \\mathbf{p} = \\mathbf{0}\n",
    "$$\n",
    "\n",
    "Where $\\mathbf{x}_i = \\left [ x_i, y_i, w_i \\right ] ^T $ and $\\mathbf{p}$ is the elements of $\\texttt{P}$.\n",
    "\n",
    "Furthermore there are only two lineary independent equation and one can be omitted, giving two equations for each correspondance. The $\\texttt{P}$ matrix have 11 degrees of freedom (12 for the elements or $\\texttt{P}$, minus one for overall scale) so 5½ correspondances are needed, where only the x (or y) coordinate of the last image point need to be know.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[  20.95906818  230.32169425]\n",
      " [ 116.88201357  116.13069439]\n",
      " [  77.54330943  890.52896277]\n",
      " [ 240.87768106 -241.53073874]\n",
      " [ -53.45059833 2910.47003561]\n",
      " [ 723.29860691   -8.67321521]\n",
      " [-962.91353737 -896.44044242]\n",
      " [  33.72376323  867.40351999]\n",
      " [ 205.98603604  362.09642768]\n",
      " [ 223.82882128   62.33598994]]\n",
      "[[  20.95906818  230.32169425]\n",
      " [ 116.88201357  116.13069439]\n",
      " [  77.54330943  890.52896277]\n",
      " [ 240.87768106 -241.53073874]\n",
      " [ -53.45059833 2910.47003561]\n",
      " [ 723.29860691   -8.67321521]\n",
      " [-962.91353737 -896.44044242]\n",
      " [  33.72376323  867.40351999]\n",
      " [ 205.98603604  362.09642768]\n",
      " [ 223.82882128   62.33598994]]\n"
     ]
    }
   ],
   "source": [
    "def compute_camera(x, X):\n",
    "    T = normalizing_transform(x)\n",
    "    U = normalizing_transform(X)\n",
    "    w = 1\n",
    "    rows = []\n",
    "    for (x, y), X in zip(transform(T, x), transform(U, X)):\n",
    "        Xh = np.append(X, 1)\n",
    "        rows.append(np.hstack([np.zeros(4), -w * Xh, y * Xh]))\n",
    "        rows.append(np.hstack([w * Xh, np.zeros(4), -x * Xh]))\n",
    "    #A = np.array(rows[:-1])  # skip last element\n",
    "    A = np.array(rows)\n",
    "    u, s, vh = svd(A)\n",
    "    P = vh[-1].reshape((3, 4))\n",
    "    return inv(T) @ P @ U  # denormalize\n",
    "\n",
    "x1 = transform(P1, X)\n",
    "#P = compute_camera(x1[0:6], X[0:6])\n",
    "P = compute_camera(x1, X)\n",
    "print(transform(P, X))\n",
    "print(x1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using thee RQ-factorization $A = RQ$ for any matrix $A$ where $R$ is a _right_ (upper) triangular matrix and $Q$ is an orthonogal matrix, we can find the Calibration matrix $\\texttt{K}$ from a projective camera $P$.\n",
    "\n",
    "$$\n",
    "\\texttt{P} = \\begin{bmatrix} \\texttt{M} | -\\texttt{M}\\tilde{C} \\end{bmatrix} = \\begin{bmatrix} \\texttt{KR} | -\\texttt{KR}\\tilde{C} \\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[350.   0. 160.]\n",
      " [  0. 350. 100.]\n",
      " [  0.   0.   1.]]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(array([[ 3.5000000e+02, -1.7450222e-13,  1.6000000e+02],\n",
       "        [ 0.0000000e+00,  3.5000000e+02,  1.0000000e+02],\n",
       "        [ 0.0000000e+00,  0.0000000e+00,  1.0000000e+00]]),\n",
       " array([[ 1.00000000e+00,  6.38391801e-16, -4.41217369e-17],\n",
       "        [-6.38391801e-16,  1.00000000e+00,  1.37880428e-17],\n",
       "        [ 4.41217369e-17, -1.37880428e-17,  1.00000000e+00]]),\n",
       " array([-2.95516247e-14, -1.20249962e-14,  6.87704033e-15]))"
      ]
     },
     "execution_count": 93,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from scipy.linalg import rq\n",
    "\n",
    "def decompose_camera(P):\n",
    "    M = P[:3, :3]\n",
    "    K, R = rq(M)\n",
    "    T = np.diag(np.sign(np.diag(K)))\n",
    "    if det(T) < 0:\n",
    "        T[1, 1] *= -1\n",
    "    K = K @ T\n",
    "    K = K / K[-1, -1]  # normalize with K[2, 2] == 1\n",
    "    C = inv(-M) @ P[:,3]\n",
    "    return K, T @ R, C\n",
    "\n",
    "print(K)\n",
    "decompose_camera(P)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Real Images\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "b49bd0b4a78346aeb455d49a082d38d8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import ipywidgets\n",
    "from ipycanvas import Canvas\n",
    "import PIL\n",
    "\n",
    "class Clicker:\n",
    "    def __init__(self, filename):\n",
    "        im = PIL.Image.open(filename)\n",
    "        self.canvas = Canvas()\n",
    "        self.scale = self.canvas.width / im.width\n",
    "        self.im = im.resize((int(im.width * self.scale), int(im.height * self.scale)))\n",
    "        self.canvas.put_image_data(np.array(self.im), 0, 0)\n",
    "        self.canvas.on_mouse_down(self)\n",
    "        self._clicks = []\n",
    "\n",
    "    def __call__(self, x, y):\n",
    "        self.canvas.stroke_style = 'white'\n",
    "        self.canvas.stroke_circle(x, y, 5)\n",
    "        self._clicks.append((x, y))\n",
    "\n",
    "    def plot(self, clicks):\n",
    "        self.canvas.stroke_style = 'white'\n",
    "        self.canvas.stroke_circles(self.scale * clicks[:, 0], self.scale * clicks[:, 1], 5 * np.ones(len(clicks)))\n",
    "        \n",
    "    def clicks(self):\n",
    "        return np.array(self._clicks) / self.scale  # scale clicks back to image resolution\n",
    "\n",
    "clicker = Clicker('data/calibration-target.jpeg')\n",
    "xc = np.load('calibration-points.npy')\n",
    "clicker.plot(xc / clicker.scale)\n",
    "clicker.canvas"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[ 1.66510775e+03, -2.99768229e+01,  3.62447692e+02],\n",
       "        [ 0.00000000e+00,  1.54767174e+03,  3.28572941e+02],\n",
       "        [ 0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]),\n",
       " array([[-0.44117229, -0.89498438,  0.06610571],\n",
       "        [-0.42298704,  0.27234152,  0.86424074],\n",
       "        [-0.79148529,  0.35331721, -0.49871633]]),\n",
       " array([1141.13955478, -293.01708214, 1022.96564233]))"
      ]
     },
     "execution_count": 99,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# measured image points\n",
    "#xc = clicker.clicks()\n",
    "xc[:, 1] = height - xc[:, 1]  # negate y-coordinates\n",
    "#print(2816, 1880)  # original resolution\n",
    "\n",
    "# measured world points\n",
    "XC = np.array([\n",
    "    [0, 0, 17],  # top left\n",
    "    [290, 0, 17],  # top right\n",
    "    [290, 287, 17],  # bottom right\n",
    "    [0, 287, 17],  # bottom left\n",
    "    [290/2, 287/2, 17],  # mid point\n",
    "    [0, 0, 0],  # top left (lower)\n",
    "    [290, 0, 0],  # top right (lower)\n",
    "    [290, 287, 0],  # bottom right (lower)\n",
    "])\n",
    "\n",
    "P_calibration = compute_camera(xc, XC)\n",
    "decompose_camera(P_calibration)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "b9d03a44c4884b13bc537fa876e2457a",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "clicker1 = Clicker('data/1.jpeg')\n",
    "x1 = np.load('1.npy')\n",
    "clicker1.plot(x1)\n",
    "clicker1.canvas"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "674cbcc42cc54ebcba418815c3fcd2e4",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "clicker2 = Clicker('data/2.jpeg')\n",
    "x2 = np.load('2.npy')\n",
    "clicker2.plot(x2)\n",
    "clicker2.canvas"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "#x1 = clicker1.clicks()\n",
    "#np.save('1.npy', x1)\n",
    "#print(x1)\n",
    "\n",
    "#x2 = clicker2.clicks()\n",
    "#np.save('2.npy', x2)\n",
    "#print(x2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-4.07436897e-08, -2.68334675e-07,  1.74847275e-04],\n",
       "       [ 2.17605863e-07, -4.18027659e-08,  1.23603206e-03],\n",
       "       [-2.80190983e-04, -1.21060142e-03,  2.68646338e-01]])"
      ]
     },
     "execution_count": 86,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "F = from_correspondences(x1, x2)\n",
    "F"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remembering\n",
    "$$\n",
    "\\mathbf{x'}^T\\texttt{F}\\mathbf{x} = 0\n",
    "$$\n",
    "We can check how close our $\\texttt{F}$ matrix is to satisfy this for the points."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 87,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "-0.005802588547876963\n",
      "0.007741771926977115\n",
      "0.006196753953334633\n",
      "-0.005793169505787121\n",
      "-0.005787415532075624\n",
      "-0.0008459411409751372\n",
      "0.00252370822891379\n",
      "-0.0010460147075077586\n",
      "-0.0013801122118559661\n",
      "0.0028476635917504822\n",
      "-0.016644175609401124\n",
      "0.00821370427332102\n",
      "0.0062735409926570895\n",
      "-0.004547283753034037\n",
      "-0.0007653903225151026\n",
      "0.004232535191603937\n",
      "0.0014048953918988083\n",
      "-0.0031895369724237455\n",
      "0.0003635545182234523\n",
      "-0.0027119640141166634\n",
      "0.002658012163275103\n",
      "-0.0038937730908563672\n",
      "-0.001608612529234943\n"
     ]
    }
   ],
   "source": [
    "for xx1, xx2 in zip(x1, x2):\n",
    "    print((homogenize(xx2[:, None]).T @ F @ homogenize(xx1[:, None])).item())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "98fa45af74c440338f9deca9520989f0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "RoughCanvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plot = Plot2D()\n",
    "plot.image(np.array(clicker2.im))\n",
    "plot.canvas.stroke_style = 'white'\n",
    "plot.lines(from_implicit((F @ homogenize(clicker1.scale * x1.T)).T, clicker1.im.width, clicker1.im.width))\n",
    "plot.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "030ce6f148364fc1bc9b6bfb7a0e28c8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "RoughCanvas()"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plot = Plot2D()\n",
    "plot.image(np.array(clicker1.im))\n",
    "plot.canvas.stroke_style = 'white'\n",
    "plot.lines(from_implicit((F.T @ homogenize(clicker2.scale * x2.T)).T, clicker2.im.width, clicker2.im.width))\n",
    "plot.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 5.38029025,  1.07856962,  1.53913612],\n",
       "       [ 5.5363009 ,  1.03335379,  1.52282094],\n",
       "       [ 5.25672353,  0.92440447,  1.38029661],\n",
       "       [ 5.30946778,  1.14694823,  1.40564353],\n",
       "       [ 4.12622499,  1.19403601,  1.12012074],\n",
       "       [ 3.84781049,  1.08060903,  1.00010755],\n",
       "       [ 7.90341479,  0.96297708,  2.19968901],\n",
       "       [ 0.24924618,  0.9404623 ,  0.21252579],\n",
       "       [-0.07184469,  1.04923489,  0.16506298],\n",
       "       [-0.06778082,  1.04704011,  0.1636718 ],\n",
       "       [-0.04372534,  1.06833738,  0.16575914],\n",
       "       [-0.03776851,  1.05411744,  0.16202525],\n",
       "       [-0.01325263,  1.0291386 ,  0.15617226],\n",
       "       [ 0.43036561,  0.79183564,  0.11726665],\n",
       "       [ 0.4743899 ,  0.78403713,  0.11867822],\n",
       "       [ 0.48215694,  0.77654595,  0.11771756],\n",
       "       [ 0.49296433,  0.76646303,  0.11770903],\n",
       "       [ 0.50203885,  0.75533982,  0.11696503],\n",
       "       [ 0.33688131,  0.87681158,  0.13173176],\n",
       "       [ 0.23709468,  0.86762723,  0.12346725],\n",
       "       [ 0.08500894,  0.89759643,  0.1178643 ],\n",
       "       [-0.39813387,  0.65876067,  0.07208433],\n",
       "       [ 0.21984734,  0.68108922,  0.07318308]])"
      ]
     },
     "execution_count": 100,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "P1, P2 = extract_cameras(K, K, F, x1[3], x2[3])\n",
    "X = triangulate(P1, P2, x1, x2)\n",
    "dehomogenize(X.T).T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Summary\n",
    "* Projective space & implicit lines\n",
    "* The projective camera\n",
    "* Calibration matrix\n",
    "* Fundamental Matrix\n",
    "* Triangulation\n",
    "* Putting it together - Computing camera positions and structure from image correspondences"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Quiz\n",
    "https://samuelcarlssontypeform.typeform.com/to/Mo1xPOlD"
   ]
  },
  {
   "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}