Stereo Visual SLAM on KITTI

Overview

This video shows the stereo visual SLAM system tested on the KITTI dataset sequence 00.

Check out my portfolio post for a detailed description of the components and algorithms used in this implementation.

This project built a stereo visual SLAM system from scratch. It has feature-based visual odometry using ORB features, and a keyframe-based map management and optimization backend. The system is integrated with ROS.

The system has six major components:

• Initialization
• Feature Detection/Matching
• Motion Estimation
• Map Management
• Bundle Adjustment
• Visualization Module

File Structure

Folders

• cmake_module/: cmake modules.
• config/: config files.
• include/: c++ header files.
• src/: ROS node and C++ source code.
• launch/: launch file.

Structure

config/
├── kitti_config.rviz
└── kitti_param.yaml
include/
└── stereo_visual_slam_main
    ├── library_include.hpp
    ├── map.hpp
    ├── optimization.hpp
    ├── types_def.hpp
    ├── visual_odometry.hpp
    └── visualization.hpp
launch/
└── run_vslam.launch
src/
├── stereo_visual_slam_main
    ├── map.cpp
    ├── optimization.cpp
    ├── types_def.cpp
    ├── visual_odometry.cpp
    └── visualization.cpp
└── run_vslam.cpp

• config/kitti_param.yaml contains the path to the dataset
• library_include.hpp include libraries that are commonly used in the package
• map.hpp definition of map management module
• optimization.hpp implementation of non-linear optimization using G2O
• types_def.hpp definition of frame, landmark, feature struct
• visual_odometry.hpp functions for the stereo visual odometry
• visualization.hpp the visualization module
• run_vslam.cpp is the ROS node for running this system

Dependency

• ROS Melodic (Link)
• OpenCV (Version 3.2)
• Eigen (Official Site)
• Sophus: Lie Groups Library (Link)
• G20: Graph Optimization Framework (Link)

Quick Start guide

• Install all the dependencies
• fork this repository, then git clone or build using wstool
• Build the package catkin_make -DCMAKE_BUILD_TYPE=Release in release mode
• Modify the path to KITTI dataset on your computer in config/kitti_param.yaml
• To download KITTI dataset, go to this link.
• If Rviz visualization is needed, set if_rviz to true in config/kitti_param.yaml
• If writing estimated trajectory to file is needed, set if_write_pose to true in config/kitti_param.yaml
• To launch the system, do source devel/setup.bash and roslaunch stereo_visual_slam_main run_vslam.launch

Results

The table below shows the performance of my implementation on KITTI Dataset sequence 00. The runtime is around 0.18s for a keyframe and 0.04s for a non-keyframe.

The figure below shows the trajectory estimated by my implementation compared to the ground truth. The system has been tested on KITTI sequence 00 and 01 without lost.