From b0aaf9bb999837f0fb566f74c31e5efbdc97d841 Mon Sep 17 00:00:00 2001
From: Zichao Zhang <zhangzichao17@gmail.com>
Date: Thu, 17 Aug 2017 18:33:01 +0200
Subject: [PATCH] euroc mono

---
 svo_ros/calib/euroc_mono_calib.yaml   | 48 +++++++++++++++++
 svo_ros/calib/euroc_stereo_calib.yaml | 74 +++++++++++++++++++++++++++
 svo_ros/launch/euroc_mono_imu.launch  | 17 ++++++
 svo_ros/param/euroc_mono_imu.yaml     | 27 ++++++++++
 4 files changed, 166 insertions(+)
 create mode 100644 svo_ros/calib/euroc_mono_calib.yaml
 create mode 100644 svo_ros/calib/euroc_stereo_calib.yaml
 create mode 100644 svo_ros/launch/euroc_mono_imu.launch
 create mode 100644 svo_ros/param/euroc_mono_imu.yaml

diff --git a/svo_ros/calib/euroc_mono_calib.yaml b/svo_ros/calib/euroc_mono_calib.yaml
new file mode 100644
index 0000000..85d6bc6
--- /dev/null
+++ b/svo_ros/calib/euroc_mono_calib.yaml
@@ -0,0 +1,48 @@
+label: "Euroc"
+id: 412eab8e4058621f7036b5e765dfe812
+cameras:
+- camera:
+    label: cam0
+    id: 54812562fa109c40fe90b29a59dd7798
+    line-delay-nanoseconds: 0
+    image_height: 480
+    image_width: 752
+    type: pinhole
+    intrinsics:
+      cols: 1
+      rows: 4
+      data: [458.6548807207614, 457.2966964634893, 367.2158039615726, 248.37534060980727]
+    distortion:
+      type: radial-tangential
+      parameters:
+        cols: 1
+        rows: 4
+        data: [-0.28340811217029355, 0.07395907389290132, 0.00019359502856909603,
+    1.7618711454538528e-05]
+  T_B_C:
+    cols: 4
+    rows: 4
+    data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+           0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+          -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+           0.0, 0.0, 0.0, 1.0]
+imu_params:
+  delay_imu_cam: 0.0
+  max_imu_delta_t: 0.01
+  acc_max: 176.0
+  omega_max: 7.8
+  sigma_omega_c: 12.0e-4
+  sigma_acc_c: 8.0e-3
+  sigma_omega_bias_c: 2.0e-5
+  sigma_acc_bias_c: 5.5e-5
+  sigma_integration: 0.0
+  g: 9.8082
+  imu_rate: 800
+
+imu_initialization:
+  velocity: [0.0, 0, 0.0]
+  omega_bias: [0.0, 0, 0.0]
+  acc_bias: [0.0, 0.0, 0.0]
+  velocity_sigma: 2.0
+  omega_bias_sigma: 0.01
+  acc_bias_sigma: 0.1
diff --git a/svo_ros/calib/euroc_stereo_calib.yaml b/svo_ros/calib/euroc_stereo_calib.yaml
new file mode 100644
index 0000000..0ea896d
--- /dev/null
+++ b/svo_ros/calib/euroc_stereo_calib.yaml
@@ -0,0 +1,74 @@
+label: "Euroc"
+id: 412eab8e4058621f7036b5e765dfe812
+cameras:
+- camera:
+    label: cam0
+    id: 54812562fa109c40fe90b29a59dd7798
+    line-delay-nanoseconds: 0
+    image_height: 480
+    image_width: 752
+    type: pinhole
+    intrinsics:
+      cols: 1
+      rows: 4
+      data: [458.6548807207614, 457.2966964634893, 367.2158039615726, 248.37534060980727]
+    distortion:
+      type: radial-tangential
+      parameters:
+        cols: 1
+        rows: 4
+        data: [-0.28340811217029355, 0.07395907389290132, 0.00019359502856909603,
+    1.7618711454538528e-05]
+  T_B_C:
+    cols: 4
+    rows: 4
+    data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+           0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+          -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+           0.0, 0.0, 0.0, 1.0]
+- camera:
+    label: cam1
+    id: 54812562fa109c40fe90b29a59dd7723
+    line-delay-nanoseconds: 0
+    image_height: 480
+    image_width: 752
+    type: pinhole
+
+    intrinsics:
+      cols: 1
+      rows: 4
+      data: [457.5874266035361, 456.13442556023665, 379.9994465203525, 255.2381853862733]
+    distortion:
+      type: radial-tangential
+      parameters:
+        cols: 1
+        rows: 4
+        data: [-0.283683654496, 0.0745128430929, -0.000104738949098, -3.55590700274e-05]
+  T_B_C:
+    cols: 4
+    rows: 4
+    data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
+           0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
+          -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
+           0.0, 0.0, 0.0, 1.0]
+
+imu_params:
+  delay_imu_cam: 0.0
+  max_imu_delta_t: 0.01
+  acc_max: 176.0
+  omega_max: 7.8
+  sigma_omega_c: 12.0e-4
+  sigma_acc_c: 8.0e-3
+  sigma_omega_bias_c: 2.0e-5
+  sigma_acc_bias_c: 5.5e-5
+  sigma_integration: 0.0
+  g: 9.8082
+  imu_rate: 800
+
+imu_initialization:
+  velocity: [0.0, 0, 0.0]
+  omega_bias: [0.0, 0, 0.0]
+  acc_bias: [0.0, 0.0, 0.0]
+  velocity_sigma: 2.0
+  omega_bias_sigma: 0.01
+  acc_bias_sigma: 0.1
diff --git a/svo_ros/launch/euroc_mono_imu.launch b/svo_ros/launch/euroc_mono_imu.launch
new file mode 100644
index 0000000..17696db
--- /dev/null
+++ b/svo_ros/launch/euroc_mono_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="/cam0/image_raw" type="str" />
+    <param name="imu_topic" value="/imu0" type="str" />
+    <param name="calib_file" value="$(find svo_ros)/calib/euroc_mono_calib.yaml" type="str" />
+
+    <rosparam file="$(find svo_ros)/param/pinhole.yaml" />
+    <rosparam file="$(find svo_ros)/param/euroc_mono_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>
diff --git a/svo_ros/param/euroc_mono_imu.yaml b/svo_ros/param/euroc_mono_imu.yaml
new file mode 100644
index 0000000..ff721f9
--- /dev/null
+++ b/svo_ros/param/euroc_mono_imu.yaml
@@ -0,0 +1,27 @@
+grid_size: 35
+use_threaded_depthfilter: False
+use_imu: True
+poseoptim_prior_lambda: 0.0
+img_align_prior_lambda_rot: 0.5
+img_align_prior_lambda_trans: 0.0
+
+# if the number of features are below this number, consider as failure
+quality_min_fts: 40
+# if the number of features reduce by this number for consecutive frames, consider as failure
+quality_max_drop_fts: 80
+
+map_scale: 5.0
+kfselect_criterion: FORWARD
+kfselect_numkfs_upper_thresh: 180
+kfselect_numkfs_lower_thresh: 90
+kfselect_min_dist_metric: 0.001
+kfselect_min_angle: 6
+kfselect_min_disparity: 40
+kfselect_min_num_frames_between_kfs: 1
+
+img_align_est_illumination_gain: true
+img_align_est_illumination_offset: true
+depth_filter_affine_est_offset: true
+depth_filter_affine_est_gain: true
+reprojector_affine_est_offset: true
+reprojector_affine_est_gain: true