From 9d9f7bb97510217faa6d754de5a664edd99043ca Mon Sep 17 00:00:00 2001
From: ke <sunke.polyu@gmail.com>
Date: Fri, 12 Jan 2018 13:22:46 -0500
Subject: [PATCH] Add the example calibration and launch files with no
 extrinsics

---
 .../camchain-imucam-euroc-noextrinsics.yaml   | 38 ++++++++++++
 launch/msckf_vio_euroc_noextrinsics.launch    | 61 +++++++++++++++++++
 2 files changed, 99 insertions(+)
 create mode 100644 config/camchain-imucam-euroc-noextrinsics.yaml
 create mode 100644 launch/msckf_vio_euroc_noextrinsics.launch

diff --git a/config/camchain-imucam-euroc-noextrinsics.yaml b/config/camchain-imucam-euroc-noextrinsics.yaml
new file mode 100644
index 0000000..dc8c2d0
--- /dev/null
+++ b/config/camchain-imucam-euroc-noextrinsics.yaml
@@ -0,0 +1,38 @@
+# The modifications of the output file from Kalibr:
+# 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix.
+# 2. Add the T_imu_body at the end of the calibration file (usually set to identity).
+cam0:
+  T_cam_imu:
+    [0.0, 1.0, 0.0, 0.05,
+    -1.0, 0.0, 0.0, 0.0,
+     0.0, 0.0, 1.0, 0.0,
+     0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
+  distortion_model: radtan
+  intrinsics: [458.654, 457.296, 367.215, 248.375]
+  resolution: [752, 480]
+  timeshift_cam_imu: 0.0
+cam1:
+  T_cam_imu:
+    [0.012555267089103,   0.999598781151433,  -0.025389800891747,  -0.044901980682509,
+    -0.999755099723116,   0.013011905181504,   0.017900583825251,  -0.020569771258915,
+     0.018223771455443,   0.025158836311552,   0.999517347077547,  -0.008638135126028,
+                     0,                   0,                   0,   1.000000000000000]
+  T_cn_cnm1:
+    [0.999997256477881,   0.002312067192424,   0.000376008102415,  -0.110073808127187,
+    -0.002317135723281,   0.999898048506644,   0.014089835846648,   0.000399121547014,
+    -0.000343393120525,  -0.014090668452714,   0.999900662637729,  -0.000853702503357,
+                     0,                   0,                   0,   1.000000000000000]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
+  distortion_model: radtan
+  intrinsics: [457.587, 456.134, 379.999, 255.238]
+  resolution: [752, 480]
+  timeshift_cam_imu: 0.0
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]
+
diff --git a/launch/msckf_vio_euroc_noextrinsics.launch b/launch/msckf_vio_euroc_noextrinsics.launch
new file mode 100644
index 0000000..23e7274
--- /dev/null
+++ b/launch/msckf_vio_euroc_noextrinsics.launch
@@ -0,0 +1,61 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-euroc-noextrinsics.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_euroc.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen">
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="2.742e-3"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="4e-4"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>