add launch and parameter files for stereo + imu

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

svo_ros/launch/fla_stereo_imu.launch

@@ -0,0 +1,17 @@
+<launch>
+  <!-- SVO -->
+  <node pkg="svo_ros" type="svo_node" name="svo" clear_params="true" output="screen">
+    <!-- Camera and IMU topics to subscribe to -->
+    <param name="cam0_topic" value="/sync/cam0/image_raw" type="str" />
+    <param name="cam1_topic" value="/sync/cam1/image_raw" type="str" />
+    <param name="imu_topic" value="/sync/imu/imu" type="str" />
+    <param name="calib_file" value="$(find svo_ros)/calib/fla_stereo_imu.yaml" type="str" />
+
+    <rosparam file="$(find svo_ros)/param/fla_stereo_imu.yaml" />
+
+  </node>
+
+  <!-- RVIZ + SVO GUI -->
+  <node name="vis" pkg="rviz" type="rviz" args=" -d $(find svo_ros)/rviz_config.rviz" />
+  <node name="svo_gui" pkg="rqt_gui" type="rqt_gui" args="-s rqt_svo.svo.Svo --args --topic svo" />
+</launch>

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