notebooks/01-Pinhole-Camera/pinhole_camera_matrix.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"text-align: center\">\n",
    "<img src=\"../../assets/images/dtlogo.png\" alt=\"Duckietown\" width=\"50%\">\n",
    "</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 💻 01 - Pinhole camera model"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A *pinhole camera* is perhaps the simplest representation of how cameras, including those used on the Duckiebot, produce images of the scene by recording levels of incident light that reflect off objects in the scene and strike the sensor. The *ideal* pinhole camera treats the aperture as being a point, however most cameras couple wider apertures that allow more light to pass through with lenses that focus reflected light. Coupled with models of lens distortion, many cameras used in practice can be modeled as a pihnole camera.\n",
    "\n",
    "<figure>\n",
    "  <div style=\"text-align:center;\">\n",
    "  <img src=\"../../assets/images/pinhole_camera_model/duckie-pinhole.png\", width=500px>\n",
    "  <p>A visualization of a simple pinhole camera.</p>\n",
    "  </div>\n",
    "</figure>\n",
    "\n",
    "The *pinhole camera model* is a mathematical model that describes the projection of a point in a three-dimensional space onto a two-dimensional image plane by an *ideal* pinhole camera, according to perspective projection. Consider a three-dimensional Cartesian reference frame with its origin at the camera (optical) center $C$ and the positive $Z$-axis pointing out of the camera. The $Z$-axis is referred to as the *principal axis*. The two-dimensional image plane is perpendicular to the principal axis and located at a distance $f$ behind the camera center (i.e., $z=-f$), where the parameter $f$ is the focal length. In order to avoid dealing with inverted images, we can treat the image plane as being in the positive $Z$-direction, again at a distance $f$ from the camera center.\n",
    "\n",
    "<figure>\n",
    "  <div style=\"text-align:center;\">\n",
    "  <img src=\"../../assets/images/pinhole_camera_model/pinhole-projection-a.png\", width=500px>\n",
    "  <p>Pinhole camera geometry.</p>\n",
    "  </div>\n",
    "</figure>\n",
    "\n",
    "Consider a point in the reference frame of the camera defined in terms of its Cartesian coordinates $\\tilde{\\mathbf{X}}_\\textrm{cam} = [x \\; y \\; z]^\\top$. We can express this point in *homogeneous coordinates* via the 4-vector, $\\mathbf{X}_\\textrm{cam} = [x \\; y \\; z\\; 1]^\\top$. For those who may not be familiar with homogeneous coordinates, we can express the coordinates of a point that we would typically represent in Cartesian coordinates as $\\tilde{\\mathbf{X}} = [X \\; Y \\; Z]^\\top$, in terms of $\\mathbf{X} = [X \\; Y \\; Z \\; 1]^\\top$, where we append a $1$ at the end. Thus, for any non-zero scalar $\\alpha$, the homogeneous coordinates $[\\alpha X \\; \\alpha Y \\; \\alpha]$ define the same Cartesian coordinates $X, Y$. Transferring a point from homogeneous to Cartesian coordinates involves dividing by the last element of the homogeneous coordinate vector ($1$ in this case) and keeping all but the last entry. Consequently, multiplying the homogeneous vector we can use the homogeneous coordinates $[\\alpha u \\; \\alpha v \\; \\alpha]$ to represent the Cartesian coordinates $\\begin{bmatrix} u \\; v \\end{bmatrix}$.\n",
    "\n",
    "Returning to the camera projection, we can relate the vector $\\mathbf{X}_\\textrm{cam}$ to its projection onto the image $\\mathbf{x}$, expressed in terms of its homogeneous 3-vector as\n",
    "\n",
    "$$ \n",
    "\\begin{align}\n",
    "\\begin{bmatrix}\n",
    "fx\\\\\n",
    "fy\\\\\n",
    "z\n",
    "\\end{bmatrix} &=\n",
    "\\begin{bmatrix}\n",
    "f & 0 & 0 & 0\\\\\n",
    "0 & f & 0 & 0\\\\\n",
    "0 & 0 & 1 & 0\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "x\\\\\n",
    "y\\\\\n",
    "z\\\\\n",
    "1\n",
    "\\end{bmatrix}\\\\\n",
    "\\mathbf{x} &= P \\mathbf{X}_\\textrm{cam}\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "where $P$ is the homogeneous *camera projection matrix*.\n",
    "\n",
    "Thus far, we have assumed that the origin in the image coordinates is at the principal point $\\textbf{p}$, which is the point at which the principal axis intersects the image plane. In practice, the origin may be located elsewhere and the principal point will have coordinates $[p_x \\; p_y]$. Meanwhile, digital cameras may include pixels that are not square, which is sometimes represented as focal lengths $(f_x, f_y)$ that differ in the two image-space directions. Together, this gives rise to a more general expression for the camera matrix:\n",
    "\n",
    "$$ \n",
    "\\mathbf{x} = \n",
    "\\begin{bmatrix}\n",
    "f_x x + p_x z\\\\\n",
    "f_y y + p_y z\\\\\n",
    "z\n",
    "\\end{bmatrix} =\n",
    "\\begin{bmatrix}\n",
    "f_x & 0 & p_x & 0\\\\\n",
    "0 & f_y & p_y & 0\\\\\n",
    "0 & 0 & 1 & 0\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "x\\\\\n",
    "y\\\\\n",
    "z\\\\\n",
    "1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "We can express this as \n",
    "\n",
    "$$\n",
    "\\mathbf{x} = K [I \\; \\vert \\; \\mathbf{0}]\\mathbf{X}_\\textrm{cam} \\qquad \n",
    "K = \n",
    "\\begin{bmatrix}\n",
    "f_x & s & p_x\\\\\n",
    "0 & f_y & p_y\\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "where $K$ is the *camera calibration matrix*, $I$ is a $3 \\times 3$ identity matrix, and $\\mathbf{0}$ is a three-vector of zeros. **Note**: in the intrinsic matrix above, we have introduced an additional *skew* parameter $s$ that represents shear. Oftentimes, the skew is (near-)zero, however we will include it in the intrinsic matrix in the remainder of the exercise for generality."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "### Run this cell to initialize the problem\n",
    "\n",
    "%matplotlib inline\n",
    "\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as  plt\n",
    "from mpl_toolkits.mplot3d import proj3d\n",
    "from matplotlib.patches import FancyArrowPatch\n",
    "\n",
    "class Arrow3D(FancyArrowPatch):\n",
    "    def __init__(self, xs, ys, zs, *args, **kwargs):\n",
    "        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)\n",
    "        self._verts3d = xs, ys, zs\n",
    "\n",
    "    def draw(self, renderer):\n",
    "        xs3d, ys3d, zs3d = self._verts3d\n",
    "        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.M)\n",
    "        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))\n",
    "        FancyArrowPatch.draw(self, renderer)\n",
    "\n",
    "    def do_3d_projection(self, renderer=None):\n",
    "        xs3d, ys3d, zs3d = self._verts3d\n",
    "        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.M)\n",
    "        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))\n",
    "        return np.min(zs)\n",
    "        \n",
    "class CameraProjection:\n",
    "    def __init__(self, width, height, f):\n",
    "        self._fig = plt.figure(figsize=(12,8), dpi= 100, facecolor='w', edgecolor='k')\n",
    "        self._ax = self._fig.add_subplot(111, projection='3d')\n",
    "        \n",
    "        # Coordinates are changed for visualization\n",
    "        # x --> z, y --> x, z --> y\n",
    "        self._ax.set_xlim(0, 4)\n",
    "        self._ax.set_ylim(-(width/2 + 1), width/2 + 1)\n",
    "        self._ax.set_zlim(-(height/2 + 1), height/2 + 1)\n",
    "        self._ax.view_init(elev=15, azim=-75)\n",
    "        self._width = width\n",
    "        self._height = height\n",
    "        self._f = f\n",
    "        self._K = np.array([[f, 0, 0, 0],[0, f, 0, 0],[0, 0, 1, 0]])\n",
    "        self._R = np.array([[0, 0, 1],[1, 0, 0], [0, 1, 0]])\n",
    "\n",
    "    def transform(self, x):\n",
    "        return self._R.dot(x)\n",
    "\n",
    "    def transform(self, x, y, z):\n",
    "        X = np.array([[x, y, z]]).transpose()\n",
    "        return self._R.dot(X)\n",
    "\n",
    "    def drawCameraModel(self):\n",
    "        length = np.minimum(self._width, self._height)/2\n",
    "        arrow_prop_dict = dict(mutation_scale=20, arrowstyle='->', shrinkA=0, shrinkB=0)\n",
    "        a = Arrow3D([0, 0], [0, length], [0, 0], **arrow_prop_dict, color='r')\n",
    "        self._ax.add_artist(a)\n",
    "        a = Arrow3D([0, 0], [0, 0], [0, length], **arrow_prop_dict, color='g')\n",
    "        self._ax.add_artist(a)\n",
    "        a = Arrow3D([0, self._f*4], [0, 0], [0, 0], **arrow_prop_dict, color='b')\n",
    "        self._ax.add_artist(a)\n",
    "        \n",
    "        # Image plane\n",
    "        x = [self._width/2, self._width/2, -self._width/2, -self._width/2, self._width/2]\n",
    "        y = [-self._height/2, self._height/2, self._height/2, -self._height/2, -self._height/2]\n",
    "        z = [self._f, self._f, self._f, self._f, self._f]\n",
    "        self._ax.plot(z, x, y)\n",
    "        self._ax.plot([self._f], [0], [0], 'b.', markersize=12)\n",
    "\n",
    "    def drawProjection(self, Xtilde):\n",
    "        cam.drawCameraModel()\n",
    "        X = np.vstack((Xtilde, 1))\n",
    "        Xtransform =self._R.dot(X[0:3])\n",
    "        x = self._K.dot(X)\n",
    "        xtilde = np.vstack((x[0:2]/x[-1],self._f))\n",
    "        xtransform = self._R.dot(xtilde)\n",
    "        self._ax.set_xlim(0, Xtransform[0] + 2)\n",
    "        ydim = np.array([width/2, np.abs(Xtilde[0])], dtype=object).max()\n",
    "        self._ax.set_ylim(-ydim - 1, ydim + 1)\n",
    "        zdim = np.array([height/2, np.abs(Xtilde[1])], dtype=object).max()\n",
    "        self._ax.set_zlim(-zdim - 1, zdim + 1)\n",
    "        \n",
    "        self._ax.plot3D(xtransform[0], xtransform[1], xtransform[2],'r.', markersize=12)\n",
    "        self._ax.plot3D(Xtransform[0], Xtransform[1], Xtransform[2],'r.', markersize=12)\n",
    "        self._ax.plot3D([0, Xtransform[0][0]], [0,Xtransform[1][0]], [0,  Xtransform[2][0]],'k--')\n",
    "        self._ax.plot3D(xtransform[0], xtransform[1], xtransform[2],'r.', markersize=12)\n",
    "        self._ax.plot3D(Xtransform[0], Xtransform[1], Xtransform[2],'r.', markersize=12)\n",
    "        plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Example: Camera projection\n",
    "\n",
    "In this example, you will experiment with the effect of changing the focal length of the camera. For our purposes, we will assume that the focal lengths in the $x$ and $y$ directions are the same, i.e., $f_x = f_y = f$.\n",
    "\n",
    "The following code will render the projection of a scene point in the camera's reference frame ($\\tilde X$) on to an image of size `width` $\\times$ `height`, with the specified focal length. If you increase the focal length, what happens to the distance between the projected point and the principal point? "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "width = 24                       # The width of the image\n",
    "height = 18                      # The height of the image\n",
    "f = 4                            # The focal length\n",
    "Xtilde = np.array([[5, 5, 15]])  # The Cartesian coordinates of the scene point\n",
    "\n",
    "\n",
    "cam = CameraProjection(width, height, f)\n",
    "cam.drawProjection(Xtilde.transpose())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In many cases, we are interested in the model that describes how points defined with respect to a reference frame differ from that of the camera (e.g., a fixed world coordinate system or the robot's body-fixed reference frame). As we saw previously, we can transform points between different reference frames based on the $3 \\times 3$ rotation matrix $R$ and the three-element translation vector $\\mathbf{t}$.\n",
    "\n",
    "<figure>\n",
    "  <div style=\"text-align:center;\">\n",
    "  <img src=\"../../assets/images/pinhole_camera_model/pinhole-projection-b.png\", width=\"600px\" />\n",
    "  <p>Points expressed in a reference frame other than that of the camera are first transformed into the camera's reference frame before being projected onto the image.</p>\n",
    "  </div>\n",
    "</figure>\n",
    "\n",
    "Consider a scene point $\\tilde{\\mathbf{X}}_w$ represented by its non-homogeneous coordinates with respect to a fixed Cartesian coordinate frame. Let $R$ be the rotation matrix from the world coordinate frame to the camera's coordinate frame, and let $\\mathbf{t}$ be the translation vector from the world frame to the camera frame (i.e., the origin of the world frame expressed in the camera's reference frame). We can express the Cartesian coordinates of the scene point with respect to the camera's coordinate frame as\n",
    "\n",
    "$$\\tilde{\\mathbf{X}}_\\textrm{cam} = R\\tilde{\\mathbf{X}}_w + \\mathbf{t}$$\n",
    "\n",
    "In terms of homogeneous coordinates, this becomes\n",
    "\n",
    "$$\\mathbf{X}_\\textrm{cam} = \n",
    "\\begin{bmatrix}\n",
    "R\\tilde{\\mathbf{X}}_w + \\mathbf{t}\\\\\n",
    "1\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Revisiting the expression for the camera matrix above, we can now include the transformation from the world coordinate frame to the camera's coordinate frame in our pinhole camera model\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "\\mathbf{x} &= K [I \\; \\vert \\; \\mathbf{0}]\\mathbf{X}_\\textrm{cam}\\\\\n",
    "           &= K [I \\; \\vert \\; \\mathbf{0}] \\begin{bmatrix}\n",
    "R\\tilde{\\mathbf{X}}_w + \\mathbf{t}\\\\\n",
    "1\n",
    "\\end{bmatrix}\\\\\n",
    "&= K [R \\; \\vert \\; \\mathbf{t}] \\mathbf{X}_w\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "The first matrix $K$ is referred to as the *intrinsic matrix*, since it includes parameters that are specific (i.e., \"intrinsic\") to the camera, irrespective of where the camera is in the world. The second matrix, which defines the coordinate transformation from the world frame to the camera frame, is referred to as the *extrinsic* matrix, since the transformation does not depend on the specific camera (i.e., the parameters are \"external\" to the particular choice of camera)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now move to the [homographies tutorial](../02-Homographies/homographies.ipynb)."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "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.8.10"
  },
  "vscode": {
   "interpreter": {
    "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}