GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation. paper link
GVINS is a non-linear optimization based system that tightly fuses GNSS raw measurements with visual and inertial information for real-time and drift-free state estimation. By incorporating GNSS pseudorange and Doppler shift measurements, GVINS is capable to provide smooth and consistent 6-DoF global localization in complex environment. The system framework and VIO part are adapted from VINS-Mono. Our system contains the following features:
- global 6-DoF estimation in ECEF frame;
- multi-constellation support (GPS, GLONASS, Galileo, BeiDou);
- online local-ENU frame alignment;
- global pose recovery in GNSS-unfriendly or even GNSS-denied area.
This package requires some features of C++11.
This package is developed under ROS Kinetic environment.
Our code uses Eigen 3.3.3 for matrix manipulation.
We use ceres 1.12.0 to solve the non-linear optimization problem.
This package also requires gnss_comm for ROS message definitions and some utility functions. Follow those instructions to build the gnss_comm package.
Clone the repository to your catkin workspace (for example ~/catkin_ws/
):
cd ~/catkin_ws/src/
git clone https://github.com/HKUST-Aerial-Robotics/GVINS.git
Then build the package with:
cd ~/catkin_ws/
catkin_make
source ~/catkin_ws/devel/setup.bash
Download our GNSS-Visual-Inertial dataset as described in the next section. Then launch GVINS via:
roslaunch gvins visensor_f9p.launch
Subscribe /gvins/gnss_enu_path
in your rviz and play the bag:
rosbag play sports_field.bag
We published our GNSS-Visual-Inertial dataset at rosbag_1 and rosbag_2. The visual and inertial data are collected using a VI-Sensor, and the GNSS raw measurement is provided by a u-blox ZED-F9P receiver. The RTCM stream from a 3km-away base station is fed to the GNSS receiver for RTK solution. In addition, the time synchronization between VI-Sensor and ZED-F9P is achieved via hardware trigger.
The system framework and VIO part are adapted from VINS-Mono. We use camodocal to model the camera and ceres to solve the optimization problem.
The source code is released under GPLv3 license.