Real-time Stereo Visual Odometry with Local Non-Linear Least Square Optimisation

About

This project implements 6-DOF camera pose estimation from a calibrated stereo camera (with local non-linear least square optimisation) for various scenarios in KITTI dataset

Installation

$ conda env create -f setup/environment.yml
$ pip install -e .

Usage

For simulation of visual odometry, run the followig command

$ python main.py --config_path configs/params.yaml

The params.yaml needs to be edited to configure the sequence to run the simulation.

About the Problem !

1. What is Visual Odometry ?
2. Problem Formulation
3. Algorith Implemented

Visual Odometry

Visual Odometry is the process of incrementally estimating the pose and trajectory of a robot or a vehicle (orientation and translation of a camera configuration rigidly attached to it) using video stream from the camera.

Problem Formulation

An agent is moving through an environment and taking images with a rigidly attached camera system at discrete time instants. Let the stream of images coming from the pair of camera (assumed stereo configuration) be denoted by I_{L, k} and I_{R, k} at time instant k . We assume that we have prior knowledge of all the intrinsic as well as extrinsic calibration parameters of the stereo rig.

We need to estimate the relative rotation R and translation t between stereo configuration at time instants k-1 and k and then to concatenate the transformation to incrementally recover the full trajectory C_k of the camera

Algorithm Implemented

3D-to-2D: Structure to feature correspondences (Source : [1])

•  Compute the first stereo image frames I_L,K and I_R,K
  
•  Extract and match stereo features f_L,K and f_R,K
  
•  Triangulate features to build point cloud X_k
  
•  Set initial camera pose C_k
  
•  Store information from the first frame as I_L,k-1, I_R,k-1, f_L,k-1, f_R,k-1, X_k-1
   While exists a new image frame:
  
  • Compute the new stereo image pair I_L,K and _R,K
    
  • Extract and match stereo features f_L,K and f_R,K
    
  • Triangulate features to build point cloud X_k
    
  • Track 2D features f_L,k-1 at I_L,k-1 to f_L,k at I_L,K and thus obtain ^tf_k-1,k
    
  • Compute correspondence for the tracked features ^tf_k-1,k
    and X_k-1
    
  • Compute camera pose estimation (P3P), thus T = [R|t]
  • Concatenate transformation by C_k = C_k-1 T_k
  • If Optimisation is enabled, do non-linear least squares optimisation of T
  • Store informaton from first frame as I_L,k-1, I_R,k-1, f_L,k-1, f_R,k-1, X_k-1 and C_k-1
    

Relative Camera Pose and Concatenation of Transformations (Source: E. F. Aguilar Calzadillas [1] )

Future Work

• Implement Windowed Bundle Adjustment
• Implement Graph Based Optimisation in Visual SLAM
• Visualise 3D point cloud of scene using Point Cloud Library (PCL)

References

[1] E. F. Aguilar Calzadillas, "Sparse Stereo Visual Odometry with Local Non-Linear Least-Squares Optimization for Navigation of Autonomous Vehicles", M. A. Sc. Thesis, Department of Mechanical and Aerospace Engineering, Carleton University, Ottawa ON, Canada, 2019

[2] D. Scaramuzza, F. Fraundorfer, "Visual Odometry: Part I - The First 30 Years and Fundamentals", IEEE Robotics and Automation Magazine, Volume 18, issue 4, 2011

[3] F. Fraundorfer, D. Scaramuzza, "Visual odometry: Part II - Matching, robustness, optimization, and applications", IEEE Robotics and Automation Magazine, Volume 19, issue 2, 2012

[4] Avi Singh, Visual Odmetry from scratch - A tutorial for beginners, Avi Singh's Blog

Contact

Saksham Jindal ([email protected])