Stereo-camera Relocalisation in LiDAR Environments

GitHub Repo : https://github.com/kuromadoshiMJ/Stereo-LIME

Introduction
Localization of mobile robots in mundane spaces has introduced the luxury of saving pre-mapped environments, which ultimately improves localization accuracy reduces computational complexity of the task. The most common approach to localization uses LiDARs, which promise high accuracy, with the tradeoff of high monetary and computational costs. To solve this, techniques have been explored in recent years of relocalizing mobile robots in LiDAR maps (to preserve accuracy to an extent), using a cost-efficient sensor. This work explores relocalization of a stereo-camera in urban LiDAR environments.

The Pipeline

In this work, we use a 4-stage process inspired by the paper "Stereo Camera Localization in 3D LiDAR Maps" by KAIST.

Stage 1 : Local Map Extraction

This stage uses a coarse estimate of the agent's pose and the global LiDAR map, and usea a kd-tree to extract the local map.

For this implementation, the coarse estimate of a GPS pose has been assumed, however, the same pipeline can be extended to GPS-denied environments, using frame-to-frame incremental pose update with only GPS initialisation.

The local map is generated via pcl::octree::OctreePointCloudSearch, which searches for neighbours within a voxel at a given point which in our case is the initial pose obtained from VINS-Fusion.

Stage 2 : Depth Map Generation

The left and the right image from the stereo camera is used to compute the disparity map. From disparity map, we obtain the depth of a point using the formula:

disparity = x - x' = (B*f)/Z

Here, B is baseline, i.e, distance between the left and right camera & f is the focal length of the camera. Z is the depth of that pixel value.

Example, depth image:

Stage 3 : Tracking

To provide an initial estimate for our non-linear optimizer (written in Ceres Solver, we perform traditional feature based tracking on one of the stereo pair cameras, using ORB-SLAM in this case. Initial experimentation with VINS-Fusion was also done.

Stage 4 : Localization

The final camera pose is obtained by minimizing the depth residual which is the difference between the depth of the map point in local map and the corresponding stereo depth. This non-linear optimization problem is solved by Ceres-Solver.

Results

The green line shows the groud_truth path of the ego vehicle and the red line shows path generated from the localization pipeline.

Libraries Used

OpenCV

OpenCV (Open Source Computer Vision Library) is an open source computer vision and machine learning software library. Assuming independent streams from the cameras, we use OpenCV to perform blob-matching and get the stereo-camera depth output.

Installation guide to OpenCV : [link]

PCL

The Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing. We use the Point Cloud Library (PCL) for point cloud manipulation methods including kd-tree implementations and plane projections.

Installation guide to PCL : [link]

Ceres Solver

Ceres Solver is an open source C++ library for modeling and solving large, complicated optimization problems. In this project, we use it to perform photometric residual minimization.

Installation guide to Ceres Solver : [link]

This template is a modification to Jon Barron's website. Find the source code to my website here.