Achieves accurate localization with the help of stereo-camera in a pre-mapped pointcloud environment

Follow Ceres Installation

Install OpenCV

Install PCL from source.

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:

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.

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.

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