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.