roger-chuh/vins-fisheye-cubemap

a VINS algorithm with a combination of stereo fisheye images, cubemap, line features, dense mapping and loop closure

FYI

This code is modified from HKUST's original "VINS-Fisheye" verion, it now can only run on datasets recorded by intel realsense T265. EUROC or TUM dataset are not supported under this version. I have provided a sample ros bag down below, you can also create your own dataset with a calibrated T265.

The code was compiled under 18.04 Melodic.

in the terminal, type (change the paths accordingly)

1. roslaunch vins fisheye_node.launch config_file:=/home/roger/vins-fisheye2/src/VINS-Fisheye/config/t265/t265_cpu_165_55_bak.yaml
2. rosrun loop_fusion loop_fusion_node /home/roger/vins-fisheye2/src/VINS-Fisheye/config/t265/t265_cpu_165_55_bak.yaml  (optional with loop closure)
3. roslaunch vins vins_rviz.launch
4. rosbag play vio6.bag --clock -r 0.5  (if the pose drifts unexpectedly, lower the play rate of the bag)

references:

1. https://github.com/HKUST-Aerial-Robotics/VINS-Fisheye
2. https://github.com/nkwangyh/CubemapSLAM
3. https://github.com/HeYijia/PL-VIO
4. https://github.com/HKUST-Aerial-Robotics/DenseSurfelMapping

sample ros bag: 链接：https://pan.baidu.com/s/1efJd0ndCIxRMVlrKQceFaw 提取码：qm2v

Some Online Demos

https://www.bilibili.com/video/BV1wT4y1C7hy/ (underground parking lot under highly dynamic illumination changes, total travelled length 500m, final drift 3m)

https://www.bilibili.com/video/BV1wS4y1F7Wf/ (indoor localization with additional line features)

https://www.bilibili.com/video/BV13Y411G7jg/ (dense mapping with loop correction)

VINS-Fisheye

This repository is a Fisheye version of VINS-Fusion with GPU and Visionworks acceleration. It can run on Nvidia TX2 in real-time, also provide depth estimation based on fisheye. This project stands as a part of Omni-swarm: A Decentralized Omnidirectional Visual-Inertial-UWB State Estimation System for Aerial Swarm. You may use it alone on any type of robot or as a part of Omni-swarm for swarm robots.

Only stereo visual-inertial-odometry is supported for fisheye cameras now. Loop closure module for fisheye camera will release later.

Drone path and RGB point cloud estimation

Feature tracker for fisheye

Disparity estimation for depth estimation

1. Prerequisites

The essential software environment is same as VINS-Fusion. Besides, it requires OpenCV cuda version.(Only test it on OpenCV 3.4.1). Visionworks: Optional

If you want to CUDA mode of this package, libSGM is required for depth estimation.

2. Usage

2.1 Change the opencv path in the CMakeLists

This package support CUDA mode and CPU mode with OpenMP enabled. If you are using on embedded device, I strongly recommend you to use this package with CUDA to achieve best performance.

By default, CUDA is automatically detected, however you can disable it by set

set(DETECT_CUDA false)

When using OpenCV, I recommend that you to compile and install opencv 3.4 with CUDA to /usr/local/

If your opencv with CUDA is installed in other localization, modify the

SET("OpenCV_DIR"  "/usr/local/share/OpenCV/")

If you don't have visionworks, please

set(ENABLE_VWORKS false)

NVIDIA VisionWorks gives slightly better performance, however, the VisionWorks support for this package is not stable yet.

Fisheye usage

Term 1

#If use CUDA
roslaunch vins fisheye_split.launch config_file:=/home/your_name/your_ws/src/VINS-Fusion-Fisheye/config/fisheye_ptgrey_n3/fisheye_cuda.yaml
#If use CPU
roslaunch vins fisheye_split.launch config_file:=/home/your_name/your_ws/src/VINS-Fusion-Fisheye/config/fisheye_ptgrey_n3/fisheye_cpu.yaml

Term 2

rosbag play fishey_vins_2020-01-30-10-38-14.bag --clock -s 12

Term 3(for visuallization only)

roslaunch vins vins_rviz.launch

GPU is default enabled, if you are not using CUDA, disable it in yaml config file.

For rosbag, you can download from https://www.dropbox.com/s/kmakksca3ns6cav/fisheye_vins_2020-01-30-10-38-14.bag?dl=0

Parameters for fisheye

depth_config: "depth_cpu.yaml" # config path for depth estimation, depth_cpu.yaml uses opencv SGBM, depth.yaml uses visionworks SGM, you must install visionworks before use visionworks sgm
image_width: 600 # For fisheye, this indicate the flattened image width; min 100; 300 - 500 is good for vins
fisheye_fov: 235 # Your fisheye fov
enable_up_top: 1 #Choose direction you use
enable_down_top: 1
enable_up_side: 1
enable_down_side: 1
enable_rear_side: 1
thres_outlier : 5.0 # outlier thres for backend
tri_max_err: 3.0 #outlier thres for triangulate

depth_estimate_baseline: 0.05 # mini baseline for pts initialization
top_cnt: 30 #number of track point for top view
side_cnt: 30 #number of track point for side view
max_solve_cnt: 30 # Max Point for solve; highly influence performace
show_track: 0 # if display track
use_vxworks: 0 #use vision works for front-end; not as stable as CUDA now

enable_depth: 1 # If estimate depth cloud; only available for dual fisheye now
rgb_depth_cloud: 0 # -1: point no texture,  0 depth cloud will be gray, 1 depth cloud will be colored;
#Note that textured and colored depth cloud will slow down whole system
use_gpu: 1 # If using GPU
use_vworks: 0 # If using visionworks

Parameter for depth estimation

#choose the depth you want estimate
enable_front: 1
enable_left: 1
enable_right: 1
enable_rear: 0
#downsample ration
downsample_ratio: 0.5
#choose use cpu or visionworks
use_vworks: 0

#Publish cloud jump step
pub_cloud_step: 1
#If show dispartity
show_disparity: 0
#If publish depth map image
pub_depth_map: 1
#Publish cloud in radius
depth_cloud_radius: 10
#If publish all depth cloud in a topic
pub_cloud_all: 1
#If publish all depth cloud in every direction
pub_cloud_per_direction: 0

Related Paper

Omni-swarm: A Decentralized Omnidirectional Visual-Inertial-UWB State Estimation System for Aerial Swarm The VINS-Fisheye is a part of Omni-swarm. If you want use VIN-Fisheye as a part of your research project, please cite this paper.

See also

Autonomous aerial robot using dual‐fisheye cameras, Wenliang Gao, Kaixuan Wang, Wenchao Ding, Fei Gao, Tong Qin, Shaojie Shen, 2020 Journal of Field Robotics (JFR) for Fisheye camera navigation. The basic idea of this project is from this paper.

H. Xu, L. Wang, Y. Zhang and S. Shen. (2020) Decentralized visual-inertial-UWB fusion for relative state estimation of aerial swarm. in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE. for swarm localization which is this project developed for.

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:

Related Papers: (papers are not exactly same with code)

• A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors, Tong Qin, Jie Pan, Shaozu Cao, Shaojie Shen, aiXiv pdf

• A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors, Tong Qin, Shaozu Cao, Jie Pan, Shaojie Shen, aiXiv pdf

• 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. Acknowledgements

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

8. 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>.

vins-fisheye-cubemap's Issues

some test problem

Hi @Roger-Chuh Thanks for your sharing code.

I met some problem when I use the code:

1. when I compile the origin code, some error occur:
   feature_tracker.cpp:893 : error: no match for ‘operator=’ prev_pyr = cur_pyr;
   depth_estimator.h:117:17: error: no matching function for call to ‘DepthEstimator::ComputeDispartiyMap’
   I change the function that the error occur to the same as the origin VINS-Fisheye , these error solved.
2. After compiling the code , when I run the code, error as follows:
   [ INFO] [1648702997.899553155]: Unsynchronized sensors, online estimate time offset, initial td: -0.0347699
   [ INFO] [1648702997.899569728]: ROW: 500 COL: 800
   exitrinsic cam 0
   -0.999964 0.00643933 -0.00549328
   0.00547534 -0.00280139 -0.999981
   -0.0064546 -0.999975 0.00276603
   0.00395783 -0.0081369 -0.0179032
   exitrinsic cam 1
   0.999907 -0.00548477 0.0124673
   -0.0124733 -0.0010551 0.999922
   -0.00547119 -0.999984 -0.00112341
   0.00548184 0.0122127 -0.0177637
   set g 0 0 9.81
   [ INFO] [1648702997.899718266]: reading paramerter of camera up.yaml
   [ INFO] [1648702997.899806854]: Use as up.yaml
   [ INFO] [1648702997.917099886]: Build for camera 0
   [ INFO] [1648702997.917233466]: Center FOV: 1.745329_center
   [ INFO] [1648702997.917285663]: Side image height: 313
   [ INFO] [1648702998.031617980]: reading paramerter of camera down.yaml
   [ INFO] [1648702998.031734827]: Use as down.yaml
   [ INFO] [1648702998.049044690]: Build for camera 1
   [ INFO] [1648702998.049067593]: Center FOV: 1.745329_center
   [ INFO] [1648702998.049074061]: Side image height: 313
   Is camera 1 will invert T
   [ INFO] [1648702998.152713079]: Will 0 GPU
   [ WARN] [1648702998.152734909]: waiting for image and imu...
   [ INFO] [1648702998.161949501]: Flatten read fisheyeup.yaml, id 0
   [ INFO] [1648702998.179105292]: Build for camera 0
   [ INFO] [1648702998.179133210]: Center FOV: 1.745329_center
   [ INFO] [1648702998.179140832]: Side image height: 313
   [ INFO] [1648702998.282012022]: Flatten read fisheye down.yaml, id 1
   [ INFO] [1648702998.299004340]: Build for camera 1
   [ INFO] [1648702998.299030236]: Center FOV: 1.745329_center
   [ INFO] [1648702998.299037288]: Side image height: 313
   Is camera 1 will invert T
   [ INFO] [1648702998.408051436]: Will directly receive raw images /cam0/image_raw and /cam1/image_raw
   disable 0 4 5 9: 0 0 0 0
   [ WARN] [1648703004.460655130]: Duration between two images is 1643171961.539995ms

assigning pinhle params... , focal_x = 0.000
disable 0 4 5 9: 0 0 0 0
disable 0 4 5 9: 0 0 0 0
disable 0 4 5 9: 0 0 0 0
disable 0 4 5 9: 0 0 0 0
disable 0 4 5 9: 0 0 0 0
disable 0 4 5 9: 0 0 0 0
lsd.size: 62
up_top_img size: 800 x 800
down_top_img size: 800 x 800
up_side_img size: 1426 x 1426
down_side_img size: 1426 x 1426
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(3.4.16) /home/rui/gwm/opencv-3.4.16/modules/core/src/matrix.cpp:751: error: (-215:Assertion failed) 0 <= roi.x && 0 <= roi.width && roi.x + roi.width <= m.cols && 0 <= roi.y && 0 <= roi.height && roi.y + roi.height <= m.rows in function 'Mat'

terminate called recursively

environments:
ubuntu16.04, ros kinetic, opencv 3.4.16 cuda with contrib

config:
%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2

is_fisheye: 1
imu_topic: "/imu0"

imu_topic: "/dji_sdk_1/dji_sdk/imu"

image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "/home/rui/output"

depth_config: "depth_cuda.yaml"
cam0_calib: "up.yaml"
cam1_calib: "down.yaml"
image_width: 800 # For fisheye, this indicate the flattened image width; min 100; 300 - 500 is good for vins
image_height: 500
show_width: 2000

#fisheye_fov: 200

lk_pyr_level: 3
lk_win_size: 21
keyframe_longtrack_thres: 20

#debug_image: 0
print_log: 0
long_track_ratio: 0.5

equalize: 0
use_line: 1 # 1 # 0 # 1 # 0 # 1
debug_image: 0
image_height_raw: 2880
image_width_raw: 2880
base_line: -1

show_line: 0 # 0 # 1 # 0 # 1 # 0 # 1 # 0
show_disp: 0

line_angle_thres: 0.35 # 1.0 # 0.35 # 1.0 # 0.35 # 0.5
line_pixel_thres: 2.0 # 5.0 # 2.0 # 13.0 # 2.0 # 1.5
use_multi_line_triangulation: 0 # 1 # 0 # 1 # 0 # 1

min_trace_to_marg: 1 # -5

cube_map: 1
use_new: 1

fisheye_fov_actual: 200
fisheye_fov: 200
center_fov: 100

enable_up_top: 1
enable_down_top: 1
enable_up_side: 1
enable_down_side: 1
enable_rear_side: 1
thres_outlier : 3.0
tri_max_err: 5.0

Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't

body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.99990724, -0.00548477,
0.01246729, 0.00548184,
-0.01247326, -0.0010551,
0.99992165, 0.01221265,
-0.00547119, -0.9999844,
-0.00112341, -0.01776372, 0., 0., 0., 1. ]
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -0.99996418, 0.00643933,
-0.00549328, 0.00395783,
0.00547534, -0.00280139,
-0.99998109, -0.0081369,
-0.0064546, -0.99997534,
0.00276603, -0.01790324, 0., 0., 0., 1. ]

pub_flatten: 1
flatten_color: 0
warn_imu_freq: 0
imu_freq: 500
image_freq: 24

multiple_thread: 1
#Gpu accleration support

use_vxworks: 0
use_gpu: 0

enable_depth: 0 # If estimate depth cloud; only available for dual fisheye now
rgb_depth_cloud: -1 # -1: point no texture, 0 depth cloud will be gray, 1 depth cloud will be colored;
#Note that textured and colored depth cloud will slow down whole system

depth_estimate_baseline: 0.05
top_cnt: 200
side_cnt: 25
max_solve_cnt: 500 # Max Point for solve; highly influence performace

min_dist: 20 # min distance between two features, this is for GFTT

min_dist: 20 # for vworks
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1 # ransac threshold (pixel)
show_track: 0 # publish tracking image as topic
show_track_id: 0
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
enable_perf_output: 0

#optimization parameters
max_solver_time: 0.15 # max solver itration time (ms), to guarantee real time
max_num_iterations: 10 # max solver itrations, to guarantee real time

max_solver_time: 1.0 # max solver itration time (ms), to guarantee real time

max_num_iterations: 100 # max solver itrations, to guarantee real time

keyframe_parallax: 12.0 # keyframe selection threshold (pixel)

#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.000945275948987 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.00160297909721 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 2.16684207108e-05 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 3.23006936842e-05 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81 # gravity magnitude

#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: -0.034769903289 #Use mynteye imu
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/rui/output/pose_graph/" # save and load path
save_image: 0

any suggestions? many thanks!