Stereo-Odometry-SOFT

This repository is a MATLAB implementation of the Stereo Odometry based on careful Feature selection and Tracking. The code is released under MIT License.

The code has been tested on MATLAB R2018a and depends on the following toolboxes:

• Parallel Processing Toolbox
• Computer Vision Toolbox

On a laptop with Intel(R) Core(TM) i7-8750H CPU @ 2.20GHz and 16GB RAM, the following average timings were observed:

• Time taken for feature processing (in ms): 261.4
• Time taken for feature matching (in ms): 3650.5
• Time taken for feature selection (in ms): 6.5
• Time taken for motion estimation (in ms): 1.1

How to run the repository?

1. Clone the repository using the following command:

git clone https://github.com/Mayankm96/Stereo-Odometry-SOFT.git

2. Import the dataset to the folder data. In case you wish to use the KITTI Dataset, such as the Residential dataset, the following command might be useful:

cd PATH/TO/Stereo-Odometry-SOFT
## For Reseidential Sequence: 61 (2011_09_46_drive_0061)
# synced+rectified data
wget -c https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0009/2011_09_26_drive_0009_sync.zip -P data
# calib.txt
wget -c https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip -P data

3. Change the corresponding paramters in the configuration file configFile.m according to your need

4. Run the script main.m to get a plot of the estimated odometry

Proposed Implementation of the Algorithm

The input to the algorithm at each time frame, are the left and right images at the current time instant, and the ones at the previous timestamp.

Keypoints Detection

In this section, the keypoints detection and matching is divided into following separate stages:

• feature processing: each image is searched for locations that are likely to match well in other images
• feature matching: efficiently searching for likely matching candidates in other images
• feature tracking: unlike to the second stage, the correspondences are searched in a small neighborhood around each detected feature and across frames at different time steps

Feature processing

Corner and blob features are extracted for each image using the following steps:

1. First, blob and checkerboard kernels are convolved over the input image.

2. Efficient Non-Maximum Suppression is applied on the filter responses to produce keypoints that may belong to either of the following classes: blob maxima, blob minima, corner maxima, and corner minima. To speed up the feature matching, correspondences are only found between features belong to the same class.

3. The feature descriptors are constructed by using a set of 16 locations out of an 11 x 11 block around each keypoint in input image's gradients. The gradient images are computed by convolving 5 x 5 sobel kernels across the input image. The descriptor has a total length of 32,

Feature matching

This part of the algorithm is concerned with finding the features for egomotion estimation. It is based on the process mentioned in the paper StereoScan: Dense 3d reconstruction in real-time. The process can be summarized as follows:

1. Correspondences in two images are found by computing the Sum of Absolute Differences (SAD) score between a feature in the first image with the one lying in the second image that belongs to the same class

2. This matching is done in a circular fasion between the left and right frames at time instants t-1 and t as shown below:

3. To remove certain outliers, Normalized Cross-Correlation (NCC) is again in a circular fasion using templates of size 21 x 21 pixels around the features that have been matched successfully in the process mentioned above.

Feature Selection

To ensure a uniform distribution of features across the image, the entire image is divided into buckets of size 50 x 50 pixels and feature selection is done to select only the strongest features present in each bucket.

Egomotion Estimation

Using P3P algorithm along with RANSAC, incremental rotation and translation is estimated.

To-Dos

• [ ] fix parfor and for loops to enable running without parallel processing
• [ ] read the camera calibration parameters from calibration file directly
• [ ] add sub-pixel refinement using parabolic fitting
• [ ] add feature selection based on feature tracking i.e. the age of features
• [ ] implement Nister's algorithm and SLERP for rotation estimation
• [ ] use Gauss-Newton optimization to estimate translation from weighted reprojection error