diff --git a/svo_ros/calib/fla_stereo_imu.yaml b/svo_ros/calib/fla_stereo_imu.yaml new file mode 100644 index 0000000..731af12 --- /dev/null +++ b/svo_ros/calib/fla_stereo_imu.yaml @@ -0,0 +1,75 @@ +cameras: +- camera: + distortion: + parameters: + cols: 1 + rows: 4 + data: [-0.24252919348116794, 0.03930108103982388, 0.0008221249960346152, -0.002043418590796441] + type: radial-tangential + image_height: 1024 + image_width: 1280 + intrinsics: + cols: 1 + rows: 4 + data: [637.7135749684152, 635.2114915927144, 664.9670062685065, 489.8734610241461] + label: /sync/cam0/image_raw + line-delay-nanoseconds: 0 + type: pinhole + T_B_C: + cols: 4 + rows: 4 + data: [0.99984983, -0.00373198, 0.01692284, -0.074783895, + 0.00403133, 0.9998354 , -0.01768969, -0.0005 , + -0.01685403, 0.01775525, 0.9997003 , -0.0041, + 0. , 0. , 0. , 1. ] + serial_no: 0 + calib_date: 0 + description: 'left cam' +- camera: + distortion: + parameters: + cols: 1 + rows: 4 + data: [-0.25170969692935957, 0.04478134226090527, 0.0002848490302781225, 0.002230802727272928] + type: radial-tangential + image_height: 1024 + image_width: 1280 + intrinsics: + cols: 1 + rows: 4 + data: [645.9125584269032, 643.8200253341743, 606.2090195940297, 508.4909138768748] + label: /sync/cam1/image_raw + line-delay-nanoseconds: 0 + type: pinhole + T_B_C: + cols: 4 + rows: 4 + data: [0.99746136, -0.00394329, -0.07110054, 0.074783895, + 0.00236645, 0.99974967, -0.02224833, 0.0005, + 0.07117047, 0.02202359, 0.997221 , 0.0041, + 0. , 0. , 0. , 1. ] + serial_no: 0 + calib_date: 0 + description: 'right cam' +label: fla1_forward_stereo + +imu_params: + delay_imu_cam: 0.0 + max_imu_delta_t: 0.01 + acc_max: 176.0 + omega_max: 7.8 + sigma_omega_c: 0.005 + sigma_acc_c: 0.01 + sigma_omega_bias_c: 4.0e-6 + sigma_acc_bias_c: 0.0002 + sigma_integration: 0.0 + g: 9.81007 + imu_rate: 200.0 + +imu_initialization: + velocity: [0, 0, 0] + omega_bias: [0, 0, 0] + acc_bias: [0, 0, 0] + velocity_sigma: 0.5 + omega_bias_sigma: 0.005 + acc_bias_sigma: 0.05 diff --git a/svo_ros/launch/fla_stereo_imu.launch b/svo_ros/launch/fla_stereo_imu.launch new file mode 100644 index 0000000..941a9bb --- /dev/null +++ b/svo_ros/launch/fla_stereo_imu.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/svo_ros/param/fla_stereo_imu.yaml b/svo_ros/param/fla_stereo_imu.yaml new file mode 100644 index 0000000..73f47cb --- /dev/null +++ b/svo_ros/param/fla_stereo_imu.yaml @@ -0,0 +1,40 @@ +# Basic +pipeline_is_stereo: True +automatic_reinitialization: True # When lost, stereo can recover immediately + +# IMU +use_imu: True +img_align_prior_lambda_rot: 5.0 # Gyroscope prior in sparse image alignment +poseoptim_prior_lambda: 2.0 # Gyroscope prior in pose optimization + +# Keyframe selection +max_n_kfs: 10 +kfselect_criterion: FORWARD +kfselect_numkfs_upper_thresh: 160 # If it has more features, it never creates a keyframe +kfselect_numkfs_lower_thresh: 70 # If it has less features, it always creates a keyframe +kfselect_min_num_frames_between_kfs: 2 # ..except this. +kfselect_min_disparity: 30 +kfselect_min_angle: 20 +kfselect_min_dist_metric: 0.01 +update_seeds_with_old_keyframes: True + +# Image Alignment +img_align_max_level: 5 # increase this level if image is double the size of VGA +img_align_min_level: 2 +poseoptim_thresh: 3.0 # reprojection threshold, maybe make a bit larger for larger images. + +# Reprojection +use_async_reprojectors: True # For stereo, use multithreading in reprojector +reprojector_max_n_kfs: 10 # Local map size. Larger is computationally more intensive. More reduces drift. + +# Feature detection +max_fts: 180 +grid_size: 50 # Larger for larger images, for every cell you have max one feature +n_pyr_levels: 4 # Increase for larger images (image align max minus one) +detector_threshold_primary: 7 # Fast detector threshold +detector_threshold_secondary: 250 # Edgelet detector threshold + +# Stereo parameters +max_depth_inv: 0.05 +min_depth_inv: 2.0 +mean_depth_inv: 0.3