docs/epipolar_geometry_essential_matrix_fundamental_matrix.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "ada27ce0-6c56-437f-8998-13b9465da31d",
   "metadata": {
    "editable": true,
    "slideshow": {
     "slide_type": ""
    },
    "tags": []
   },
   "source": [
    "## 1.Epipolar Geometry\n",
    "\n",
    "\n",
    "When a 3D scene projected into two cameras views , there are a number of geometric relations between the 3D points and their projections that lead to constraints between the image points.\n",
    "\n",
    "<img src=\"images/Epipolar_geometry.svg\">\n",
    "\n",
    "\n",
    "1. <img src=\"https://latex.codecogs.com/svg.image?O_L\" title=\"https://latex.codecogs.com/svg.image?O_L\" /> and <img src=\"https://latex.codecogs.com/svg.image?O_R\" title=\"https://latex.codecogs.com/svg.image?O_R\" /> represent the optical centers of cameras.\n",
    "\n",
    "2. <img src=\"https://latex.codecogs.com/svg.image?X\" title=\"https://latex.codecogs.com/svg.image?X\" /> represents a 3D point.\n",
    "\n",
    "3. The line connecting <img src=\"https://latex.codecogs.com/svg.image?O_L\" title=\"https://latex.codecogs.com/svg.image?O_L\" /> and <img src=\"https://latex.codecogs.com/svg.image?O_R\" title=\"https://latex.codecogs.com/svg.image?O_R\" /> called **baseline**. \n",
    "\n",
    "\n",
    "4. Points <img src=\"https://latex.codecogs.com/svg.image?x_L\" title=\"https://latex.codecogs.com/svg.image?x_L\" /> and <img src=\"https://latex.codecogs.com/svg.image?x_R\" title=\"https://latex.codecogs.com/svg.image?x_R\" /> are the projections of point X onto the image planes.\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "5. The projection of the different points lying on the line <img src=\"https://latex.codecogs.com/svg.image?O_L&space;-&space;X\" title=\"https://latex.codecogs.com/svg.image?O_L - X\" /> are seen by the left camera as a point, However, the right camera sees these points as a line in its image plane. That line <img src=\"https://latex.codecogs.com/svg.image?l^{%27}\" alt=\"https://latex.codecogs.com/svg.image?l^{'}\" /> (<img src=\"https://latex.codecogs.com/svg.image?e_R&space;-&space;x_R\" title=\"https://latex.codecogs.com/svg.image?e_R - x_R\" />)  in the right camera is called an **epipolar line** or **epiline**. We call it epiline corresponding to the point <img src=\"https://latex.codecogs.com/svg.image?x_l\" alt=\"https://latex.codecogs.com/svg.image?x_l\" /> It means, to find the point <img src=\"https://latex.codecogs.com/svg.image?x\" alt=\"https://latex.codecogs.com/svg.image?x\" /> on the right image, search along this epiline. It should be somewhere on this line. An epipolar line is a function of the position of point <img src=\"https://latex.codecogs.com/svg.image?X\" title=\"https://latex.codecogs.com/svg.image?X\" /> in the 3D space.\n",
    "\n",
    "5. The projection of optical centers of each the cameras lenses onto  the other camera's image plane denoted by <img src=\"https://latex.codecogs.com/svg.image?e_L\" title=\"https://latex.codecogs.com/svg.image?e_L\" /> and <img src=\"https://latex.codecogs.com/svg.image?e_R\" title=\"https://latex.codecogs.com/svg.image?e_R\" />, are called **epipoles** or **epipolar points**. The Baseline contact camera image plane on epipolar points. All the epilines pass through its epipole. So to find the location of epipole, we can find many epilines and find their intersection point.. In some cases you won't be able to locate the epipole in the image, they may be outside the image (which means, one camera doesn't see the other).\n",
    "\n",
    "7.  <img src=\"https://latex.codecogs.com/svg.image?X,&space;O_L&space;\\text{&space;and&space;}&space;O_R\" title=\"https://latex.codecogs.com/svg.image?X, O_L \\text{ and } O_R\" /> form a plane that is called the **epipolar plane**. The epipolar plane intersects each camera's image plane where it forms the epipolar lines.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "161238dd-8d3e-4260-94c4-19dc5942b297",
   "metadata": {},
   "source": [
    "### 1.2 draw epipolar lines\n",
    "```\n",
    "std::vector<cv::Vec3f> rightLines, leftLines;\n",
    "// Find epilines corresponding to points in left image (first image) and drawing its lines on right image\n",
    "cv::computeCorrespondEpilines(projectedPointsInLeftCamera, 1, F, rightLines);\n",
    "\n",
    "// Find epilines corresponding to points in right image (second image) and  drawing its lines on left image\n",
    "cv::computeCorrespondEpilines(projectedPointsInRightCamera, 2, F, leftLines);\n",
    "\n",
    "\n",
    "  // Drawing epipolar lines\n",
    "  for (std::size_t i = 0; i < rightLines.size(); i++) {\n",
    "\n",
    "    std::cout << rightLines.at(i) << std::endl;\n",
    "    cv::Vec3f l = rightLines.at(i);\n",
    "\n",
    "    double a = l.val[0];\n",
    "    double b = l.val[1];\n",
    "    double c = l.val[2];\n",
    "\n",
    "    std::cout << \"a: \" << a << \" b: \" << b << \" c: \" << c << std::endl;\n",
    "\n",
    "    /*ax+by+c=0*/\n",
    "    double x0, y0, x1, y1;\n",
    "    x0 = 0;\n",
    "    y0 = (-c - a * x0) / b;\n",
    "\n",
    "    std::cout << \"x0: \" << x0 << \" y0: \" << y0 << std::endl;\n",
    "\n",
    "    x1 = rightImageRGB.cols;\n",
    "    y1 = (-c - a * x1) / b;\n",
    "\n",
    "    std::cout << \"x1: \" << x1 << \" y1: \" << y1 << std::endl;\n",
    "\n",
    "    std::cout << \"error: \"\n",
    "              << a * projectedPointsInRightCamera.at(i).x +\n",
    "                     b * projectedPointsInRightCamera.at(i).y + c\n",
    "              << std::endl;\n",
    "\n",
    "    // cv::line(right_image, cvPoint(x0,y0),\n",
    "    // cvPoint(right_image.cols,right_image.rows), cvScalar(0,255,0), 1);\n",
    "    cv::line(rightImageRGB, cv::Point(x0, y0), cv::Point(x1, y1),\n",
    "             cv::Scalar(0, 255, 0), 1);\n",
    "    cv::imshow(\"right_image_epipolarline\", rightImageRGB);\n",
    "    cv::waitKey(0);\n",
    "  }\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1bee1bf7-8266-40dc-aa56-f6c1a0be48dd",
   "metadata": {},
   "source": [
    "<img src=\"images/left_cam_image_2.000000_.jpg\" />\n",
    "<img src=\"images/right_cam_image_2.000000_.jpg\" />\n",
    "\n",
    "<img src=\"images/leftImageRGB.jpg\" />\n",
    "<img src=\"images/rightImageRGB.jpg\" />\n",
    "\n",
    "\n",
    "<img src=\"images/Good_Matches_Epipolar_Geometry.png\" />\n",
    "<img src=\"images/Correspond_Epilines_from_points_in_image.png\" />\n",
    "\n",
    "\n",
    "<img src=\"images/right_image_epipolarline.png\" />\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a377ac1c-642d-4e9d-b2ec-f9ad6ace1683",
   "metadata": {},
   "source": [
    "c++ code: [1](../src/epipolar_geometry.cpp), [2](../src/essential_matrix_estimation.cpp), [3](../src/fundamental_matrix_estimation.cpp)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "673ba7d4-d7fa-48fd-8c30-ad566e314280",
   "metadata": {},
   "source": [
    "## 2.Essential Matrix\n",
    "\n",
    " Essential matrix <img src=\"https://latex.codecogs.com/svg.image?\\mathbf{E}\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{E}\" /> is a <img src=\"https://latex.codecogs.com/svg.image?3\\times&space;3&space;\" title=\"https://latex.codecogs.com/svg.image?3\\times 3 \" /> that relates optical center of cameras\n",
    "<img src=\"images/Epipolar_geometry.svg\" />\n",
    "\n",
    "We can relate any two frames by a rotation matrix and a translation vector:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?O_L=RO_R&plus;T&space;\" title=\"https://latex.codecogs.com/svg.image?O_L=RO_R+T \" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "We cross product both side by <img src=\"https://latex.codecogs.com/svg.image?T\" title=\"https://latex.codecogs.com/svg.image?T\" />:\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?T\\times&space;O_L=T\\times&space;RO_R&plus;&space;T\\times&space;T&space;\" title=\"https://latex.codecogs.com/svg.image?T\\times O_L=T\\times RO_R+ T\\times T \" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "For any vector cross product of with itself is zero:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?T&space;\\times&space;T=0\" title=\"https://latex.codecogs.com/svg.image?T \\times T=0\" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "We dot product both side by <img src=\"https://latex.codecogs.com/svg.latex?O_%7BL_%7B1%5Ctimes%203%7D%7D%5ET\" title=\"https://latex.codecogs.com/svg.image?O_{L_{1\\times 3}}^T\" />:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?O_%7BL%7D%5ET.%28T%5Ctimes%20O_L%29%3DO_%7BL%7D%5ET.%28T%5Ctimes%20RO_R%29\" title=\"https://latex.codecogs.com/svg.image?O_{L}^T.(T\\times O_L)=O_{L}^T.(T\\times RO_R) \" />\n",
    "<br/>\n",
    "<br/>\n",
    "on the lhs, <img src=\"https://latex.codecogs.com/svg.image?O_L\" title=\"https://latex.codecogs.com/svg.image?O_L\" />   is perpendicular to <img src=\"https://latex.codecogs.com/svg.image?T\\times&space;O_L\" title=\"https://latex.codecogs.com/svg.image?T\\times O_L\" />\n",
    "so the result is zero vector, also we can write any cross product as skew-symmetric matrix multiplication, therefore:\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?0_%7B1%20%5Ctimes%201%7D%3DO_%7BL_%7B1%5Ctimes%203%7D%7D%5ET%20%5Cleft%20%5Clfloor%20T%20%5Cright%20%5Crfloor_%7B3%5Ctimes%203%7D%20R_%7B3%5Ctimes%203%7D%20O_%7BR_%7B3%20%5Ctimes%201%7D%20%7D\" title=\"https://latex.codecogs.com/svg.image?0_{1 \\times 1}=O_{L_{1\\times 3}}^T \\left \\lfloor T \\right \\rfloor_{3\\times 3} R_{3\\times 3} O_{R_{3 \\times 1} }\" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=O_L^T&space;\\mathbf{E}&space;O_R&space;&space;\" title=\"https://latex.codecogs.com/svg.image?0=O_L^T \\mathbf{E} O_R \" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\mathbf{E}=\\left&space;\\lfloor&space;T&space;\\right&space;\\rfloor_{3\\times&space;3}&space;&space;&space;&space;&space;&space;R_{3\\times&space;3}\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{E}=\\left \\lfloor T \\right \\rfloor_{3\\times 3} R_{3\\times 3}\" />\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0b968735-09b7-43b2-8492-99a656f02cc9",
   "metadata": {},
   "source": [
    "### 2.1 Properties of the essential matrix\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5Cmathbf%7BE%7D%3D%5Cleft%20%5Clfloor%20T%20%5Cright%20%5Crfloor_%7B3%5Ctimes%203%7D%20R_%7B3%5Ctimes%203%7D%3DSR\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{E}=\\left \\lfloor T \\right \\rfloor_{3\\times 3} R_{3\\times 3}=SR\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "where `S` is skew-symmetric matrix.\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20%5Cmathbf%7BF%7D%3D%5Cmathbf%7BC%7D%5E%7B-T%7D_L%20%5Cmathbf%7BE%7D%20%5Cmathbf%7BC%7D%5E%7B-1%7D_R%20%5C%5C%20%5Cmathbf%7BE%7D%3D%5Cmathbf%7BC%7D%5E%7BT%7D_L%20%5Cmathbf%7BF%7D%20%5Cmathbf%7BC%7D_R\" title=\"https://latex.codecogs.com/svg.image?\\\\ \\mathbf{F}=\\mathbf{C}^{-T}_L \\mathbf{E} \\mathbf{C}^{-1}_R \\\\ \\mathbf{E}=\\mathbf{C}^{T}_L \\mathbf{F} \\mathbf{C}_R\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "It is possible to bring every skew-symmetric matrix to a block diagonal form by a special orthogonal transformation:\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?A=Q\\Sigma%20Q^T\" alt=\"https://latex.codecogs.com/svg.latex?A=Q\\Sigma Q^T\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%7B%5Cdisplaystyle%20Q%5E%7B%5Cmathrm%20%7BT%7D%20%7DQ%3DQQ%5E%7B%5Cmathrm%20%7BT%7D%20%7D%3DI%2C%7D\" alt=\"https://latex.codecogs.com/svg.latex?{\\displaystyle Q^{\\mathrm {T} }Q=QQ^{\\mathrm {T} }=I,}\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "where <img src=\"https://latex.codecogs.com/svg.latex?\\Sigma\" alt=\"https://latex.codecogs.com/svg.latex?\\Sigma\" /> is a **block-diagonal** matrix\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%7B%5Cdisplaystyle%20%5CSigma%20%3D%7B%5Cbegin%7Bbmatrix%7D%7B%5Cbegin%7Bmatrix%7D0%26%5Clambda%20_%7B1%7D%5C%5C-%5Clambda%20_%7B1%7D%260%5Cend%7Bmatrix%7D%7D%260%26%5Ccdots%20%260%5C%5C0%26%7B%5Cbegin%7Bmatrix%7D0%26%5Clambda%20_%7B2%7D%5C%5C-%5Clambda%20_%7B2%7D%260%5Cend%7Bmatrix%7D%7D%26%260%5C%5C%5Cvdots%20%26%26%5Cddots%20%26%5Cvdots%20%5C%5C0%260%26%5Ccdots%20%26%7B%5Cbegin%7Bmatrix%7D0%26%5Clambda%20_%7Br%7D%5C%5C-%5Clambda%20_%7Br%7D%260%5Cend%7Bmatrix%7D%7D%5C%5C%26%26%26%26%7B%5Cbegin%7Bmatrix%7D0%5C%5C%26%5Cddots%20%5C%5C%26%260%5Cend%7Bmatrix%7D%7D%5Cend%7Bbmatrix%7D%7D%7D\" alt=\"https://latex.codecogs.com/svg.latex?{\\displaystyle \\Sigma ={\\begin{bmatrix}{\\begin{matrix}0&\\lambda _{1}\\\\-\\lambda _{1}&0\\end{matrix}}&0&\\cdots &0\\\\0&{\\begin{matrix}0&\\lambda _{2}\\\\-\\lambda _{2}&0\\end{matrix}}&&0\\\\\\vdots &&\\ddots &\\vdots \\\\0&0&\\cdots &{\\begin{matrix}0&\\lambda _{r}\\\\-\\lambda _{r}&0\\end{matrix}}\\\\&&&&{\\begin{matrix}0\\\\&\\ddots \\\\&&0\\end{matrix}}\\end{bmatrix}}}\" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "we can write it as:\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?Z%3D%5Cbegin%7Bbmatrix%7D%200%20%26%201%20%5C%5C%20-1%20%26%200%5C%5C%20%5Cend%7Bbmatrix%7D\" alt=\"https://latex.codecogs.com/svg.latex?Z=\\begin{bmatrix}\n",
    "0 & 1 \\\\ \n",
    "-1 & 0\\\\ \n",
    "\\end{bmatrix}\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?\\Sigma=diag%28a_1Z%2Ca_2Z%2C...%2Ca_mZ%2C0%2C...%2C0%29\" alt=\"https://latex.codecogs.com/svg.latex?\\Sigma=diag(a_1Z,a_2Z,...,a_mZ,0,...,0)\" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "A 3×3 matrix is an essential matrix if and only if two of its singular values are equal, and the third is zero.\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?W%3D%5Cbegin%7Bbmatrix%7D%200%20%26%20-1%20%26%200%5C%5C%201%20%26%200%20%26%200%5C%5C%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D\" alt=\"https://latex.codecogs.com/svg.latex?W=\\begin{bmatrix}\n",
    "0 & -1 & 0\\\\ \n",
    "1 & 0 & 0\\\\ \n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}\" />\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?Z%3D%5Cbegin%7Bbmatrix%7D%200%20%26%201%20%26%200%5C%5C%20-1%20%26%200%20%26%200%5C%5C%200%20%26%200%20%26%200%20%5Cend%7Bbmatrix%7D\" alt=\"https://latex.codecogs.com/svg.latex?Z=\\begin{bmatrix}\n",
    "0 & 1 & 0\\\\ \n",
    "-1 & 0 & 0\\\\ \n",
    "0 & 0 & 0\n",
    "\\end{bmatrix}\" />\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20S%3DkUZU%5ET%20%5C%5C%20Z%3Ddiag%281%2C1%2C0%29W%20%5C%5C%20E%3DSR%3DkUdiag%281%2C1%2C0%29WU%5ETR\" alt=\"https://latex.codecogs.com/svg.latex?S=kUZU^T\n",
    "\\\\\n",
    "Z=diag(1,1,0)W\n",
    "\\\\\n",
    "E=SR=kUdiag(1,1,0)WU^TR\" />\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20E%3DSR%3DUdiag%281%2C1%2C0%29V%5E%7BT%7D%20%5C%5C%20P%3D%5BI%7C0%5D%20%5C%5C%20P_1%3D%5BUWV%5ET%7C&plus;u_3%5D%20%5C%5C%20P_2%3D%5BUWV%5ET%7C-u_3%5D%20%5C%5C%20P_3%3D%5BUW%5ETV%5ET%7C&plus;u_3%5D%20%5C%5C%20P_4%3D%5BUW%5ETV%5ET%7C-u_3%5D\" alt=\"https://latex.codecogs.com/svg.latex?\\\\\n",
    "E=SR=Udiag(1,1,0)V^{T}\n",
    "\\\\\n",
    "P=[I|0]\n",
    "\\\\\n",
    "P_1=[UWV^T|+u_3]\n",
    "\\\\\n",
    "P_2=[UWV^T|-u_3]\n",
    "\\\\\n",
    "P_3=[UW^TV^T|+u_3]\n",
    "\\\\\n",
    "P_4=[UW^TV^T|-u_3]\" />\n",
    "\n",
    "\n",
    "\n",
    "Suppose that the SVD of E is\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?E%3DUdiag%281%2C1%2C0%29V%5ET\" alt=\"https://latex.codecogs.com/svg.latex?E=Udiag(1,1,0)V^T\" />\n",
    "\n",
    "\n",
    "\n",
    "There are two possible factorizations E=SR\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20S%3DUZU%5ET%20%5C%5C%20%5C%5C%20R%3D%20%5Cleft%5C%7B%5Cbegin%7Bmatrix%7D%20UWV%5ET%20%5C%5C%20or%20%5C%5C%20UW%5ETV%5ET%20%5Cend%7Bmatrix%7D%5Cright.\" alt=\"https://latex.codecogs.com/svg.latex?\\\\\n",
    "S=UZU^T\n",
    "\\\\\n",
    "\\\\\n",
    "R=\n",
    "\\left\\{\\begin{matrix}\n",
    "UWV^T\n",
    "\\\\ \n",
    "or\n",
    "\\\\\n",
    "UW^TV^T\n",
    "\\end{matrix}\\right.\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61e77b50-faa4-4228-96ca-a48383073c41",
   "metadata": {},
   "source": [
    "## 3. Fundamental Matrix \n",
    "Fundamental Matrix contains the same information as Essential Matrix in addition to the information about the intrinsics of both cameras so that we can relate the two cameras in pixel coordinates. \n",
    "If we are using **rectified** images and **normalize the point** by dividing by the focal lengths, <img src=\"https://latex.codecogs.com/svg.image?F=E\" alt=\"https://latex.codecogs.com/svg.image?F=E\" />. In simple words, Fundamental Matrix F, maps a point in one image to a line (epiline) in the other image. This is calculated from matching points from both the images.\n",
    "\n",
    "In <img src=\"https://latex.codecogs.com/svg.image?F\" title=\"https://latex.codecogs.com/svg.image?F\" /> if we set a point on the left camera <img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}&space;u&&space;v&space;&&space;1&space;\\\\\\end{bmatrix}\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix} u& v & 1 \\\\\\end{bmatrix}\" /> it will give us a line on the other camera.\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u&space;&&space;v&space;&&space;1&space;\\\\\\end{bmatrix}\\begin{bmatrix}f_{11}&space;&&space;f_{12}&space;&&space;f_{13}&space;\\\\f_{21}&space;&&space;f_{22}&space;&&space;f_{23}&space;\\\\f_{31}&space;&&space;f_{32}&space;&&space;f_{33}&space;\\\\\\end{bmatrix}\\begin{bmatrix}u'&space;\\\\v'&space;\\\\1\\end{bmatrix}=0\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u & v & 1 \\\\\\end{bmatrix}\\begin{bmatrix}f_{11} & f_{12} & f_{13} \\\\f_{21} & f_{22} & f_{23} \\\\f_{31} & f_{32} & f_{33} \\\\\\end{bmatrix}\\begin{bmatrix}u' \\\\v' \\\\1\\end{bmatrix}=0\" />\n",
    "\n",
    "\n",
    "\n",
    "### 3.1 Epipolar Constraint and Triangulation\n",
    "\n",
    "If the points <img src=\"https://latex.codecogs.com/svg.image?x_L&space;\\text{&space;and&space;}&space;x_R\" title=\"https://latex.codecogs.com/svg.image?x_L \\text{ and } x_R\" /> are known, their projection lines can be found via camera matrix. These lines should intersect precisely at <img src=\"https://latex.codecogs.com/svg.image?X\" title=\"https://latex.codecogs.com/svg.image?X\" />. This process called **triangulation**.\n",
    "\n",
    "\n",
    "\n",
    "<br/>\n",
    "\n",
    "\n",
    "If we have camera matrix we can compute the **ray** that goes the through optical center of <img src=\"https://latex.codecogs.com/svg.image?O_L\" title=\"https://latex.codecogs.com/svg.image?O_L\" /> (left cameras) and <img src=\"https://latex.codecogs.com/svg.image?O_R\" title=\"https://latex.codecogs.com/svg.image?O_R\" /> (right camera) and the point <img src=\"https://latex.codecogs.com/svg.image?X\" title=\"https://latex.codecogs.com/svg.image?X\" /> :\n",
    "\n",
    "\n",
    "<img src=\"images/TriangulationIdeal.svg\" />  \n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\mathbf{y}_1=&space;\\begin{bmatrix}y_1{_u}&space;\\\\y_1_{v}&space;\\\\1\\end{bmatrix},\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{y}_1= \\begin{bmatrix}y_1{_u} \\\\y_1_{v} \\\\1\\end{bmatrix},\" />\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\mathbf{y}_2=&space;\\begin{bmatrix}y_2{_u}&space;\\\\y_2_{v}&space;\\\\1\\end{bmatrix}\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{y}_2= \\begin{bmatrix}y_2{_u} \\\\y_2_{v} \\\\1\\end{bmatrix}\" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?ray_1=\\mathbf{C}^{-1}_1{_{3\\times3}}&space;\\times&space;\\begin{bmatrix}y_1{_u}&space;\\\\y_1_{v}&space;\\\\1\\end{bmatrix}=\\mathbf{C}^{-1}_1\\mathbf{y}_1\" title=\"https://latex.codecogs.com/svg.image?ray_1=\\mathbf{C}^{-1}_1{_{3\\times3}} \\times \\begin{bmatrix}y_1{_u} \\\\y_1_{v} \\\\1\\end{bmatrix}=\\mathbf{C}^{-1}_1\\mathbf{y}_1\" />\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?ray_2=\\mathbf{C}^{-1}_2{_{3\\times3}}&space;\\times&space;\\begin{bmatrix}y_2{_u}&space;\\\\y_2_{v}&space;\\\\1\\end{bmatrix}=\\mathbf{C}^{-1}_2\\mathbf{y}_2\" title=\"https://latex.codecogs.com/svg.image?ray_2=\\mathbf{C}^{-1}_2{_{3\\times3}} \\times \\begin{bmatrix}y_2{_u} \\\\y_2_{v} \\\\1\\end{bmatrix}=\\mathbf{C}^{-1}_2\\mathbf{y}_2\" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "Also from essential matrix we know:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=O_L^T&space;\\mathbf{E}&space;O_R&space;&space;\" title=\"https://latex.codecogs.com/svg.image?0=O_L^T \\mathbf{E} O_R \" />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "if we substitute them in the upper equation:\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=(\\mathbf{C}^{-1}_L\\mathbf{y}_L)^T&space;\\mathbf{E}&space;\\mathbf{C}^{-1}_R\\mathbf{y}_R\" title=\"https://latex.codecogs.com/svg.image?0=(\\mathbf{C}^{-1}_L\\mathbf{y}_L)^T \\mathbf{E} \\mathbf{C}^{-1}_R\\mathbf{y}_R\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L&space;^T\\mathbf{C}^{-T}_L&space;\\mathbf{E}&space;\\mathbf{C}^{-1}_R\\mathbf{y}_R\" title=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L ^T\\mathbf{C}^{-T}_L \\mathbf{E} \\mathbf{C}^{-1}_R\\mathbf{y}_R\" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "we call the <img src=\"https://latex.codecogs.com/svg.latex?%5Cmathbf%7BC%7D%5E%7B-T%7D_L%20%5Cmathbf%7BE%7D%20%5Cmathbf%7BC%7D%5E%7B-1%7D_R\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{C}^{-T}_L \\mathbf{E} \\mathbf{C}^{-1}_R\" /> matrix **Fundamental Matrix**: \n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20%5Cmathbf%7BF%7D%3D%5Cmathbf%7BC%7D%5E%7B-T%7D_L%20%5Cmathbf%7BE%7D%20%5Cmathbf%7BC%7D%5E%7B-1%7D_R%20%5C%5C%20%5Cmathbf%7BE%7D%3D%5Cmathbf%7BC%7D%5E%7BT%7D_L%20%5Cmathbf%7BF%7D%20%5Cmathbf%7BC%7D_R\" title=\"https://latex.codecogs.com/svg.image?\\\\ \\mathbf{F}=\\mathbf{C}^{-T}_L \\mathbf{E} \\mathbf{C}^{-1}_R \\\\ \\mathbf{E}=\\mathbf{C}^{T}_L \\mathbf{F} \\mathbf{C}_R\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L&space;^T\\mathbf{F}&space;\\mathbf{y}_R\" title=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L ^T\\mathbf{F} \\mathbf{y}_R\" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "Now how to compute it:\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u&space;&&space;v&space;&&space;1&space;\\\\\\end{bmatrix}\\begin{bmatrix}f_{11}&space;&&space;f_{12}&space;&&space;f_{13}&space;\\\\f_{21}&space;&&space;f_{22}&space;&&space;f_{23}&space;\\\\f_{31}&space;&&space;f_{32}&space;&&space;f_{33}&space;\\\\\\end{bmatrix}\\begin{bmatrix}u'&space;\\\\v'&space;\\\\1\\end{bmatrix}=0\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u & v & 1 \\\\\\end{bmatrix}\\begin{bmatrix}f_{11} & f_{12} & f_{13} \\\\f_{21} & f_{22} & f_{23} \\\\f_{31} & f_{32} & f_{33} \\\\\\end{bmatrix}\\begin{bmatrix}u' \\\\v' \\\\1\\end{bmatrix}=0\" />\n",
    "\n",
    "if we write this equation for 8 points, we can arrange the following matrix:\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u'_1u_1&space;&&space;u'_1v_1&space;&&space;u'_1&space;&&space;v'_1u_1&space;&&space;v'_1v_1&space;&&space;v'_1&space;&&space;u_1&space;&&space;v_1&space;&&space;1&space;\\\\u'_2u_2&space;&&space;u'_2v_2&space;&&space;u'_2&space;&&space;v'_2u_2&space;&&space;v'_2v_2&space;&&space;v'_2&space;&&space;u_2&space;&&space;v_2&space;&&space;1&space;\\\\u'_3u_3&space;&&space;u'_3v_3&space;&&space;u'_3&space;&&space;v'_3u_3&space;&&space;v'_3v_3&space;&&space;v'_3&space;&&space;u_3&space;&&space;v_3&space;&&space;1&space;\\\\u'_4u_4&space;&&space;u'_4v_4&space;&&space;u'_4&space;&&space;v'_4u_4&space;&&space;v'_4v_4&space;&&space;v'_4&space;&&space;u_4&space;&&space;v_4&space;&&space;1&space;\\\\u'_5u_5&space;&&space;u'_5v_5&space;&&space;u'_5&space;&&space;v'_5u_5&space;&&space;v'_5v_5&space;&&space;v'_5&space;&&space;u_5&space;&&space;v_5&space;&&space;1&space;\\\\u'_6u_6&space;&&space;u'_6v_6&space;&&space;u'_6&space;&&space;v'_6u_6&space;&&space;v'_6v_6&space;&&space;v'_6&space;&&space;u_6&space;&&space;v_6&space;&&space;1&space;\\\\u'_7u_7&space;&&space;u'_7v_7&space;&&space;u'_7&space;&&space;v'_7u_7&space;&&space;v'_7v_7&space;&&space;v'_7&space;&&space;u_7&space;&&space;v_7&space;&&space;1&space;\\\\u'_8u_8&space;&&space;u'_8v_8&space;&&space;u'_8&space;&&space;v'_8u_8&space;&&space;v'_8v_8&space;&&space;v'_8&space;&&space;u_8&space;&&space;v_8&space;&&space;1&space;\\\\\\end{bmatrix}\\begin{bmatrix}f_{11}&space;\\\\&space;f_{12}&space;\\\\&space;f_{13}&space;\\\\f_{21}&space;\\\\&space;f_{22}&space;\\\\&space;f_{23}&space;\\\\f_{31}&space;\\\\&space;f_{32}&space;\\\\&space;f_{33}&space;\\\\\\end{bmatrix}=0\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u'_1u_1 & u'_1v_1 & u'_1 & v'_1u_1 & v'_1v_1 & v'_1 & u_1 & v_1 & 1 \\\\u'_2u_2 & u'_2v_2 & u'_2 & v'_2u_2 & v'_2v_2 & v'_2 & u_2 & v_2 & 1 \\\\u'_3u_3 & u'_3v_3 & u'_3 & v'_3u_3 & v'_3v_3 & v'_3 & u_3 & v_3 & 1 \\\\u'_4u_4 & u'_4v_4 & u'_4 & v'_4u_4 & v'_4v_4 & v'_4 & u_4 & v_4 & 1 \\\\u'_5u_5 & u'_5v_5 & u'_5 & v'_5u_5 & v'_5v_5 & v'_5 & u_5 & v_5 & 1 \\\\u'_6u_6 & u'_6v_6 & u'_6 & v'_6u_6 & v'_6v_6 & v'_6 & u_6 & v_6 & 1 \\\\u'_7u_7 & u'_7v_7 & u'_7 & v'_7u_7 & v'_7v_7 & v'_7 & u_7 & v_7 & 1 \\\\u'_8u_8 & u'_8v_8 & u'_8 & v'_8u_8 & v'_8v_8 & v'_8 & u_8 & v_8 & 1 \\\\\\end{bmatrix}\\begin{bmatrix}f_{11} \\\\ f_{12} \\\\ f_{13} \\\\f_{21} \\\\ f_{22} \\\\ f_{23} \\\\f_{31} \\\\ f_{32} \\\\ f_{33} \\\\\\end{bmatrix}=0\" />\n",
    "<br/>\n",
    "<br/>\n",
    "the above equation has zero on the rhs, so we can use SVD to solve it,\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?F^{*}%20\\underset{H}{\\mathrm{argmin}}=%20\\|AF\\|^{2}\" title=\"https://latex.codecogs.com/svg.image?F^{*} \\underset{H}{\\mathrm{argmin}}= \\|AF\\|^{2}\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "Singular-value Decomposition (SVD) of any given matrix <img src=\"https://latex.codecogs.com/svg.image?A_{M{\\times}N}\" title=\"https://latex.codecogs.com/svg.image?A_{M{\\times}N}\" />\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\begin{equation}&space;\\underbrace{\\mathbf{A}}_{M&space;\\times&space;N}&space;=&space;\\underbrace{\\mathbf{U}}_{M&space;\\times&space;M}&space;\\times&space;\\underbrace{\\mathbf{\\Sigma}}_{M\\times&space;N}&space;\\times&space;\\underbrace{\\mathbf{V}^{\\text{T}}}_{N&space;\\times&space;N}&space;\\end{equation}\" title=\"https://latex.codecogs.com/svg.image?\\begin{equation} \\underbrace{\\mathbf{A}}_{M \\times N} = \\underbrace{\\mathbf{U}}_{M \\times M} \\times \\underbrace{\\mathbf{\\Sigma}}_{M\\times N} \\times \\underbrace{\\mathbf{V}^{\\text{T}}}_{N \\times N} \\end{equation}\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "- <img src=\"https://latex.codecogs.com/svg.image?U\" title=\"https://latex.codecogs.com/svg.image?U\" /> is an <img src=\"https://latex.codecogs.com/svg.image?M\\times&space;M\" title=\"https://latex.codecogs.com/svg.image?M\\times M\" /> matrix with orthogonal matrix (columns are eigen vectors of A).\n",
    "- <img src=\"https://latex.codecogs.com/svg.image?\\Sigma\" title=\"https://latex.codecogs.com/svg.image?\\Sigma\" /> is an <img src=\"https://latex.codecogs.com/svg.image?M\\times&space;N\" title=\"https://latex.codecogs.com/svg.image?M\\times N\" /> matrix with non-negative entries, termed the singular values  (diagonal entries are eigen values of A).\n",
    "- <img src=\"https://latex.codecogs.com/svg.image?V\" title=\"https://latex.codecogs.com/svg.image?V\" /> is an <img src=\"https://latex.codecogs.com/svg.image?N\\times&space;N\" title=\"https://latex.codecogs.com/svg.image?N\\times N\" /> orthogonal matrix.\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?F^{*}\" title=\"https://latex.codecogs.com/svg.image?F^{*}\" /> is the last column of <img src=\"https://latex.codecogs.com/svg.image?V\" title=\"https://latex.codecogs.com/svg.image?V\" />\n",
    "\n",
    "\n",
    "However, this is not the complete answer, let have a review: the cross product can be written as:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\mathbf&space;{a}&space;\\times&space;\\mathbf&space;{b}&space;=[\\mathbf&space;{a}&space;]_{\\times&space;}\\mathbf&space;{b}\" title=\"https://latex.codecogs.com/svg.image?\\mathbf {a} \\times \\mathbf {b} =[\\mathbf {a} ]_{\\times }\\mathbf {b}\" />\n",
    "\n",
    "\n",
    "where:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?[\\mathbf&space;{a}&space;]_{\\times&space;}={\\begin{bmatrix}\\,\\,0&\\!-a_{3}&\\,\\,\\,a_{2}\\\\\\,\\,\\,a_{3}&0&\\!-a_{1}\\\\\\!-a_{2}&\\,\\,a_{1}&\\,\\,0\\end{bmatrix}}\" title=\"https://latex.codecogs.com/svg.image?[\\mathbf {a} ]_{\\times }={\\begin{bmatrix}\\,\\,0&\\!-a_{3}&\\,\\,\\,a_{2}\\\\\\,\\,\\,a_{3}&0&\\!-a_{1}\\\\\\!-a_{2}&\\,\\,a_{1}&\\,\\,0\\end{bmatrix}}\" />\n",
    "\n",
    "This matrix has rank of 2 and if we multiply it by any matrix, that would also have rank 2.\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\Sigma=\\begin{bmatrix}&space;r&&space;0&space;&&space;0&space;\\\\&space;0&&space;&space;s&&space;&space;0\\\\&space;0&&space;&space;0&&space;t&space;\\\\\\end{bmatrix}\" title=\"https://latex.codecogs.com/svg.image?\\Sigma=\\begin{bmatrix} r& 0 & 0 \\\\ 0& s& 0\\\\ 0& 0& t \\\\\\end{bmatrix}\" />\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?r>s>t\" title=\"https://latex.codecogs.com/svg.image?r>s>t\" />\n",
    "<br/>\n",
    "<br/>\n",
    "so we set the <img src=\"https://latex.codecogs.com/svg.image?t=0\" title=\"https://latex.codecogs.com/svg.image?t=0\" /> is \n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\hat{\\Sigma}&space;=\\begin{bmatrix}&space;r&&space;0&space;&&space;0&space;\\\\&space;0&&space;&space;s&&space;&space;0\\\\&space;0&&space;&space;0&&space;0&space;\\\\\\end{bmatrix}\" title=\"https://latex.codecogs.com/svg.image?\\hat{\\Sigma} =\\begin{bmatrix} r& 0 & 0 \\\\ 0& s& 0\\\\ 0& 0& 0 \\\\\\end{bmatrix}\" />\n",
    "\n",
    "\n",
    "and compute <img src=\"https://latex.codecogs.com/svg.image?\\hat{A}\" title=\"https://latex.codecogs.com/svg.image?\\hat{A}\" />:\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\hat{A}=U\\times&space;\\hat{\\Sigma}&space;&space;\\times&space;V^T\" title=\"https://latex.codecogs.com/svg.image?\\hat{A}=U\\times \\hat{\\Sigma} \\times V^T\" />\n",
    "\n",
    "\n",
    "If we compute the SVD of <img src=\"https://latex.codecogs.com/svg.image?\\hat{A}\" title=\"https://latex.codecogs.com/svg.image?\\hat{A}\" /> the last column of <img src=\"https://latex.codecogs.com/svg.image?V\" title=\"https://latex.codecogs.com/svg.image?V\" /> is <img src=\"https://latex.codecogs.com/svg.image?F^{*}\" title=\"https://latex.codecogs.com/svg.image?F^{*}\" /> that we looking for.\n",
    "\n",
    "Now that we have <img src=\"https://latex.codecogs.com/svg.image?F\" title=\"https://latex.codecogs.com/svg.image?F\" /> if we set a point on the left camera <img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}&space;u&&space;v&space;&&space;1&space;\\\\\\end{bmatrix}\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix} u& v & 1 \\\\\\end{bmatrix}\" /> it will give us a line on the other camera.\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u&space;&&space;v&space;&&space;1&space;\\\\\\end{bmatrix}\\begin{bmatrix}f_{11}&space;&&space;f_{12}&space;&&space;f_{13}&space;\\\\f_{21}&space;&&space;f_{22}&space;&&space;f_{23}&space;\\\\f_{31}&space;&&space;f_{32}&space;&&space;f_{33}&space;\\\\\\end{bmatrix}\\begin{bmatrix}u'&space;\\\\v'&space;\\\\1\\end{bmatrix}=0\" title=\"https://latex.codecogs.com/svg.image?\\begin{bmatrix}u & v & 1 \\\\\\end{bmatrix}\\begin{bmatrix}f_{11} & f_{12} & f_{13} \\\\f_{21} & f_{22} & f_{23} \\\\f_{31} & f_{32} & f_{33} \\\\\\end{bmatrix}\\begin{bmatrix}u' \\\\v' \\\\1\\end{bmatrix}=0\" />\n",
    "\n",
    "\n",
    "To find the epipoles, since every line should go through that, so in the following equation:\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L&space;^T\\mathbf{F}&space;\\mathbf{y}_R\" title=\"https://latex.codecogs.com/svg.image?0=\\mathbf{y}_L ^T\\mathbf{F} \\mathbf{y}_R\" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.image?\\mathbf{F}&space;\\mathbf{y}_R=0\" title=\"https://latex.codecogs.com/svg.image?\\mathbf{F} \\mathbf{y}_R=0\" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "[code](../src/fundamental_matrix_estimation.cpp)    \n",
    "\n",
    "\n",
    "\n",
    "Refs: [1](https://www8.cs.umu.se/kurser/TDBD19/VT05/reconstruct-4.pdf), [2](http://www.robots.ox.ac.uk/~vgg/hzbook/code/)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "54d5f431-15d0-4cdf-8848-81be1e31ddc8",
   "metadata": {},
   "source": [
    "## 3. Finding Essential Matrix and Fundamental Matrix\n",
    "\n",
    "\n",
    "In OpenCV depends on the intrinsic camera parameters, use `findEssentialMat` or `findFundamentalMat` \n",
    "\n",
    "| **Property**             | **Essential Matrix**                        | **Fundamental Matrix**                |\n",
    "|--------------------------|---------------------------------------------|---------------------------------------|\n",
    "| **Input Requirements**    | Intrinsic camera parameters (`K`) required | No camera parameters required         |\n",
    "| **Use Case**              | Calibrated cameras                         | Uncalibrated cameras                  |\n",
    "| **Coordinates**           | Normalized camera coordinates              | Pixel coordinates                     |\n",
    "| **Typical Applications**  | Recover pose, SfM, Visual Odometry         | Epipolar geometry, Stereo correspondence |\n",
    "\n",
    "### 3.1 findFundamentalMat\n",
    "```cpp\n",
    "Mat cv::findFundamentalMat\t(\tInputArray \tpoints1,\n",
    "InputArray \tpoints2,\n",
    "int \tmethod,\n",
    "double \transacReprojThreshold,\n",
    "double \tconfidence,\n",
    "int \tmaxIters,\n",
    "OutputArray \tmask = noArray() \n",
    ")\t\n",
    "```\n",
    "method:\n",
    "- `FM_7POINT` for a 7-point algorithm. N=7\n",
    "- `FM_8POINT` for an 8-point algorithm. N≥8\n",
    "- `FM_RANSAC` for the RANSAC algorithm. N≥8\n",
    "- `FM_LMEDS` for the LMedS algorithm. N≥8\n",
    "\n",
    "\n",
    "```cpp\n",
    "Mat F = findFundamentalMat(points1, points2, FM_RANSAC, 3.0, 0.99);\n",
    "```\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6dea72e2-6f35-443f-b570-ac2cb760eebe",
   "metadata": {},
   "source": [
    "#### 8-point Algorithm\n",
    "\n",
    "8 point correspondences are needed\n",
    "\n",
    "The 8-point algorithm assumes that the entries of E are all independent\n",
    "(which is not true since, for the calibrated case, they depend on 5 parameters (R and T))\n",
    "\n",
    "The solution of the 8-point algorithm is degenerate when the 3D points are coplanar.\n",
    " Conversely, the 5-point algorithm works also for coplanar points\n",
    "\n",
    "#### Over-determined solution\n",
    "n > 8 points \n",
    "\n",
    "\n",
    "\n",
    "                                                              \n",
    "#### 5-point Algorithm\n",
    "\n",
    "The 5-point algorithm uses the epipolar constraint considering the dependencies among all entries.\n",
    "\n",
    "\n",
    "Refs: [1](https://en.wikipedia.org/wiki/Essential_matrix)\n",
    "\n",
    "\n",
    "#### Normalized 8-point Algorithm\n",
    "Problem with 8-point algorithm: Poor numerical conditioning, which makes results very sensitive to noise and can be fixed by rescaling the data.\n",
    "Idea: Transform image coordinates so that they are in the range: \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "The Normalized 8-point algorithm can be summarized in three steps:\n",
    "\n",
    "1. Normalize the point correspondences:\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5C%5C%20%5Chat%7Bp_1%7D%3DB_1p_1%20%5C%5C%20%5Chat%7Bp_2%7D%3DB_2p_2\" alt=\"https://latex.codecogs.com/svg.latex?\\\\\n",
    "\\hat{p_1}=B_1p_1 \n",
    "\\\\\n",
    "\\hat{p_2}=B_2p_2\"/>\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7Bp_i%7D%3D%5Cfrac%7B%5Csqrt%7B2%7D%7D%7B%5Csigma%7D%20%28p_i-%5Cmu%29\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{p_i}=\\frac{\\sqrt{2}}{\\sigma} (p_i-\\mu) \" />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5Cmu%3D%5Cbegin%7Bpmatrix%7D%20%5Cmu_x%5C%5C%20%5Cmu_y%20%5Cend%7Bpmatrix%7D%3D%5Cfrac%7B1%7D%7BN%7D%5Csum_%7Bi%3D1%7D%5E%7BN%7D%20p_i\" alt=\"https://latex.codecogs.com/svg.latex?\"  />\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5Csigma%3D%5Cfrac%7B1%7D%7BN%7D%5Csum_%7Bi%3D1%7D%5E%7BN%7D%20%7C%7Cp_i-%5Cmu%7C%7C%5E2\" alt=\"https://latex.codecogs.com/svg.latex?\\sigma=\\frac{1}{N}\\sum_{i=1}^{N} ||p_i-\\mu||^2\"  />\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7Bp_i%7D%3D%5Cbegin%7Bbmatrix%7D%20%5Cfrac%7B%5Csqrt%7B2%7D%7D%7B%5Csigma%7D%20%26%200%20%26%20-%5Cfrac%7B%5Csqrt%7B2%7D%7D%7B%5Csigma%7D%20%5Cmu_x%5C%5C%200%20%26%20%5Cfrac%7B%5Csqrt%7B2%7D%7D%7B%5Csigma%7D%20%26%20-%5Cfrac%7B%5Csqrt%7B2%7D%7D%7B%5Csigma%7D%5Cmu_y%20%5C%5C%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7Dp_i\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{p_i}=\\begin{bmatrix}\n",
    "\\frac{\\sqrt{2}}{\\sigma}  & 0 & -\\frac{\\sqrt{2}}{\\sigma} \\mu_x\\\\ \n",
    "0 & \\frac{\\sqrt{2}}{\\sigma}  & -\\frac{\\sqrt{2}}{\\sigma}\\mu_y \\\\ \n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}p_i \n",
    "\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "2. Estimate normalized <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7BF%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{F}\" /> with 8-point algorithm using normalized coordinates\n",
    "\n",
    "3. Compute unnormalized <img src=\"https://latex.codecogs.com/svg.latex?F\" alt=\"https://latex.codecogs.com/svg.latex?F\" /> from <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7BF%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{F}\" />:\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?F%3DB_2%5ET%5Chat%7BF%7DB_1\" alt=\"https://latex.codecogs.com/svg.latex?F=B_2^T\\hat{F}B_1\" />\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12adadca-1b3f-4daa-bcd3-550157c005eb",
   "metadata": {},
   "source": [
    "### 3.2 findEssentialMat\n",
    "```cpp\n",
    "Mat cv::findEssentialMat\t(\tInputArray \tpoints1,\n",
    "InputArray \tpoints2,\n",
    "InputArray \tcameraMatrix,\n",
    "int \tmethod,\n",
    "double \tprob,\n",
    "double \tthreshold,\n",
    "int \tmaxIters,\n",
    "OutputArray \tmask = noArray() \n",
    ")\t\n",
    "```\n",
    "\n",
    "method\n",
    "- `RANSAC` for the RANSAC algorithm.\n",
    "- `LMEDS` (least-median of squares algorithm) for the LMedS algorithm.\n",
    "\n",
    "```\n",
    "Mat E = findEssentialMat(points1, points2, K, RANSAC, 0.999, 1.0);\n",
    "```\n",
    "\n",
    "c++ code:  [1](../src/essential_matrix_estimation.cpp)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8d1a8d60-4c55-4bc6-b045-e30fed81bc1e",
   "metadata": {},
   "source": [
    "#### Decompose Essential Matrix\n",
    "4 possible solutions of R and T\"\n",
    "\n",
    "<img src=\"images/essential_four_solutions.jpg\" alt=\"\" />\n",
    " \n",
    "point X will be in front of both cameras in one of these four solutions only. Thus, testing\n",
    "with a single point to determine if it is in front of both cameras is sufficient to decide\n",
    "between the four different solutions for the camera matrix.\n",
    "\n",
    "\n",
    "\n",
    "<br/>\n",
    "<br/>\n",
    "\n",
    "The essential matrix is the specialization of the fundamental matrix to the case of normalized image coordinates\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%5BR_1%2Ct%5D%2C%20%5BR_1%2C-t%5D%2C%20%5BR_2%2Ct%5D%2C%20%5BR_2%2C-t%5D.\" alt=\"https://latex.codecogs.com/svg.latex? [R_1,t], [R_1,-t], [R_2,t], [R_2,-t].\" />\n",
    "\n",
    "\n",
    "There exists only one solution where points are in front of both cameras.\n",
    "(For detail please read chapter 9.6.2 Extraction of cameras from the essential matrix, Multiple View Geometry in Computer Vision (Second Edition))\n",
    "\n",
    "Refs: [1](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga54a2f5b3f8aeaf6c76d4a31dece85d5d)\n",
    "\n",
    "<!---\n",
    "@incollection{grandstrand:2004,\n",
    "  author      = \"Ove Grandstrand\",\n",
    "  title       = \"Innovation and Intellectual Property Rights\",\n",
    "  editor      = \"Jan Fagerberg and David C. Mowery and Richard R. Nelson\",\n",
    "  booktitle   = \"The Oxford Handbook of Innovation\",\n",
    "  publisher   = \"Oxford University Press\",\n",
    "  address     = \"Oxford\",\n",
    "  year        = 2004,\n",
    "  pages       = \"266-290\",\n",
    "  chapter     = 10,\n",
    "}\n",
    "-->\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5372ce6e-263c-46d9-af8c-0bec4bf95a01",
   "metadata": {
    "editable": true,
    "slideshow": {
     "slide_type": ""
    },
    "tags": []
   },
   "source": [
    "#### Extract Translation and Rotation from Fundamental Matrix\n",
    "\n",
    "if the coordinates of the principal points of each camera are known\n",
    "and the two cameras have the same focal length 𝑓 in pixels, then 𝑅, 𝑇, 𝑓 can\n",
    "determined uniquely\n",
    "\n",
    "\n",
    "\n",
    "```cpp\n",
    "int cv::recoverPose\t(\tInputArray \tE,\n",
    "InputArray \tpoints1,\n",
    "InputArray \tpoints2,\n",
    "InputArray \tcameraMatrix,\n",
    "OutputArray \tR,\n",
    "OutputArray \tt,\n",
    "InputOutputArray \tmask = noArray() \n",
    ")\n",
    "```\n",
    "Refs: [1](https://www.cse.unr.edu/~bebis/CS485/Handouts/hartley.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "570c2135-3929-4a12-893b-1a2cf46c59d5",
   "metadata": {},
   "source": [
    "# Recovery of R,T from Essential Matrix\n",
    "\n",
    "```cpp\n",
    "recoverPose() [1/4]\n",
    "int cv::recoverPose\t(\tInputArray \tpoints1,\n",
    "InputArray \tpoints2,\n",
    "InputArray \tcameraMatrix1,\n",
    "InputArray \tdistCoeffs1,\n",
    "InputArray \tcameraMatrix2,\n",
    "InputArray \tdistCoeffs2,\n",
    "OutputArray \tE,\n",
    "OutputArray \tR,\n",
    "OutputArray \tt,\n",
    "int \tmethod = cv::RANSAC,\n",
    "double \tprob = 0.999,\n",
    "double \tthreshold = 1.0,\n",
    "InputOutputArray \tmask = noArray() \n",
    ")\t\n",
    "```\n",
    "The $ R $ from `cv::recoverPose()` is the rotation that transforms points **from the first camera’s coordinate system to the second camera’s coordinate system**. In other words, if $\\mathbf{X}_1$ is a 3D point in the **first** camera frame, then the corresponding point in the **second** camera frame is\n",
    "\n",
    "$\n",
    "\\mathbf{X}_2 \\;=\\; R_1^2 \\,\\mathbf{X}_1 \\;+\\; \\mathbf{t_1^2}.\n",
    "$\n",
    "\n",
    "So you can denote $R$ as $R_{1 \\to 2}$ (sometimes written as $R_1^2$, $R_{12}$, etc.).\n",
    "\n",
    "\n",
    "\n",
    "Refs: [1](https://www.cse.unr.edu/~bebis/CS485/Handouts/hartley.pdf), [2](https://www.ece.ualberta.ca/~lcheng5/papers/XuEtAl_TPAMI17.pdf), [3](https://inst.eecs.berkeley.edu/~ee290t/fa19/lectures/lecture10-3-decomposing-F-matrix-into-Rotation-and-Translation.pdf), [4](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga1b2f149ee4b033c4dfe539f87338e243)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ce624484-8200-46a3-ac05-94f97894b8aa",
   "metadata": {},
   "source": [
    "# Normalized Image Coordinates\n",
    "\n",
    "Consider a camera matrix decomposed as <img src=\"https://latex.codecogs.com/svg.latex?P%20%3D%20K%5BR%20%7C%20t%5D\" alt=\"https://latex.codecogs.com/svg.latex?P = K[R | t]\" />,\n",
    "and let <img src=\"https://latex.codecogs.com/svg.latex?%5Cmathbf%7Bx%7D%20%3D%20P%20%5Cmathbf%7BX%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\mathbf{x} = P \\mathbf{X}\" /> be a point in the image. If the calibration matrix <img src=\"https://latex.codecogs.com/svg.latex?K\" alt=\"https://latex.codecogs.com/svg.latex?K\" /> is known, then we\n",
    "may apply its inverse to the point <img src=\"https://latex.codecogs.com/svg.latex?%5Cmathbf%7Bx%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\mathbf{x}\" /> to obtain the point <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7B%5Cmathbf%7Bx%7D%7D%20%3DK%20%5E%7B-1%7D%20%5Cmathbf%7Bx%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{\\mathbf{x}} =K ^{-1} \\mathbf{x}\" />. Then <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7B%5Cmathbf%7Bx%7D%7D%20%3D%5BR%7Ct%5D%5Cmathbf%7BX%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{\\mathbf{x}} =[R|t]\\mathbf{X}\" />,\n",
    "where <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7B%5Cmathbf%7Bx%7D%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{\\mathbf{x}}\" /> is the image point expressed in normalized coordinates. It may be thought of\n",
    "as the image of the point <img src=\"https://latex.codecogs.com/svg.latex?%5Chat%7B%5Cmathbf%7BX%7D%7D\" alt=\"https://latex.codecogs.com/svg.latex?\\hat{\\mathbf{X}}\" /> with respect to a camera <img src=\"https://latex.codecogs.com/svg.latex?%5BR%7Ct%5D\" alt=\"https://latex.codecogs.com/svg.latex?[R|t]\" /> having the identity matrix I\n",
    "as calibration matrix. The camera matrix <img src=\"https://latex.codecogs.com/svg.latex?K%5E%7B-1%7DP%3D%5BR%7Ct%5D\" alt=\"https://latex.codecogs.com/svg.latex?K^{-1}P=[R|t]\" /> is called a **normalized camera\n",
    "matrix**, the effect of the known calibration matrix having been removed.\n",
    "Now, consider a pair of normalized camera matrices <img src=\"https://latex.codecogs.com/svg.latex?P%20%3D%20%5BI%20%7C%200%20%5D\" alt=\"https://latex.codecogs.com/svg.latex?P = [I | 0 ]\" /> and <img src=\"https://latex.codecogs.com/svg.latex?P%5E%7B%5Cprime%7D%20%3D%20%5BR%20%7C%20t%20%5D\" alt=\"https://latex.codecogs.com/svg.latex?P^{\\prime} = [R | t ]\" />. The\n",
    "fundamental matrix corresponding to the pair of normalized cameras is customarily\n",
    "called the essential matrix, and has the form:\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?E%3D%5Br%5D_%7B%5Ctimes%7DR%3DR%5BR%5ETt%5D_%7B%5Ctimes%7D.\" alt=\"https://latex.codecogs.com/svg.latex?E=[r]_{\\times}R=R[R^Tt]_{\\times}.\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"\" alt=\"\" />\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "normalize your image coordinate in pixel to the range [-1 ; 1]:\n",
    "\n",
    "\n",
    "To provide a uniform treatment of image coordinates between modules, independently of image size / resolution (different modules may use different image resolutions), it is convenient to adopt the following standard:\n",
    "\n",
    "Let the image have size (w, h), where w is the image width and h is the image height.\n",
    "Let the image coordinates be (u,v), in pixel units, where u is the horizontal coordinate, starting at the top-left corner and growing to the right, and v is the vertical image coordinate, starting at the top-left corner and growing to the bottom.\n",
    "The normalized image coordinates are defined as:\n",
    "x = 2*u/w - 1\n",
    "\n",
    "y = 2*v/h - 1\n",
    "\n",
    "Thus, the normalized image coordinates (both x and y) are in the range [-1,1], being 0 the center of the image, -1 the left/top boundaries, and 1 the right/bottom boundaries.\n",
    "\n",
    "The x coordinate starts at the image center and grows to the right. The y coordinate starts at the image center and grows to the bottom.\n",
    "\n",
    "To go back to image pixels, the following expressions should be used:\n",
    "\n",
    "u = w*(x+1)/2\n",
    "\n",
    "v = h*(y+1)/2\n",
    "\n",
    "\n",
    "# Normalized Camera Matrix \n",
    "\n",
    "you normalize in order to have the normalized image plane located at the focal length=1 by dividing the 3D coordinate expressed in the camera frame by the Z coordinate (\n",
    "\n",
    "\n",
    "The camera matrix derived above can be simplified even further if we assume that f = 1:\n",
    "\n",
    "\n",
    "\n",
    "<img src=\"https://latex.codecogs.com/svg.latex?%7B%5Cmathbf%20%7BC%7D%7D_%7B%7B0%7D%7D%3D%7B%5Cbegin%7Bpmatrix%7D1%260%260%260%5C%5C0%261%260%260%5C%5C0%260%261%260%5Cend%7Bpmatrix%7D%7D%3D%5Cleft%28%7B%5Cbegin%7Barray%7D%7Bc%7Cc%7D%7B%5Cmathbf%20%7BI%7D%7D%26%7B%5Cmathbf%20%7B0%7D%7D%5Cend%7Barray%7D%7D%5Cright%29\" alt=\"{\\mathbf  {C}}_{{0}}={\\begin{pmatrix}1&0&0&0\\\\0&1&0&0\\\\0&0&1&0\\end{pmatrix}}=\\left({\\begin{array}{c|c}{\\mathbf  {I}}&{\\mathbf  {0}}\\end{array}}\\right)\"/>\n",
    "\n",
    "Refs: [1](http://wiki.icub.org/wiki/Image_Coordinate_Standard), [2](https://en.wikipedia.org/wiki/Camera_matrix#Normalized_camera_matrix_and_normalized_image_coordinates), [3](http://users.ece.northwestern.edu/~yingwu/teaching/EECS432/Notes/camera.pdf), [4](https://opensfm.readthedocs.io/en/latest/geometry.html), [5](https://www.cc.gatech.edu/classes/AY2016/cs4476_fall/results/proj3/html/agartia3/index.html#:~:text=Normalized%20images%20are%20mean%20centred,as%20a%20measure%20of%20performance.), [6](https://www.csc.kth.se/~madry/courses/mvg10/Attachments/Oscar_norm_8pt_alg.pdf), [7](https://robotics.stackexchange.com/questions/12741/normalized-point-coordinates-in-undistortpoints-function)\n",
    "\n",
    "\n",
    "# General Camera Matrix\n",
    "\n",
    "\n",
    "\n"
   ]
  }
 ],
 "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.12.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}