add scripts to interpolate and apply calibration to inertial measurem…

…ents; phab data seems working already ...

cfg/phab.json

@@ -52,11 +52,9 @@
     "bg"  : [8.93594e-05, -0.000113499, -4.95253e-05],
     "ba"  : [0.106957, 0.0822465, -0.092585],
 
-    "Wbc" : [[-0.99952504,  0.00750192, -0.02989013],
-             [0.02961534, -0.03439736, -0.99896935],
-             [-0.00852233, -0.99938008,  0.03415885]],
+    "Wbc" : [-2.21, -2.21, 0],
     // Wbc in tangent space: [-0.0418, -2.172, 2.248]
-    "Tbc": [0.04557484, -0.0711618, -0.04468125],
+    "Tbc": [0, 0, 0],
     "Wg"  : [0, 0, 0],
     "td"  : 0
   },
@@ -67,8 +65,8 @@
     "V"   : 0.5,
     "bg"  : 1e-4,
     "ba"  : 1e-3,
-    "Wbc" : 0.0001,
-    "Tbc" : 0.001,
+    "Wbc" : 3.1,
+    "Tbc" : 0.01,
     "Wg"  : 3.01,
     "td"  : 1e-5, // 1ms
     "Cg"  : 1e-5,

node/launch/xivo.launch

@@ -1,10 +1,5 @@
 <!-- -*- mode: XML -*- -->
 <launch>
-   <!-- Launch Rviz -->
-  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d  $(find vlcore2)/misc/vlCore.rviz" /> -->
-
-   <!-- Get the covis Node -->
   <include file="$(find xivo)/launch/xivo.xml" />
-
 </launch>
 

node/launch/xivo.xml

@@ -1,6 +1,5 @@
 <launch>
 
-  <!-- Launch Corvis Filter Node  -->
   <!-- launch-prefix="xterm -e gdb \-\-args" for debugging -->
 
   <!-- launch-prefix="xterm -e gdb \-\-args" for debugging -->
@@ -9,11 +8,11 @@
   <!-- launch-prefix="GLOG_v=0 GLOG_logtostderr=-1 GLOG_log_dir=log" logging options -->
 
   <node pkg="xivo" type="mynode" name="xivo" output="screen" >
-    <param name="config_path" value="$(find xivo)/../cfg/pointgrey_xsens.json"/>
-    <param name="viewer_config_path" value="$(find xivo)/../cfg/pointgrey_viewer.json"/>
+    <param name="config_path" value="$(find xivo)/../cfg/phab.json"/>
+    <param name="viewer_config_path" value="$(find xivo)/../cfg/phab_viewer.json"/>
     <param name="estimator_queue_size" value="1000" />
-    <param name="image_topic"   value="/camera/image_raw" />
-    <param name="imu_topic"   value="/imu/data" />
+    <param name="image_topic"   value="/cam0/image_raw" />
+    <param name="imu_topic"   value="/imu0_calib" />
     <param name="viewer_type" value="pangolin" />
     <!-- <param name="viewer_type" value="ros" /> -->
   </node>

scripts/correct_imu.py

@@ -0,0 +1,84 @@
+# apply scale and misalignment correction to IMU measurement
+# usage:
+# python correct_imu.py --input /INPUT/CSV/FILE/OF/IMU/MEASUREMENTS --output /OUTPUT/CSV/FILE/OF/IMU/MEASUREMENTS
+import numpy as np
+import argparse
+import sys
+import os
+
+parser = argparse.ArgumentParser()
+parser.add_argument('--input', type=str, required=True,
+        help='input CSV file of inertial measurements')
+parser.add_argument('--output', type=str, required=True,
+        help='output CSV file')
+args = parser.parse_args()
+
+def load_imu_intrinsics(filename, tag=''):
+  ''' Load scaling, misalgnment matrix and bias vector from calibration results
+  returned by imu_tk library.
+  '''
+  with open(filename, 'r') as fid:
+    lines = fid.readlines()
+    T = []  # misalignment
+    for i in range(3):
+      T.append([float(x) for x in lines[i].split()])
+    T = np.array(T)
+    # skip blank line
+    K = []  # scaling
+    for i in range(4, 7):
+      K.append([float(x) for x in lines[i].split()])
+    K = np.array(K)
+    # skip blank line
+    # bias
+    b = np.array([float(x) for x in lines[8:11]])
+
+  print("T{}=\n{}".format(tag, T))
+  print("K{}=\n{}".format(tag, K))
+  print("b{}=\n{}".format(tag, b))
+
+  return T, K, b
+
+if __name__ == '__main__':
+  # load imu intrinsics
+  Ta, Ka, ba = load_imu_intrinsics('test_imu_acc.calib', 'a')
+  Tg, Kg, bg = load_imu_intrinsics('test_imu_gyro.calib', 'g')
+
+
+  csv_filename =args.input
+  output = args.output
+
+  raw = []
+  with open(csv_filename, 'r') as fid:
+    # skip the header
+    head = fid.readline()
+    for line in fid.readlines():
+      data = line.split(',')
+      ts = int(data[0])
+      # gyro = np.array([float(x) for x in data[1:4]])
+      # accel = np.array([float(x) for x in data[4:]])
+
+      gyro = np.array([float(x) for x in data[1:4]])
+      accel = np.array([float(x) for x in data[4:7]])
+
+      if np.linalg.norm(gyro) == 0 or np.linalg.norm(accel) == 0:
+          raise ValueError('empty gyro or accel! need interpolation first?')
+
+      raw.append((ts, gyro, accel))
+
+
+  if len(raw) > 0:
+    corrected = []
+    for ts, gyro, accel in raw:
+
+      calib_gyro = Tg.dot(Kg.dot(gyro + bg))
+      calib_accel = Ta.dot(Ka.dot(accel + ba))
+
+      corrected.append((ts, calib_gyro, calib_accel))
+
+    with open(output, 'w') as ofid:
+      ofid.write(head)
+      for ts, gyro, accel in corrected:
+        ofid.write('{},{},{},{},{},{},{}\n'.format(ts, gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2]))
+
+  else:
+    raise ValueError('empty list of raw measurement!')

scripts/interpolate_imu.py

@@ -0,0 +1,112 @@
+# Perform linear interpolation on asynchronized inertial measurements.
+# usage:
+# python interpolate_imu.py --input /INPUT/CSV/FILE/OF/IMU/MEASUREMENTS --output /OUTPUT/CSV/FILE/OF/IMU/MEASUREMENTS
+import numpy as np
+import matplotlib.pyplot as plt
+import argparse, sys, os
+
+parser = argparse.ArgumentParser()
+parser.add_argument('--input', type=str, required=True,
+        help='input CSV file of inertial measurements')
+parser.add_argument('--output', type=str, required=True,
+        help='output CSV file')
+parser.add_argument('--check-ts', action='store_true', default=False,
+        help='if set, check timestamps')
+args = parser.parse_args()
+
+def interpolate(now, prev_m, next_m):
+  t0, m0 = prev_m
+  t1, m1 = next_m
+  assert now > t0 and now < t1
+  duration = t1 - t0
+  return m0 * (t1 - now) / duration + m1 * (now - t0) / duration
+
+def find_next(curr, data, field):
+  if curr >= len(data):
+    return None
+
+  for i, (ts, gyro, accel) in enumerate(data[curr + 1:]):
+    if field == 'gyro' and np.linalg.norm(gyro) > 0:
+      return (ts, gyro)
+
+    if field == 'accel' and np.linalg.norm(accel) > 0:
+      return (ts, accel)
+
+  return None
+
+def find_next_gyro(curr, data):
+  return find_next(curr, data, field='gyro')
+
+def find_next_accel(curr, data):
+  return find_next(curr, data, field='accel')
+
+
+
+
+if __name__ == '__main__':
+
+  csv_filename =args.input
+  output = args.output
+
+  raw = []
+  with open(csv_filename, 'r') as fid:
+    # skip the header
+    head = fid.readline()
+    for line in fid.readlines():
+      data = line.split(',')
+      ts = int(data[0])
+      # gyro = np.array([float(x) for x in data[1:4]])
+      # accel = np.array([float(x) for x in data[4:]])
+
+      gyro = np.array([float(x) for x in data[1:4]])
+      accel = np.array([float(x) for x in data[4:7]])
+
+      raw.append((ts, gyro, accel))
+
+
+  if len(raw) > 0:
+    interp = []
+    prev_gyro = None
+    prev_accel = None
+    for i, (ts, gyro, accel) in enumerate(raw):
+      if np.linalg.norm(gyro) == 0 and np.linalg.norm(accel) == 0:
+        raise ValueError('both gyro and accel measurements are empty!')
+
+      if np.linalg.norm(gyro) == 0:
+        if prev_gyro is None:
+          prev_gyro = (ts, gyro)
+        else:
+          next_gyro = find_next_gyro(i, raw)
+          if next_gyro is not None:
+            gyro = interpolate(ts, prev_gyro, next_gyro)
+            interp.append((ts, gyro, accel))
+        # accel must not be empty, update
+        prev_accel = (ts, accel)
+      elif np.linalg.norm(accel) == 0:
+        if prev_accel is None:
+          prev_accel = (ts, accel)
+        else:
+          next_accel = find_next_accel(i, raw)
+          if next_accel is not None:
+            accel = interpolate(ts, prev_accel, next_accel)
+            interp.append((ts, gyro, accel))
+        # gyro must not be empty, update
+        prev_gyro = (ts, gyro)
+      else:
+        # both fields are non-empty, forward
+        prev_gyro, prev_accel = gyro, accel
+        interp.append((ts, gyro, accel))
+
+    with open(output, 'w') as ofid:
+      ofid.write(head)
+      for ts, gyro, accel in interp:
+        ofid.write('{},{},{},{},{},{},{}\n'.format(ts, gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2]))
+
+    if args.check_ts:
+      ts = [triplet[0] for triplet in interp]
+      dt = np.diff(ts) * 1e-6
+      plt.plot(dt, '.')
+      plt.show()
+
+  else:
+    raise ValueError('empty list of raw measurement!')