Visual Odometry

Visual Odometry was my main contribution to my bachelor’s degree. It represents the update step in the Kalman Filter and I based it on the lane-detection algorithm that was already implemented on the Raspberry. It’s goal is to give the yaw-rate with respect to a starting point.

The main disadvantage of this approach is that the camera is very sensitive to any environmental variables, the most significant one being the proper lightning.

I approached this case by using the RANSAC - Random Sample Consensus algorithm.

By taking the POV to Bird-Eye-View, I succeeded in constraining the lines to always be parallel to each other. Having this achieved, I only needed to find out how to determine the rotation from one frame to the next one.

To enhance the confidence of the rotation I used RANSAC because the camera measurements can be very noisy depending on the lightning, angle, etc. It succeeds in separating the data into inliers and outliers and it tries to fit a model based on the inliers.

The algorithm used is based on affine transformations which preserves the parallelism of the lanes by solving the next equational system:

$$\begin{array}{rlrl}T(x)& =Ax+b,& A& =\left[\begin{array}{cc}a& b\\ c& d\end{array}\right],\end{array}$$ $$\left[\begin{array}{c}{x}_i^{\prime }\\ y_i^{\prime }\end{array}\right]=\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\end{array}\right],$$ $$\left[\begin{array}{cccccc}{x}_1& y_1& 1& 0& 0& 0\\ 0& 0& 0& x_1& y_1& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_n& y_n& 1& 0& 0& 0\\ 0& 0& 0& x_n& y_n& 1\end{array}\right]\left[\begin{array}{c}a\\ b\\ t_x\\ c\\ d\\ t_y\end{array}\right]=\left[\begin{array}{c}{x}_1^{\prime }\\ y_1^{\prime }\\ ⋮\\ x_n^{\prime }\\ y_n^{\prime }\end{array}\right],$$ $$\begin{array}{rl}scale=\sqrt{a^2+b^2},& R=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]=\left[\begin{array}{cc}\frac{a}{\text{scale}}& \frac{b}{\text{scale}}\\ \frac{c}{\text{scale}}& \frac{d}{\text{scale}}\end{array}\right]\end{array}$$

The third equation is only the second one but extended. In my case I has a vector of pixel coordinates for X and Y.

How good were the results?

I would say it depends on more factors but I won’t go in depth. The only one I am willing to share is that I came up with a threshold in order to determine if the curve is to the left; to the right; or simply a straight line.

By using a threshold of 7, I managed to accentuate the angle of the curves, but the straight path suffered in this representation. On the other hand, a treshold of 10 accentuated the straight path while the curves had a little bit more to suffer.

Threshold of 10

Threshold of 7