## Table of Contents

Stereo Vision is traditionally done with the help of two cameras which take images simultaneously. Me and Shivang Agarwal thought of working on Single View Stereo or Monocular Stereo Vision for Swarm Robotics as the starting point of something called as "Distributed Vision". More about distributed vision in a future post.

## The Problem

Traditionally stereo vision (generating disparity maps) is achieved by using two synchronized cameras taking the pictures at the same time and in which we know how far the cameras are (the translational matrix $ T $) and how are the cameras orientated with respect to each other (The rotational matrix $ R $) by doing the camera calibration once at the start. Now the problem is that we don't want to use two cameras but we want to achieve the exact same thing using only one camera and the $ R $ and $ T $ matrix change with every set of images and plus for camera calibration we traditionally use chess board in which we know the real world distances, while right now we don't know that. What we intend to do is to take a frame, then move the camera and take another frame. So now we have two images and technically we can apply stereo vision. The only problem now is that we don't have the $ R $ and $ T $ matrix in this case. So we can't compute the fundamental matrix $ F $. We now need to figure out how to find $ F $, $ R $ and $ T $.

## Mathematical Theory

Let's the vector **x** represent any point in the left image and the vector
**x'** represent any point in the right image. The matrix relation between
these is:

\begin{equation} x^T F x' = 0 \end{equation}

Also let the matrix **F** be:

\begin{bmatrix} a & b & c \\ d & e & f \\ g & h & i \end{bmatrix}

So as of now, the problem boils down to finding a, b, c, d, e, f, g, h, i.

### No of distinct points required

As the equation *(1)* suggests, if we have the coordinate of many points
in left image and their corresponding points in the right image, we can
easily figure out the value of the matrix **F**. Let's just start off by
taking a single point in the left image $ (x_1, y_1) $
and the corresponding point in the right image $ (x_1', y_1') $

On plugging these values in equation *(1)*, we get

\begin{equation} \begin{bmatrix} x_1 & y_1 & 1 \end{bmatrix} \begin{bmatrix} a & b & c \\ d & e & f \\ g & h & i \end{bmatrix} \begin{bmatrix}{c} x_1' \\ y_1' \\ 1 \end{bmatrix} = 0 \end{equation}

After simplifying, we get

\begin{equation} (x_1 x_1')a + (y_1 y_1')d + (x_1')g + (x_1 y_1')b + (y_1 y_1')e + (y_1')h + (x_1)c + (y_1)f + i = 0 \end{equation}

where we have 9 unknowns but only one equation. Interestingly, if we try for 9 points, we can have 9 such equations which can be written as

\begin{equation} \begin{bmatrix} x_1 x_1' & y_1 y_1' & x_1' & x_1 y_1' & y_1 y_1' & y_1' & x_1 & y_1 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_9 x_9' & y_9 y_9' & x_9' & x_9 y_9' & y_9 y_9' & y_9' & x_9 & y_9 & 1 \\ \end{bmatrix} \begin{bmatrix} a \\ b \\ c \\ d \\ e \\ f \\ g \\ h \\ i \end{bmatrix} = 0 \end{equation}

which is of the form $ AX = 0 $. For this system of equations, we should avoid the trivial solution of every variable being zero thus we will only require 8 points.

Note: We will use SVD to solve this system of equations. While the minimum number required is 8, a higher number of points will have high accuracy.

### Getting the value of Fundamental matrix F

Thus we can effectively get the solution of this by using just 8 points. On solving, we can easily obtain the values of a, b, c, d, e, f, g, h and i. Please note, it is very essential to make sure that the fundamental matrix is of rank 2 ie. $ rank(F) = 2 $.

### Getting the Essential Matrix E

Fundamental matrix **F** represents the 2D relationship between the point on
the left image and the right image while Essential Matrix **E** tries to
actually map the relationships in 3D space. In traditional stereo vision,
fundamental matrix is obtained from essential matrix using,

\begin{equation} F = K^{-T} E K^{-1} \end{equation}

where **K** is the camera calibration matrix. Using the equation *(5)* we
can easily reverse engineer to get the value of **E**.

\begin{equation} E = K^T F K \end{equation}

It is essential to note here that Essential Matrix also rank 1 ie. $ rank(E) = 2 $.

### Calculate Projection

Let $ P_1 $ and $ P_2 $ be the projection vectors from both the images.
Now assuming $ P_1 $ to be our reference point, we can get the $ P_2 $ in
terms of the Rotational Matrix **R** and **T**.

\begin{equation} P_1 = K \begin{bmatrix} I_{3x3} & 0 \end{bmatrix} \end{equation}

\begin{equation} P_2 = K \begin{bmatrix} R & T \end{bmatrix} \end{equation}

### Relation between E, R and T

Now that we have got E, we can decompose that into **R** and **T**. The relation
between these is:

\begin{equation} E = [ T ]_{\times} R \end{equation}

Note: Here we take a cross product between the two matrix.

### Reconstructing T

This can be visualized with the help of Epipolar Geometry. The ray
between the two camera locations plus the rays pointing to the point from
both the camera locations form a plane. As you sweep a plane across these,
all the epilines will converge into a single point a.k.a **epipole** which
will be somewhere in between the line joining the two camera locations.
With both the epipoles we can now know the vector **T**. Because the
epipole is the projection of the camera location on the image plane of
the other camera image on the line joining the two locations, we can say
that,

\begin{equation} P_2 \begin{bmatrix} 0 \\ 1 \end{bmatrix} = \begin{bmatrix} R & T \end{bmatrix} \begin{bmatrix} 0 \\ 1 \end{bmatrix} \end{equation}

Now we can also say using the priciples in traditional stereo vision,

\begin{equation} T^T E = 0 \end{equation}

which basically means that left nullspace of the essential matrix is the
epipole in right image. Thus we can decompose **E** by using SVD as,

\begin{equation} E = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation}

where

\begin{equation} U = \begin{bmatrix} u_1 & u_2 & u_3 \end{bmatrix} \end{equation}

and thus we can say that $ T = -u_3 $ or $ T = u_3 $ which are two values.

### Reconstructing R

Once the value of **T** is successfully extracted from **E** matrix, we can
show using some linear algebra that

\begin{equation} R = U \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} V^T \end{equation}

or

\begin{equation} R = U \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} V^T \end{equation}

where **U** and **V** are matrix found from SVD of **E** of matrix.

### Removing Extraneous Values

Since we are getting two values of **R** and **T**, we have total of 4
permutations of these while only one is valid. Now, it is actually
interesting to note what all of these values represent. The correct value
represents the point actually in front of both the images. The other
values represent the point at back in either of the images or both of
them. So to get this we can get the 3D location of the point (this is
called as **Point Triangulation** that we are seeing. If the z-coordinate is
positive then we have found our correct solution because all other values
should be negative since we have assumed left image to be our reference
point.

### Point Triangulation

We can get the point triangulation by,

\begin{equation} \lambda \begin{bmatrix} X \ 1 \end{bmatrix} = P_1 \begin{bmatrix} X \ 1 \end{bmatrix} \end{equation}

This implies that the cross product of two vectors must be zero. Thus,

\begin{equation} \begin{bmatrix} X_1 \\ 1 \end{bmatrix} P_1 \begin{bmatrix} X \\ 1 \end{bmatrix} = 0 \end{equation}

And,

\begin{equation} \begin{bmatrix} X_2 \\ 1 \end{bmatrix} = P_2 \begin{bmatrix} X \\ 1 \end{bmatrix} = 0 \end{equation}

Thus we will get the x, y, and z coordinate of all the points. Now we can discard the solutions by checking their z coordinate as described in the above section.

Note: To get more accurate values, we can take multiple points and use least squares method to get better results.

## Actual Implementation

### Getting corresponding feature points

As we said above that we can find the fundamental matrix if we get the corresponding points from both the images, we first need to find all of those points. Technically we need only 8 points to uniquely determine the fundamental matrix but since we are dealing with real world coordinates from camera (which aren't perfect) so we will take as many points as possible. We will use linear least square algorithms which can be solved using SVD, with RANSAC to get more accurate results which will work better with more number of points.

To get those points, we can use either **SIFT (Scale-invariant Feature
Transform** or **ORB (Oriented FAST and rotated BRIEF)** or **DAISY** algorithm to
get the feature points. In this SIFT algorithm is generally more accurate
but it takes more computational resources and since we are dealing with
a real time problem, it will be better to use **ORB** which is fast and
reliable and comes with opencv unlike Daisy.

### Why not cv::stereoCalibrate()

This function could have been used which does all of our work to
calculate the **R**, **T**, **F**, **K** and **E** matrix for us but unfortunately we
can't use this as we need the corresponding points in the actual 3D world
that we are going to pass which we don't know. In traditional stereo
vision using two cameras, this is not a problem since we use a chessboard
in which we know the actual world distances between the two points and we
just do it once so as to get all the values. For our problem, we need to
calibrate the cameras again and again for every frames that we take in
and we won't even know the actual 3D coordinate for the same.

```
cv::Mat cv::findFundamentalMat()
```

which takes in two input arrays each of which correspond to their
respective points in the left and the right image and gives us
Fundamental Matrix **F**. This method uses RANSAC to compute the fundamental
matrix and gives it to us after cleaning it up so this is finally of
rank 2 as we require.

### Decomposing $ F $ into $ E $, $ T $, $ P_1 $ and $ P_2 $

This is the most challenging part of this problem. We now have **F** but we
don't have **E**, **R**, **T**, $ P_1 $ and $ P_2 $. Unfortunately there is
no inbuilt method in opencv to actually implement this for us so we will
have to type this out all by ourselves. Since we know the mathematics
behind it, we can easily use the OpenCV library for linear algebra and
compute this. Again in this since we have many points and not just one
we will employ SVD algorithm to help us in solving the various matrix
equations which in turn gives us an approximate solution. While doing
this we will have to be careful about which format we choosing our data
to be because that format has to be compatible with the rest of the
OpenCV functions as well because we intend to use the other methods in
OpenCV to simply our job afterwards.

**Note:** This gives us 4 values of **R** and **T**. We need to remove these by
triangulation of points.

### Removing extra points by triangulation

As we mentioned above, we are getting 4 values of **R** and **T** while we only
require one. Now we choose a point and try to triangulate it with respect
to both the image planes so that we can know whether they lie in front of
them or at the back. To do this there exists a function named

```
void cv::triangulatePoints()
```

which reconstructs the point by triangulation. We have mentioned the
maths behind it above. It takes Projection Matrix $ P_1 $ and $ P_2 $
and arrays of points corresponding to each image in two separate arrays
and outputs the 3D coordinates in a 4D array in which the last one is the
homogeneous coordinate. Now we can easily see which points are in the
front and thus we can take that particular value of **R** and **T** while we
discard the rest.

### Image Rectification parameters

Now we have all the values that we require but still the images have a definite epipole. This means that the images aren't exactly "straight" and thus we will need to straighten this out so that we get epipoles at infinity. The benefit of doing this is that now we will need to search for the corresponding point in the x-coordinate only instead of seaching in the whole area. Luckily, we have an opencv function which can do this for us, namely

```
void cv::stereoRectify()
```

which gives us the new $ R_1 $, $ R_2 $, $ P_1 $ and $ P_2 $ for
each of the image. The better part about this function is that this can
even get the value of **Q** which gives us the relation between the disparity
map and the depth map which we will need further to show the image in 3D.

### Rectified Image

Now we have got the rectification parameters. After this we will have to make the corresponding changes in the image according to those parameters. Luckily, we have an opencv function namely

```
void cv::undistortPoints()
```

which maps the initial point coordinates to their final destination coordinates.

### Generating disparity map with Blob Matching

Now we can finally the use opencv function,

```
cv::StereoBM::StereoBM()
```

```
void cv::StereoBM::operator()
```

which operates only on rectified images to generate the disparity map for us.

### Getting the depth map

Since we now have the disparity map and the **Q** matrix we can easily
compute the depth map. Luckily there exists an opencv function for the
same namely,

```
void cv::reprojectImageTo3D()
```

You can find the source code for all of this on github.