IMU + X Fusion

IMU + X Loosely-Coupled Fusion Localization

Overview

• IMU + GPS

  • ESKF: Multi-Sensor Fusion: IMU and GPS loose fusion based on ESKF

• IMU + 6DoF Odom (e.g.: Stereo Visual Odometry)

  • ESKF: IMU and 6 DoF Odometry (Stereo Visual Odometry) Loosely-Coupled Fusion Localization based on ESKF (Presentation)

  • IEKF: https://cgabc.xyz/posts/784a80cb/#iekf

  • UKF(including JUKF and SVD-UKF): IMU and 6 DoF Odometry (Stereo Visual Odometry) Loosely-Coupled Fusion Localization based on UKF

  • MAP(User-defined L-M, Ceres-Solver, G2O and GTSAM)

Features

• Dataset

  • KAIST dataset: ROS publish

• Sensors

  • IMU
    • Numerical Integration (TODO: RK4)
    • Pre-Integration
  • Wheel Odometer
  • Manometer
  • GPS
  • 6DoF Odom (Stereo Visual Odometry)
  • LiDAR
  • Multi-modal data

• State Estimation

  • EKF
    • ESKF
    • IEKF
  • UKF
    • JUKF
    • SVD-UKF
  • Particle Filter
  • MAP
    • User-defined G-N
    • Ceres-Solver
    • G2O
    • GTSAM

• Local / Global Angular Error (Rotation Perturbation)

  • Local Angular Error (OK)
  • Global Angular Error (TODO: why Poor)

  enum ANGULAR_ERROR { LOCAL_ANGULAR_ERROR, GLOBAL_ANGULAR_ERROR };

• Debug

  • Check Measurement Jacobian Matrix

Requirements

tested on Ubuntu 16.04 and Ubuntu 18.04

• Ubuntu 16.04 & ROS Kinetic

  • OpenCV 3
  • ROS package: nmea_navsat_driver
  • GeographicLib 1.50.1 (built from souce code, cmake 3.18.0 tested)

• Ubuntu 18.04 & ROS Melodic

  • OpenCV 3
  • ROS package: nmea_navsat_driver
  • GeographicLib 1.49

    sudo apt install libgeographic-dev

Build

mkdir -p ws_msf/src
cd ws_msf/src
git clone xxx
cd ..
catkin_make -j4 # error happened when using the default cmake 3.5.1 on Ubuntu 16.04, upgrade it
# or
catkin build -j4

Run on Host

IMU + GNSS

test data: utbm_robocar_dataset_20180719_noimage.bag

• [sensor_msgs/Imu] /imu/data: 100 hz
• [nmea_msgs/Sentence] /nmea_sentence: 15 hz
• [sensor_msgs/NavSatFix] /fix: 5 hz
• [nav_msgs/Path] /nav_path: 63 hz

roslaunch imu_x_fusion imu_gnss_fusion.launch
rosbag play -s 25 utbm_robocar_dataset_20180719_noimage.bag

ROS graph and path on rviz:

plot the result path (fusion_gps.csv & fusion_state.csv) on Google Map using the scripts folium_csv.py:

IMU + 6DoF Odom

VO: ORB-SLAM2 (Stereo) + EuRoC V1_01_easy.bag

roslaunch imu_x_fusion imu_vo_fusion.launch [est:=ekf, ukf or map]

run ORB-SLAM2 (Stereo) and play back bag file

# https://github.com/cggos/orbslam2_cg
# pose cov:
# sigma_pv: 0.001
# sigma_rp: 0.5
# sigma_yaw: 0.5
roslaunch orbslam2_ros run_stereo_euroc.launch

rosbag play V1_01_easy.bag

results(Green path: estimated pose; Red path: pose of VO):

Use the recorded bag file directly

Download orbslam2_v101easy.bag

rosbag play orbslam2_v101easy.bag

VO: ORB-SLAM2 (Stereo) + MYNTEYE-S1030 Camera

# TODO: Test
roslaunch imu_x_fusion imu_vo_fusion_mynteye.launch

roslaunch mynt_eye_ros_wrapper mynteye.launch

Run with KAIST Dataset

rosrun imu_x_fusion kaist_pub /dev_sdb/datasets/KAIST/urban39-pankyo

Run with Docker

# pull from DockerHub
sudo docker pull cggos/ubuntu-ros-slam:bionic-melodic

# run, e.g.: imu_vo_fusion
./scripts/run_docker.sh

# modify the script for running others

Code Format

code format based on Google style

./batch_format.sh

Docs

• Requirements

  sudo apt install graphviz
  sudo apt install doxygen

• Generate

  # output in html and latex dir
  doxygen Doxygen.config

Community

• Welcom to join the Discord channel #multi-sensor-fusion