# Implement Monocular Visual Odometry

Dec 19, 2022 · 11 min · computer vision

Monocular Visual Odometry (VO) is the process of estimating the pose (position and orientation) of a camera using only visual information from a single camera. It is a crucial component of many robotics and augmented reality systems, as it allows a device to understand its own movement and position in the world.

This blog would be focussing on how to implement a simple monocular VO algorithm in Python. If you are new to it, I suggest having a look at this article.

The overall process can be broken down into the following steps:

- Feature detection
- Feature tracking
- Estimating fundamental matrix
- Estimating essential matrix (from fundamental matrix)
- Recovering pose (from essential matrix)

## Problem

Suppose that we have a camera attached to a vehicle. A video coming from this camera has been processed for lens distortion and converted to frames. We denote the frames captured at time $i$ and $i+1$ as $I_i$ and $I_{i+1}$, respectively. For every pair of images $(I_i, I_{i+1})$, our job is to find the rotation matrix $R_i$ and the translation vector $t_i$, which describe the motion of the vehicle between the two frames.

## Feature Detection

We first look for salient landmarks, called keypoints $K_i$, in $I_i$. Keypoints are features that differ from their immediate neighborhood such as corners or areas with unique colors or textures. These features should ideally be found in both two adjacent frames.

Using OpenCV, detecting keypoints is trivial:

Here GFTT (Good Features to Track) detector is used to detect keypoints. In my experiments, other detectors like SIFT (Scale Invariant Feature Transform) and ORB (Oriented Fast and Rotated Brief) perform worse than GFTT. Meanwhile, the speed of SIFT is much slower.

## Feature Tracking

We then need to track $K_i$‘s corresponding keypoints $K_{i+1}$ in $I_{i+1}$ using optical flow. To improve the performance, you can choose to remove the keypoints that are not tracked in $I_{i+1}$ (`status`

returned by `cv2.calcOpticalFlowPyrLK`

is 0) or are outside $I_{i+1}$.

## Fundamental Matrix

The fundamental matrix $F = \begin{bmatrix} f_{11} & f_{12} & f_{13} \\ f_{21} & f_{22} & f_{23} \\ f_{31} & f_{32} & f_{33} \end{bmatrix}$ is a $3 \times 3$ matrix that encodes the relative orientation and position of the camera between $I_i$ and $I_{i+1}$.

### 8-Point Algorithm

To estimate $F$, one of the solutions is Hartley’s normalized 8-point algorithm, where the constraint for $F$ is:

$\begin{bmatrix} x_{i+1}^1 x_i^1 & x_{i+1}^1 y_i^1 & x_{i+1}^1 & y_{i+1}^1 x_i^1 & y_{i+1}^1 y_i^1 & y_{i+1}^1 & x_i^1 & y_i^1 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_{i+1}^8 x_i^8 & x_{i+1}^8 y_i^8 & x_{i+1}^8 & y_{i+1}^8 x_i^8 & y_{i+1}^8 y_i^8 & y_{i+1}^8 & x_i^8 & y_i^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,$which can be answered by solving the linear least squares using Singular Value Decomposition (SVD). This algorithm requires 8 keypoint correspondences exists, where $(x_i, y_i)$ are the coordinates of the 8 keypoints selected from $K_i$, and $(x_{i+1}, y_{i+1})$ are the coordinates of their corresponding keypoints in $K_{i+1}$. The coordinates should be normalized by shifting them around the mean of the points and enclosing them at a distance of $\sqrt{2}$ from the new center.

Given 8 keypoint correspondences, the code for computing $F$ is:

### RANSAC

Our feature detection and tracking algorithms are not perfect, leading to several erroneous correspondences (outliers). Therefore, RANSAC is used to remove these outliers.

RANSAC is an iterative algorithm terminating after a fixed number of iterations. At every iteration, it works in the following way:

- Picks 8 keypoint correspondences from $K_i$ and $K_{i+1}$
- Computes $F$ using Hartley’s 8-point algorithm mentioned above
- Determines how many of all other keypoints are inliers

Finally, the $F$ with the maximum number of inliers will be used.

Then the problem is how to pick 8 keypoint correspondences (`sample_eight_points`

). While Hartley’s 8-point algorithm samples them randomly, to make the algorithm more robust to outliers, we can use the way mentioned here (page 39) instead: uniformly dividing a frame into an $8 \times 8$ grid, then randomly selecting 8 grid cells and picking one pair of corresponding keypoints from each grid cell.

## Essential Matrix

Essential matrix $E$ is another $3 \times 3$ matrix. But unlike $F$, $E$ assumes that the cameras obey the pinhole model. More specifically, given the camera calibration matrix $K$, $E = K^T F K$, which also can be solved using SVD.

Note that the singular values of $E$ are not necessarily $(1, 1, 0)$ due to the noise in K. Thus $E$ should be reconstructed with $(1, 1, 0)$ singular values:

$E = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} V^\top$Now we have successfully estimated the essential matrix $E$ manually, looks cool! However, this can also be done with the help of OpenCV, and all you need is one line:

OpenCV uses Nistér’s 5-point algorithm, a more recent approach that is shown to give better results, to compute $E$. It requires the minimum number of keypoints possible since $E$ has only 5 degrees of freedom ($F$ has 7 degrees of freedom, $E$ has only 5 as it takes camera parameters into account).

## Camera Pose

Just follow here to recover the camera rotation matrix $R_i$ and translation vector $t_i$ from $E$.

Again, here is how to implement it in OpenCV:

## Trajectory

Denote the pose of the camera as $R$ and $t$, which should be initialized as:

We can update the trajectory of the camera by:

$t = t + R t_i$ $R = R_i R$That’s it! We can now move to the next pair of frames $(I_{i+1}, I_{i+2})$ and repeat from here.

Of course, there are many details and optimizations (e.g. bundle adjustment) that can be added to improve the accuracy and performance of the VO system, but this should give a good starting point for building a monocular VO system.

## References

- Structure from Motion (University of Maryland)
- The 8-point algorithm (Carnegie Mellon University)
- Fundamental Matrix (University of Central Florida)
- An Efficient Solution to the Five-Point Relative Pose Problem.
*David Nistér*. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2004. - Monocular Visual Odometry using OpenCV