An invariant filter for visual-inertial-raw GNSS fusion.
Paper: InGVIO: A Consistent Invariant Filter For Fast and High-Accuracy GNSS-Visual-Inertial Odometry
Paper Author: Changwu Liu, Chen Jiang and Haowen Wang
Paper Status: Manuscript submitted to RA-L for possible publication. Preprint version available on ArXiv.
Current Paper Link: https://arxiv.org/abs/2210.15145
InGVIO is an invariant filter approach for fusion of monocular/stereo camera, IMU and raw GNSS measurements including pseudo ranges and Doppler shifts. InGVIO is intrinsically consistent under conditional infinitesimal invariance of the GNSS-Visual-Inertial system. InGVIO has the following key features: (a) fast due to decoupled IMU propagation, key-frame marginalization strategy and no SLAM-features; (b) accurate due to intrinsic consistency maintenance; (c) better convergence properties than 'naive' EKF-based filters.
Moreover, we offer our fixed-wing datasets in the form of ROS Bags including stereo-visual, IMU and raw-GNSS measurements.
Fixed-Wing Dataset Link: https://cloud.tsinghua.edu.cn/d/38dd9d1a470944cda8fb password to extract: lcw18_thu_uav
Fixed-Wing Dataset Videos: The videos are uploaded to the link that stores the fixed-wing datasets.
The links to the datasets will be continuously updated. The config files for this dataset are contained in the InGVIO code configuration in path 'config/fw_zed2i_f9p'.
The compiler should at least support c++14 standards.
InGVIO is developed under ROS-Noetic with its default OpenCV4 library. However, InGVIO should be working on ROS-Melodic with OpenCV3. In the future, we may add support to ROS 2.
Eigen is a fantastic matrix computation library. InGVIO is developed under Eigen3.3.7. Other Eigen 3 versions should be OK for InGVIO.
We use SuiteSparse Library for sparse QR-decomposition in visual updates.
A wrapper for GNSS messages in ROS. See gnss_comm. The fantastic optimization-based work GVINS also relies on this library. We reserve a copy of gnss_comm in this repo.
Download or clone this repo to your ROS workspace.
cd ~/ws_catkin
catkin_make
Source the setup file to let ROS recognize the related launch files.
source devel/setup.bash
First download and unzip our fixed-wing dataset to your environment.
The 'fw_zed2i_f9p' folder contains the configuration files for fixed-wing datasets. To run InGVIO in your environment, the following should be conducted first.
Please adjust and modify the paths in 'config/fw_zed2i_f9p/ingvio_mono.yaml' and 'config/fw_zed2i_f9p/ingvio_stereo.yaml' to the case in your environment.
If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_fw_record.launch' files.
To run with monocular camera, please enter:
roslaunch ingvio_estimator ingvio_mono_fw.launch
For stereo-case, please enter:
roslaunch ingvio_estimator ingvio_stereo_fw.launch
To record the output topics, you can directly use our script file by:
roslaunch ingvio_estimator ingvio_mono_fw_record.launch
roslaunch ingvio_estimator ingvio_stereo_fw_record.launch
Rviz will be automatically launched. Run the ROS Bag to see the results:
rosbag play fw_easy.bag --pause
The GVINS datasets can be acquired from GitHub - HKUST-Aerial-Robotics/GVINS-Dataset. The config files for GVINS Datasets are already integrated in 'config/sportsfield'. It's valid for all 3 datasets provided by GVINS.
To run InGVIO in your environment, the following should be conducted first.
Please adjust and modify the paths in 'config/sports_field/ingvio_mono.yaml' and 'config/sports_field/ingvio_stereo.yaml' to the case in your environment.
If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_sf_record.launch' files.
To run with monocular camera, please enter:
roslaunch ingvio_estimator ingvio_mono_sf.launch
For stereo-case, please enter:
roslaunch ingvio_estimator ingvio_stereo_sf.launch
To record the output topics, you can directly use our script file by:
roslaunch ingvio_estimator ingvio_mono_sf_record.launch
roslaunch ingvio_estimator ingvio_stereo_sf_record.launch
Rviz will be automatically launched. Run the ROS Bag to see the results:
rosbag play sports_field.bag --pause
rosbag play complex_environment.bag --pause
rosbag play urban_driving.bag --pause
For sports_field.bag: Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = false, imu_buffer_size = 3000.
For urban_driving.bag: Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = true, imu_buffer_size = 3000. To avoid polluted GNSS measurements, a stronger version could be gnss_chi2_test = false, gnss_strong_reject = true.
For indoors_outdoors.bag: Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.18, gnss_chi2_test = false, gnss_strong_reject = true.
Please modify or try other parameters if the above behave not well. Good parameters may be different under different environment settings.
The realization of the type-based index system in filter framework is inspired by OpenVINS. The author himself has learned lots of programming skills in SLAM by reading the codes of OpenVINS.
The gnss_comm provided by GVINS is a great wrapper in developing codes involving raw GNSS measurements in ROS.
This software is open-sourced under GPLv3 license. See GNU GPL v3.0 - GNU.
Please cite our paper if you use either our code or our fixed-wing datasets.