added msf config file

aslam_offline_calibration/kalibr/python/kalibr_msf_config

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+import kalibr_common as kc
+
+import numpy as np
+import pylab as pl
+import sm
+import argparse
+import sys
+import rospkg
+
+from matplotlib.colors import LogNorm 
+from sm.libsm_python import Transformation
+np.set_printoptions(suppress=True)
+
+if __name__ == "__main__":        
+    parser = argparse.ArgumentParser(description='Convert camchain files generated for each imu into a msf_config file.')
+    parser.add_argument('--viimu', dest='viYaml', help='Camera configuration as yaml file relative to the vi imu', required=True)
+    parser.add_argument('--mavimu', dest='mavYaml', help='Camera configuration as yaml file relative to the mav imu', required=True)
+    parser.add_argument('--out', dest='outLoc', help='Path to output parameters to', required=False)
+    parsed = parser.parse_args()
+
+    #load the transforms
+    camchain_SB = kc.ConfigReader.CameraChainParameters(parsed.viYaml)
+    T_SB = camchain_SB.getExtrinsicsImuToCam(0)
+    camchain_B = kc.ConfigReader.CameraChainParameters(parsed.mavYaml)
+    T_B = camchain_B.getExtrinsicsImuToCam(0)
+
+    #find the transformation between the imus
+    T_SB_B = T_SB.inverse().T().dot(T_B.T());    
+    q_SB_B = sm.r2quat(T_SB_B[0:3,0:3])
+
+    # get an instance of RosPack with the default search paths
+    rospack = rospkg.RosPack()
+
+    # get the file path
+    kalibr_path = rospack.get_path('kalibr')
+
+    #write the header
+    with open(kalibr_path + '/python/msf_header.txt', 'r') as file_:
+        CONFIG = file_.read()
+
+    #write the body
+    CONFIG += "pose_sensor/init/q_ic/x: {0}\n".format(q_SB_B[0])
+    CONFIG += "pose_sensor/init/q_ic/y: {0}\n".format(q_SB_B[1])
+    CONFIG += "pose_sensor/init/q_ic/z: {0}\n".format(q_SB_B[2])
+    CONFIG += "pose_sensor/init/q_ic/w: {0}\n\n".format(q_SB_B[3])
+    CONFIG += "pose_sensor/init/p_ic/x: {0}\n".format(T_SB_B[0,3])
+    CONFIG += "pose_sensor/init/p_ic/y: {0}\n".format(T_SB_B[1,3])
+    CONFIG += "pose_sensor/init/p_ic/z: {0}\n".format(T_SB_B[2,3])
+
+    #write the footer
+    with open(kalibr_path + '/python/msf_footer.txt', 'r') as file_:
+        CONFIG += file_.read()
+
+    #output the stuff
+    if(parsed.outLoc):
+        with open(parsed.outLoc, 'w') as file_:
+          file_.write(CONFIG)
+    else:
+        print
+        print "Copy the following block to your msf configuration:"
+        print "-----------------------------------------------------"
+        print
+        print CONFIG

aslam_offline_calibration/kalibr/python/msf_footer.txt

@@ -0,0 +1,10 @@
+
+pose_sensor/pose_fixed_scale: true
+pose_sensor/pose_fixed_p_ic: true
+pose_sensor/pose_fixed_q_ic: true
+pose_sensor/pose_fixed_p_wv: true
+pose_sensor/pose_fixed_q_wv: true
+
+pose_sensor/pose_absolute_measurements: true
+pose_sensor/pose_use_fixed_covariance: true
+pose_sensor/pose_measurement_world_sensor: true  # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam

aslam_offline_calibration/kalibr/python/msf_header.txt

@@ -0,0 +1,19 @@
+core/data_playback: true
+core/fixed_bias: 0
+
+#########IMU PARAMETERS#######
+core/core_noise_acc: 0.083
+core/core_noise_accbias: 0.00005
+core/core_noise_gyr: 0.0013
+core/core_noise_gyrbias: 0.0000013
+
+pose_sensor/fixed_scale: 1.0
+pose_sensor/pose_noise_scale: 0.0
+pose_sensor/pose_noise_p_wv: 0.0
+pose_sensor/pose_noise_q_wv: 0.0
+pose_sensor/pose_noise_q_ic: 0.0
+pose_sensor/pose_noise_p_ic: 0.0
+pose_sensor/pose_delay: -0.08
+pose_sensor/pose_noise_meas_p: 0.002
+pose_sensor/pose_noise_meas_q: 0.005
+