Skip to content

完全可理解的VINS-Fusion:1.代码风格重构、2.全量靠谱注释、3.代码即文档、4.忠实于原代码;5.ROS解耦、6.状态量可视化、7.日志系统。

License

Notifications You must be signed in to change notification settings

KennyWGH/VINS-Fusion-Understood

 
 

Repository files navigation

VINS-Fusion-Understood

Welcome to VINS-Fusion-Understood: A fully understandable version of VINS-Fusion, codes refactored in Google style, reliable comments on almost every line, necessary modification on confusing member names, ROS code decoupled, glog embedded.

欢迎来到 VINS-Fusion-Understood,一个人人可懂的VINS-Fusion版本。

在这里,我们对原版VINS-Fusion的代码风格进行了重构,不仅包括【1】一些必要的Google风格变量重命名/缩进/大括号位置调整,更重要的是,【2】我们按照含义聚类和逻辑顺序重新排列了几乎所有的成员函数和数据成员,【3】并添加了详尽的、经得起推敲的注释,尤其是三个主要类、以及滑窗优化、边缘化等部分。由此,我们希望VINS-Fusion-Understood能够做到代码即文档——让读代码就像读文档一样轻松。此外,【4】我们还在VINS算法中剥离了所有的ROS代码,【5】并引入了glog作为日志工具,以方便调试和保存日志文件。这些改动都有助于您将VINS方便地移植到其它通信框架或计算平台上。【6】为了便于理解VINS,我们还引入了ui窗口以绘制VINS系统内部的状态信息,使VINS可视化。以上所有重构工作,您都可以从本仓库的代码中看到。您也可以从下方的动图中对VINS-Fusion-Understood的代码风格和注释内容窥见一二。

更详细的介绍,您可以前往中文博客查看:【知乎】VINS-Fusion-Understood:完全可理解的VINS-Fusion,靠谱注释+工程重构版

尽管做了以上重构,但VINS-Fusion-Understood的代码内容严格忠实于原版,在精度和运行性能上也绝无二致,下图是原版和本版本的运行结果对比。如果您对VINS-Fusion的源码尚不了解,这将是您上手VINS最合适的版本或参考。

注1:为了和VINS-Fusion原版不至于差异太大,大部分函数并没有做重命名,除非原版函数的命名让人费解 —— 比如我们把原版 Estimator::relativePose() 重命名为了 VinsEstimator::searchRelativePose(),把原版Estimator::vector2double()重命名为了VinsEstimator::StateToCeresParam(),把原版FeatureManager::getCount()重命名为了FeatureManager::getRobustFeatureCount()
注2:目前,我们仅对VINS-Fusion中精华的vins_estimator模块完成了重构,其它部分留作将来TODO(若有需要)。


图1:运行结果对比,左为官方版,右为本仓库版


图2:VINS顶层类(class VinsEstimator)重构版预览


图3:特征点光流追踪类(class FeatureTracker)重构版预览


图4:特征点管理类(class FeatureManager)重构版预览


图5:Vins-Fusion-Understood运行界面







VINS-Fusion

An optimization-based multi-sensor state estimator

VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension of VINS-Mono, which supports multiple visual-inertial sensor types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS with GPS. Features:

  • multiple sensors support (stereo cameras / mono camera+IMU / stereo cameras+IMU)
  • online spatial calibration (transformation between camera and IMU)
  • online temporal calibration (time offset between camera and IMU)
  • visual loop closure

We are the top open-sourced stereo algorithm on KITTI Odometry Benchmark (12.Jan.2019).

Authors: Tong Qin, Shaozu Cao, Jie Pan, Peiliang Li, and Shaojie Shen from the Aerial Robotics Group, HKUST

Videos:

VINS

Related Paper: (paper is not exactly same with code)

  • Online Temporal Calibration for Monocular Visual-Inertial Systems, Tong Qin, Shaojie Shen, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS, 2018), best student paper award pdf

  • VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Shaojie Shen, IEEE Transactions on Robotics pdf

If you use VINS-Fusion for your academic research, please cite our related papers. bib

1. Prerequisites

1.1 Ubuntu and ROS

Ubuntu 64-bit 16.04 or 18.04. ROS Kinetic or Melodic. ROS Installation

1.2. Ceres Solver

Follow Ceres Installation.

2. Build VINS-Fusion

Clone the repository and catkin_make:

    cd ~/catkin_ws/src
    git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
    cd ../
    catkin_make
    source ~/catkin_ws/devel/setup.bash

(if you fail in this step, try to find another computer with clean system or reinstall Ubuntu and ROS)

3. EuRoC Example

Download EuRoC MAV Dataset to YOUR_DATASET_FOLDER. Take MH_01 for example, you can run VINS-Fusion with three sensor types (monocular camera + IMU, stereo cameras + IMU and stereo cameras). Open four terminals, run vins odometry, visual loop closure(optional), rviz and play the bag file respectively. Green path is VIO odometry; red path is odometry under visual loop closure.

3.1 Monocualr camera + IMU

    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
    rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag

3.2 Stereo cameras + IMU

    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
    rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag

3.3 Stereo cameras

    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
    rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag

4. KITTI Example

4.1 KITTI Odometry (Stereo)

Download KITTI Odometry dataset to YOUR_DATASET_FOLDER. Take sequences 00 for example, Open two terminals, run vins and rviz respectively. (We evaluated odometry on KITTI benchmark without loop closure funtion)

    roslaunch vins vins_rviz.launch
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml
    rosrun vins kitti_odom_test ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/ 

4.2 KITTI GPS Fusion (Stereo + GPS)

Download KITTI raw dataset to YOUR_DATASET_FOLDER. Take 2011_10_03_drive_0027_synced for example. Open three terminals, run vins, global fusion and rviz respectively. Green path is VIO odometry; blue path is odometry under GPS global fusion.

    roslaunch vins vins_rviz.launch
    rosrun vins kitti_gps_test ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/ 
    rosrun global_fusion global_fusion_node

5. VINS-Fusion on car demonstration

Download car bag to YOUR_DATASET_FOLDER. Open four terminals, run vins odometry, visual loop closure(optional), rviz and play the bag file respectively. Green path is VIO odometry; red path is odometry under visual loop closure.

    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml 
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/vi_car/vi_car.yaml 
    rosbag play YOUR_DATASET_FOLDER/car.bag

6. Run with your devices

VIO is not only a software algorithm, it heavily relies on hardware quality. For beginners, we recommend you to run VIO with professional equipment, which contains global shutter cameras and hardware synchronization.

6.1 Configuration file

Write a config file for your device. You can take config files of EuRoC and KITTI as the example.

6.2 Camera calibration

VINS-Fusion support several camera models (pinhole, mei, equidistant). You can use camera model to calibrate your cameras. We put some example data under /camera_models/calibrationdata to tell you how to calibrate.

cd ~/catkin_ws/src/VINS-Fusion/camera_models/camera_calib_example/
rosrun camera_models Calibrations -w 12 -h 8 -s 80 -i calibrationdata --camera-model pinhole

7. Docker Support

To further facilitate the building process, we add docker in our code. Docker environment is like a sandbox, thus makes our code environment-independent. To run with docker, first make sure ros and docker are installed on your machine. Then add your account to docker group by sudo usermod -aG docker $YOUR_USER_NAME. Relaunch the terminal or logout and re-login if you get Permission denied error, type:

cd ~/catkin_ws/src/VINS-Fusion/docker
make build

Note that the docker building process may take a while depends on your network and machine. After VINS-Fusion successfully built, you can run vins estimator with script run.sh. Script run.sh can take several flags and arguments. Flag -k means KITTI, -l represents loop fusion, and -g stands for global fusion. You can get the usage details by ./run.sh -h. Here are some examples with this script:

# Euroc Monocualr camera + IMU
./run.sh ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml

# Euroc Stereo cameras + IMU with loop fusion
./run.sh -l ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml

# KITTI Odometry (Stereo)
./run.sh -k ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/

# KITTI Odometry (Stereo) with loop fusion
./run.sh -kl ~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml YOUR_DATASET_FOLDER/sequences/00/

#  KITTI GPS Fusion (Stereo + GPS)
./run.sh -kg ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/

In Euroc cases, you need open another terminal and play your bag file. If you need modify the code, simply re-run ./run.sh with proper auguments after your changes.

8. Acknowledgements

We use ceres solver for non-linear optimization and DBoW2 for loop detection, a generic camera model and GeographicLib.

9. License

The source code is released under GPLv3 license.

We are still working on improving the code reliability. For any technical issues, please contact Tong Qin <qintonguavATgmail.com>.

For commercial inquiries, please contact Shaojie Shen <eeshaojieATust.hk>.

About

完全可理解的VINS-Fusion:1.代码风格重构、2.全量靠谱注释、3.代码即文档、4.忠实于原代码;5.ROS解耦、6.状态量可视化、7.日志系统。

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published

Languages

  • C++ 94.3%
  • CMake 4.7%
  • Other 1.0%