roger-chuh/vins-fisheye-cubemap

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

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!