Add analytics tools

.gitignore

@@ -19,3 +19,9 @@ _output/
 /wrappers/ros/install
 /wrappers/ros/.catkin_workspace
 /wrappers/ros/src/CMakeLists.txt
+
+# tools
+
+*.pyc
+/mynteye/
+/mynteye.bag

tools/README.md

@@ -0,0 +1,40 @@
+# Samples for MYNT® EYE tools
+
+## Prerequisites
+
+Ubuntu 16.04, ROS Kinetic.
+
+```bash
+cd <sdk>/tools/
+sudo pip install -r requirements.txt
+```
+
+## Record data
+
+```bash
+cd <sdk>
+make ros
+```
+
+```bash
+source wrappers/ros/devel/setup.bash
+roslaunch mynt_eye_ros_wrapper mynteye.launch
+```
+
+```bash
+rosbag record -O mynteye.bag /mynteye/left /mynteye/imu /mynteye/temp
+```
+
+## Analytics
+
+### imu_analytics.py
+
+```bash
+python tools/analytics/imu_analytics.py -i mynteye.bag
+```
+
+### stamp_analytics.py
+
+```bash
+python tools/analytics/stamp_analytics.py -i mynteye.bag
+```

tools/analytics/imu_analytics.py

@@ -0,0 +1,653 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# pylint: disable=missing-docstring
+
+from __future__ import print_function
+
+import os
+import sys
+
+TOOLBOX_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.append(os.path.join(TOOLBOX_DIR, 'internal'))
+
+# pylint: disable=import-error,wrong-import-position
+from data import DataError, Dataset, ROSBag, What
+
+
+TIME_SCALE_FACTORS = {
+    's': 1.,
+    'm': 1. / 60,
+    'h': 1. / 3600
+}
+
+
+ANGLE_DEGREES = 'd'
+ANGLE_RADIANS = 'r'
+ANGLE_UNITS = (ANGLE_DEGREES, ANGLE_RADIANS)
+
+
+BIN_CONFIG_NAME = 'imu_analytics_bin.cfg'
+BIN_IMU_NAME = 'imu_analytics_imu.bin'
+BIN_TEMP_NAME = 'imu_analytics_temp.bin'
+
+
+class RawDataset(Dataset):
+
+  def __init__(self, path, dataset_creator):
+    super(RawDataset, self).__init__(path)
+    self.dataset_creator = dataset_creator
+    self._digest()
+
+  def _digest(self):
+    dataset = self.dataset_creator(self.path)
+    results = dataset.collect(What.imu, What.temp)
+    self._dataset = dataset
+    self._results = results
+    self._has_imu = What.imu in results.keys()
+    self._has_temp = What.temp in results.keys()
+    print('  ' + ', '.join('{}: {}'.format(k, len(v))
+                           for k, v in results.items()))
+
+  @staticmethod
+  def _hypot(*args):
+    from math import sqrt
+    return sqrt(sum(x ** 2 for x in args))
+
+  def plot(self, t_scale_factor, gryo_converter,
+           ax_accel_x, ax_accel_y, ax_accel_z, ax_accel,
+           ax_gyro_x, ax_gyro_y, ax_gyro_z, ax_temp):
+    results = self._results
+
+    if self._has_imu:
+      imu_t_beg = results[What.imu][0].timestamp
+      imu_ts = [(imu.timestamp - imu_t_beg) * t_scale_factor
+                for imu in results[What.imu]]
+
+      ax_accel_x.plot(imu_ts, [imu.accel_x for imu in results[What.imu]])
+      ax_accel_y.plot(imu_ts, [imu.accel_y for imu in results[What.imu]])
+      ax_accel_z.plot(imu_ts, [imu.accel_z for imu in results[What.imu]])
+      import math
+      my_gryo_converter = \
+          lambda x: gryo_converter(x, math.degrees, math.radians)
+      ax_gyro_x.plot(imu_ts, [my_gryo_converter(imu.gyro_x)
+                              for imu in results[What.imu]])
+      ax_gyro_y.plot(imu_ts, [my_gryo_converter(imu.gyro_y)
+                              for imu in results[What.imu]])
+      ax_gyro_z.plot(imu_ts, [my_gryo_converter(imu.gyro_z)
+                              for imu in results[What.imu]])
+
+      ax_accel.plot(imu_ts, [self._hypot(imu.accel_x, imu.accel_y, imu.accel_z)
+                             for imu in results[What.imu]])
+
+    if self._has_temp:
+      temp_t_beg = results[What.temp][0].timestamp
+      temp_ts = [(temp.timestamp - temp_t_beg) * t_scale_factor
+                 for temp in results[What.temp]]
+      ax_temp.plot(temp_ts, [temp.value for temp in results[What.temp]])
+
+  def generate(self, *what):  # pylint: disable=unused-argument
+    raise DataError('DataError: method not implemented')
+
+  def iterate(self, action, *what):  # pylint: disable=unused-argument
+    raise DataError('DataError: method not implemented')
+
+  def collect(self, *what):  # pylint: disable=unused-argument
+    raise DataError('DataError: method not implemented')
+
+  @property
+  def timebeg(self):
+    return self._dataset.timebeg
+
+  @property
+  def timeend(self):
+    return self._dataset.timeend
+
+  @property
+  def duration(self):
+    return self._dataset.duration
+
+  @property
+  def has_imu(self):
+    return self._has_imu
+
+  @property
+  def has_temp(self):
+    return self._has_temp
+
+
+class BinDataset(RawDataset):
+  """
+  Binary memory-mapped files of large dataset.
+
+  References:
+    https://stackoverflow.com/questions/5854515/large-plot-20-million-samples-gigabytes-of-data
+    https://stackoverflow.com/questions/1053928/very-large-matrices-using-python-and-numpy
+  """
+
+  # def __init__(self, path, dataset_creator):
+  #   super(BinDataset, self).__init__(path, dataset_creator)
+
+  def _digest(self):
+    bindir = os.path.splitext(self.path)[0]
+    bincfg = os.path.join(bindir, BIN_CONFIG_NAME)
+    if os.path.isfile(bincfg):
+      with open(bincfg, 'r') as f_cfg:
+        import yaml
+        cfg = yaml.load(f_cfg)
+        self._info = cfg['info']
+        self._binimu = os.path.join(bindir, cfg['bins']['imu'])
+        self._bintemp = os.path.join(bindir, cfg['bins']['temp'])
+        print('find binary files ...')
+        print('  binimu: {}'.format(self._binimu))
+        print('  bintemp: {}'.format(self._bintemp))
+        print('  bincfg: {}'.format(bincfg))
+        if self._exists():
+          while True:
+            sys.stdout.write('Do you want to use it directly? [Y/n] ')
+            choice = raw_input().lower()
+            if choice == '' or choice == 'y':
+              return
+            elif choice == 'n':
+              break
+            else:
+              print('Please respond with \'y\' or \'n\'.')
+    self._convert()
+
+  def _exists(self):
+    return os.path.isfile(self._binimu) or os.path.isfile(self._bintemp)
+
+  def _convert(self):
+    import numpy as np
+
+    dataset = self.dataset_creator(self.path)
+    bindir = os.path.splitext(self.path)[0]
+    if not os.path.exists(bindir):
+      os.makedirs(bindir)
+    binimu = os.path.join(bindir, BIN_IMU_NAME)
+    bintemp = os.path.join(bindir, BIN_TEMP_NAME)
+    bincfg = os.path.join(bindir, BIN_CONFIG_NAME)
+    print('save to binary files ...')
+    print('  binimu: {}'.format(binimu))
+    print('  bintemp: {}'.format(bintemp))
+    print('  bincfg: {}'.format(bincfg))
+
+    has_imu = False
+    has_temp = False
+    with open(binimu, 'wb') as f_imu, open(bintemp, 'wb') as f_temp:
+      imu_t_beg = -1
+      imu_count = 0
+      temp_t_beg = -1
+      temp_count = 0
+      for result in dataset.generate(What.imu, What.temp):
+        if What.imu in result:
+          imu = result[What.imu]
+          if imu_t_beg == -1:
+            imu_t_beg = imu.timestamp
+          np.array([(
+              (imu.timestamp - imu_t_beg),
+              imu.accel_x, imu.accel_y, imu.accel_z,
+              self._hypot(imu.accel_x, imu.accel_y, imu.accel_z),
+              imu.gyro_x, imu.gyro_y, imu.gyro_z
+          )], dtype="f8, f8, f8, f8, f8, f8, f8, f8").tofile(f_imu)
+          imu_count = imu_count + 1
+          has_imu = True
+        if What.temp in result:
+          temp = result[What.temp]
+          if temp_t_beg == -1:
+            temp_t_beg = temp.timestamp
+          np.array([(
+              (temp.timestamp - temp_t_beg),
+              temp.value
+          )], dtype="f8, f8").tofile(f_temp)
+          temp_count = temp_count + 1
+          has_temp = True
+        sys.stdout.write('\r  imu: {}, temp: {}'.format(imu_count, temp_count))
+      sys.stdout.write('\n')
+
+    # pylint: disable=attribute-defined-outside-init
+    self._info = {
+        'timebeg': dataset.timebeg,
+        'timeend': dataset.timeend,
+        'duration': dataset.duration,
+        'has_imu': has_imu,
+        'has_temp': has_temp
+    }
+    self._binimu = binimu
+    self._bintemp = bintemp
+
+    with open(bincfg, 'w') as f_cfg:
+      import yaml
+      yaml.dump({'info': self._info, 'bins': {
+          'imu': BIN_IMU_NAME,
+          'temp': BIN_TEMP_NAME
+      }}, f_cfg, default_flow_style=False)
+
+  def plot(self, t_scale_factor, gryo_converter,
+           ax_accel_x, ax_accel_y, ax_accel_z, ax_accel,
+           ax_gyro_x, ax_gyro_y, ax_gyro_z, ax_temp):
+    import numpy as np
+    if self.has_imu:
+      imus = np.memmap(self._binimu, dtype=[
+          ('t', 'f8'),
+          ('accel_x', 'f8'), ('accel_y', 'f8'), ('accel_z', 'f8'),
+          ('accel', 'f8'),
+          ('gyro_x', 'f8'), ('gyro_y', 'f8'), ('gyro_z', 'f8'),
+      ], mode='r')
+      imus_t = imus['t'] * t_scale_factor
+      ax_accel_x.plot(imus_t, imus['accel_x'])
+      ax_accel_y.plot(imus_t, imus['accel_y'])
+      ax_accel_z.plot(imus_t, imus['accel_z'])
+      ax_accel.plot(imus_t, imus['accel'])
+
+      my_gryo_converter = \
+          lambda x: gryo_converter(x, np.degrees, np.radians)
+      ax_gyro_x.plot(imus_t, my_gryo_converter(imus['gyro_x']))
+      ax_gyro_y.plot(imus_t, my_gryo_converter(imus['gyro_y']))
+      ax_gyro_z.plot(imus_t, my_gryo_converter(imus['gyro_z']))
+    if self.has_temp:
+      temps = np.memmap(self._bintemp, dtype=[
+          ('t', 'f8'), ('value', 'f8')
+      ], mode='r')
+      temps_t = temps['t'] * t_scale_factor
+      ax_temp.plot(temps_t, temps['value'])
+
+  @property
+  def timebeg(self):
+    return self._info['timebeg']
+
+  @property
+  def timeend(self):
+    return self._info['timeend']
+
+  @property
+  def duration(self):
+    return self._info['duration']
+
+  @property
+  def has_imu(self):
+    return self._info['has_imu']
+
+  @property
+  def has_temp(self):
+    return self._info['has_temp']
+
+
+def analyze(dataset, profile):
+  if not profile.time_unit:
+    if dataset.duration > 3600:
+      time_unit = 'h'
+    elif dataset.duration > 60:
+      time_unit = 'm'
+    else:
+      time_unit = 's'
+  else:
+    time_unit = profile.time_unit
+
+  t_name = 'time ({})'.format(time_unit)
+  t_scale_factor = TIME_SCALE_FACTORS[time_unit]
+
+  time_limits = profile.time_limits
+  if not time_limits:
+    time_limits = [0, dataset.duration * t_scale_factor]
+  accel_limits = profile.accel_limits
+  gyro_limits = profile.gyro_limits
+  temp_limits = profile.temp_limits
+  auto = profile.auto
+
+  import matplotlib.pyplot as plt
+
+  fig_1 = plt.figure(1, [16, 12])
+  fig_1.suptitle('IMU Analytics')
+  fig_1.subplots_adjust(wspace=0.4, hspace=0.2)
+
+  ax_accel_x = fig_1.add_subplot(241)
+  ax_accel_x.set_title('accel_x')
+  ax_accel_x.set_xlabel(t_name)
+  ax_accel_x.set_ylabel('accel_x (m/s^2)')
+  ax_accel_x.axis('auto')
+  ax_accel_x.set_xlim(time_limits)
+  if not auto and accel_limits and accel_limits[0]:
+    ax_accel_x.set_ylim(accel_limits[0])
+
+  ax_accel_y = fig_1.add_subplot(242)
+  ax_accel_y.set_title('accel_y')
+  ax_accel_y.set_xlabel(t_name)
+  ax_accel_y.set_ylabel('accel_y (m/s^2)')
+  ax_accel_y.axis('auto')
+  ax_accel_y.set_xlim(time_limits)
+  if not auto and accel_limits and accel_limits[1]:
+    ax_accel_y.set_ylim(accel_limits[1])
+
+  ax_accel_z = fig_1.add_subplot(243)
+  ax_accel_z.set_title('accel_z')
+  ax_accel_z.set_xlabel(t_name)
+  ax_accel_z.set_ylabel('accel_z (m/s^2)')
+  ax_accel_z.axis('auto')
+  ax_accel_z.set_xlim(time_limits)
+  if not auto and accel_limits and accel_limits[2]:
+    ax_accel_z.set_ylim(accel_limits[2])
+
+  ax_accel = fig_1.add_subplot(244)
+  ax_accel.set_title('accel hypot(x,y,z)')
+  ax_accel.set_xlabel(t_name)
+  ax_accel.set_ylabel('accel (m/s^2)')
+  ax_accel.axis('auto')
+  ax_accel.set_xlim(time_limits)
+  if not auto and accel_limits and accel_limits[3]:
+    ax_accel.set_ylim(accel_limits[3])
+
+  ax_gyro_ylabels = {
+      ANGLE_DEGREES: 'deg/sec',
+      ANGLE_RADIANS: 'rad/sec'
+  }
+  ax_gyro_ylabel = ax_gyro_ylabels[profile.gyro_show_unit]
+
+  ax_gyro_x = fig_1.add_subplot(245)
+  ax_gyro_x.set_title('gyro_x')
+  ax_gyro_x.set_xlabel(t_name)
+  ax_gyro_x.set_ylabel('gyro_x ({})'.format(ax_gyro_ylabel))
+  ax_gyro_x.axis('auto')
+  ax_gyro_x.set_xlim(time_limits)
+  if not auto and gyro_limits and gyro_limits[0]:
+    ax_gyro_x.set_ylim(gyro_limits[0])
+
+  ax_gyro_y = fig_1.add_subplot(246)
+  ax_gyro_y.set_title('gyro_y')
+  ax_gyro_y.set_xlabel(t_name)
+  ax_gyro_y.set_ylabel('gyro_y ({})'.format(ax_gyro_ylabel))
+  ax_gyro_y.axis('auto')
+  ax_gyro_y.set_xlim(time_limits)
+  if not auto and gyro_limits and gyro_limits[1]:
+    ax_gyro_y.set_ylim(gyro_limits[1])
+
+  ax_gyro_z = fig_1.add_subplot(247)
+  ax_gyro_z.set_title('gyro_z')
+  ax_gyro_z.set_xlabel(t_name)
+  ax_gyro_z.set_ylabel('gyro_z ({})'.format(ax_gyro_ylabel))
+  ax_gyro_z.axis('auto')
+  ax_gyro_z.set_xlim(time_limits)
+  if not auto and gyro_limits and gyro_limits[2]:
+    ax_gyro_z.set_ylim(gyro_limits[2])
+
+  ax_temp = None
+  if dataset.has_temp:
+    ax_temp = fig_1.add_subplot(248)
+    ax_temp.set_title('temperature')
+    ax_temp.set_xlabel(t_name)
+    ax_temp.set_ylabel('temperature (degree Celsius)')
+    ax_temp.axis('auto')
+    ax_temp.set_xlim(time_limits)
+    if not auto and temp_limits:
+      ax_temp.set_ylim(temp_limits)
+
+  def gryo_converter(x, degrees, radians):
+    if profile.gyro_show_unit == profile.gyro_data_unit:
+      return x
+    if profile.gyro_show_unit == ANGLE_DEGREES and \
+            profile.gyro_data_unit == ANGLE_RADIANS:
+      return degrees(x)
+    if profile.gyro_show_unit == ANGLE_RADIANS and \
+            profile.gyro_data_unit == ANGLE_DEGREES:
+      return radians(x)
+    sys.exit('Error: gryo_converter wrong logic')
+
+  dataset.plot(t_scale_factor, gryo_converter,
+               ax_accel_x, ax_accel_y, ax_accel_z, ax_accel,
+               ax_gyro_x, ax_gyro_y, ax_gyro_z, ax_temp)
+
+  outdir = profile.outdir
+  if outdir:
+    figpath = os.path.join(outdir, 'imu_analytics.png')
+    print('save figure to:\n  {}'.format(figpath))
+    if not os.path.exists(outdir):
+      os.makedirs(outdir)
+    fig_1.savefig(figpath, dpi=100)
+
+  plt.show()
+
+
+def _parse_args():
+  def limits_type(string, num=1):
+    if not string:
+      return None
+    if num < 1:
+      sys.exit('Error: limits_type must be greater than one pair')
+    pairs = string.split(':')
+    pairs_len = len(pairs)
+    if pairs_len == 1:
+      values = pairs[0].split(',')
+      if len(values) != 2:
+        sys.exit('Error: limits_type must be two values'
+                 ' as \'min,max\' for each pair')
+      results = (float(values[0]), float(values[1]))
+      if num > 1:
+        return [results for i in xrange(num)]
+      else:
+        return results
+    elif pairs_len == num:
+      results = []
+      for i in xrange(num):
+        if pairs[i]:
+          values = pairs[i].split(',')
+          if len(values) != 2:
+            sys.exit('Error: limits_type must be two values'
+                     ' as \'min,max\' for each pair')
+          results.append((float(values[0]), float(values[1])))
+        else:
+          results.append(None)
+      return results
+    else:
+      sys.exit('Error: limits_type must one or {:d} pairs'.format(num))
+
+  from functools import partial
+
+  import argparse
+  parser = argparse.ArgumentParser(
+      prog=os.path.basename(__file__),
+      formatter_class=argparse.RawTextHelpFormatter,
+      description='usage examples:'
+      '\n  python %(prog)s -i DATASET'
+      '\n  python %(prog)s -i DATASET -al=-10,10'
+      '\n  python %(prog)s -i DATASET -al=-5,5::5,15: -gl=-0.1,0.1:: -kl=')
+  parser.add_argument(
+      '-i',
+      '--input',
+      dest='input',
+      metavar='DATASET',
+      required=True,
+      help='the input dataset path')
+  parser.add_argument(
+      '-o',
+      '--outdir',
+      dest='outdir',
+      metavar='OUTDIR',
+      help='the output directory')
+  parser.add_argument(
+      '-c',
+      '--config',
+      dest='config',
+      metavar='CONFIG',
+      help='yaml config file about input dataset')
+  parser.add_argument(
+      '-tu',
+      '--time-unit',
+      dest='time_unit',
+      metavar='s|m|h',
+      help='the time unit (seconds, minutes or hours)')
+  parser.add_argument(
+      '-gdu',
+      '--gyro-data-unit',
+      dest='gyro_data_unit',
+      metavar='r|d',
+      default='r',
+      help='the gyro data unit (radians or degrees, default: %(default)s)')
+  parser.add_argument(
+      '-gsu',
+      '--gyro-show-unit',
+      dest='gyro_show_unit',
+      metavar='r|d',
+      help='the gyro show unit (radians or degrees, '
+      'default: same as gyro data unit)')
+  parser.add_argument(
+      '-tl',
+      '--time-limits',
+      dest='time_limits',
+      metavar='min,max',
+      type=limits_type,
+      help='the time limits, in time unit')
+  parser.add_argument(
+      '-al',
+      '--accel-limits',
+      dest='accel_limits',
+      metavar='min,max [min,max:...]',
+      default='-10,10',
+      type=partial(limits_type, num=4),
+      help='the accel limits (default: %(default)s)'
+      '\n  or 4 limits of accel_x,y,z,accel like \'min,max:...\'')
+  parser.add_argument(
+      '-gl',
+      '--gyro-limits',
+      dest='gyro_limits',
+      metavar='min,max [min,max:...]',
+      default='-0.02,0.02',
+      type=partial(limits_type, num=3),
+      help='the gyro limits (default: %(default)s)'
+      '\n  or 3 limits of gyro_x,y,z like \'min,max:...\'')
+  parser.add_argument(
+      '-kl',
+      '--temp-limits',
+      dest='temp_limits',
+      metavar='min,max',
+      default='-20,80',
+      type=limits_type,
+      help='the temperature limits (default: %(default)s)')
+  parser.add_argument(
+      '-l',
+      '--limits',
+      dest='all_limits',
+      metavar='min,max [min,max:...]',
+      # nargs='+',
+      type=partial(limits_type, num=8),
+      help='the all limits, absent one will auto scale'
+      '\n  accel_x,y,z,accel,gyro_x,y,z,temp like \'min,max:...\'')
+  parser.add_argument(
+      '-a',
+      '--auto',
+      dest='auto',
+      action='store_true',
+      help='make all limits auto scale to data limits, except the time')
+  parser.add_argument(
+      '-b',
+      '--binary',
+      dest='binary',
+      action='store_true',
+      help='save large dataset to binary files'
+      ', and plot them with numpy.memmap()')
+  return parser.parse_args()
+
+
+def _dict2obj(d):
+  from collections import namedtuple
+  return namedtuple('X', d.keys())(*d.values())
+
+
+def _main():
+  args = _parse_args()
+  # print(args)
+
+  dataset_path = args.input
+  if not dataset_path or not os.path.exists(dataset_path):
+    sys.exit('Error: the dataset path not exists, %s' % dataset_path)
+  dataset_path = os.path.normpath(dataset_path)
+
+  outdir = args.outdir
+  if not outdir:
+    outdir = os.path.splitext(dataset_path)[0]
+  else:
+    outdir = os.path.abspath(outdir)
+
+  print('imu analytics ...')
+  print('  input: %s' % dataset_path)
+  print('  outdir: %s' % outdir)
+
+  profile = {
+      'auto': False,
+      'time_unit': None,
+      'gyro_data_unit': None,
+      'gyro_show_unit': None,
+      'time_limits': None,
+      'accel_limits': None,
+      'gyro_limits': None,
+      'temp_limits': None
+  }
+  profile['auto'] = args.auto
+
+  if args.time_unit:
+    if args.time_unit not in TIME_SCALE_FACTORS.keys():
+      sys.exit('Error: the time unit must be \'s|m|h\'')
+    else:
+      profile['time_unit'] = args.time_unit
+
+  if args.gyro_data_unit:
+    if args.gyro_data_unit not in ANGLE_UNITS:
+      sys.exit('Error: the gyro unit must be \'r|d\'')
+    else:
+      profile['gyro_data_unit'] = args.gyro_data_unit
+  else:
+    profile['gyro_data_unit'] = ANGLE_RADIANS
+
+  if args.gyro_show_unit:
+    if args.gyro_show_unit not in ANGLE_UNITS:
+      sys.exit('Error: the gyro unit must be \'r|d\'')
+    else:
+      profile['gyro_show_unit'] = args.gyro_show_unit
+  else:
+    profile['gyro_show_unit'] = profile['gyro_data_unit']
+
+  if args.time_limits:
+    if not args.time_unit:
+      sys.exit('Error: the time unit must be set')
+    profile['time_limits'] = args.time_limits
+
+  if args.all_limits:
+    profile['accel_limits'] = args.all_limits[:4]
+    profile['gyro_limits'] = args.all_limits[4:7]
+    profile['temp_limits'] = args.all_limits[7]
+  else:
+    profile['accel_limits'] = args.accel_limits
+    profile['gyro_limits'] = args.gyro_limits
+    profile['temp_limits'] = args.temp_limits
+
+  for k, v in profile.items():
+    print('  {}: {}'.format(k, v))
+
+  def dataset_creator(path):
+    print('open dataset ...')
+    if args.config:
+      import yaml
+      config = yaml.load(file(args.config, 'r'))
+      if config['dataset'] != 'rosbag':
+        sys.exit('Error: dataset model only support rosbag now')
+      dataset = ROSBag(path, **config['rosbag'])
+    else:
+      dataset = ROSBag(
+          path,
+          topic_imu='/mynteye/imu',
+          topic_temp='/mynteye/temp')
+    return dataset
+
+  if args.binary:
+    dataset = BinDataset(dataset_path, dataset_creator)
+  else:
+    dataset = RawDataset(dataset_path, dataset_creator)
+  print('  timebeg: {:f}, timeend: {:f}, duration: {:f}'.format(
+      dataset.timebeg, dataset.timeend, dataset.duration))
+
+  profile['outdir'] = outdir
+  analyze(dataset, _dict2obj(profile))
+
+  print('imu analytics done')
+
+
+if __name__ == '__main__':
+  _main()

tools/analytics/stamp_analytics.py

@@ -0,0 +1,330 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# pylint: disable=missing-docstring
+
+from __future__ import print_function
+
+import os
+import sys
+
+TOOLBOX_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+sys.path.append(os.path.join(TOOLBOX_DIR, 'internal'))
+
+# pylint: disable=import-error,wrong-import-position
+from data import ROSBag, What
+
+
+ANGLE_DEGREES = 'd'
+ANGLE_RADIANS = 'r'
+ANGLE_UNITS = (ANGLE_DEGREES, ANGLE_RADIANS)
+
+BIN_IMG_NAME = 'stamp_analytics_img.bin'
+BIN_IMU_NAME = 'stamp_analytics_imu.bin'
+
+RESULT_FIGURE = 'stamp_analytics.png'
+
+
+class BinDataset(object):
+
+  def __init__(self, path, dataset_creator):
+    self.path = path
+    self.dataset_creator = dataset_creator
+    self._digest()
+
+  def _digest(self):
+    bindir = os.path.splitext(self.path)[0]
+    binimg = os.path.join(bindir, BIN_IMG_NAME)
+    binimu = os.path.join(bindir, BIN_IMU_NAME)
+    if os.path.isfile(binimg) and os.path.isfile(binimu):
+      print('find binary files ...')
+      print('  binimg: {}'.format(binimg))
+      print('  binimu: {}'.format(binimu))
+      while True:
+        sys.stdout.write('Do you want to use it directly? [Y/n] ')
+        choice = raw_input().lower()
+        if choice == '' or choice == 'y':
+          self._binimg = binimg
+          self._binimu = binimu
+          self._has_img = True
+          self._has_imu = True
+          return
+        elif choice == 'n':
+          break
+        else:
+          print('Please respond with \'y\' or \'n\'.')
+    self._convert()
+
+  def _convert(self):
+    import numpy as np
+
+    dataset = self.dataset_creator(self.path)
+    bindir = os.path.splitext(self.path)[0]
+    if not os.path.exists(bindir):
+      os.makedirs(bindir)
+    binimg = os.path.join(bindir, BIN_IMG_NAME)
+    binimu = os.path.join(bindir, BIN_IMU_NAME)
+    print('save to binary files ...')
+    print('  binimg: {}'.format(binimg))
+    print('  binimu: {}'.format(binimu))
+
+    has_img = False
+    has_imu = False
+    with open(binimg, 'wb') as f_img, open(binimu, 'wb') as f_imu:
+      img_count = 0
+      imu_count = 0
+      for result in dataset.generate(What.img_left, What.imu):
+        if What.img_left in result:
+          img = result[What.img_left]
+          np.array([(
+              img.timestamp
+          )], dtype="f8").tofile(f_img)
+          img_count = img_count + 1
+          has_img = True
+        if What.imu in result:
+          imu = result[What.imu]
+          np.array([(
+              imu.timestamp,
+              imu.accel_x, imu.accel_y, imu.accel_z,
+              imu.gyro_x, imu.gyro_y, imu.gyro_z
+          )], dtype="f8, f8, f8, f8, f8, f8, f8").tofile(f_imu)
+          imu_count = imu_count + 1
+          has_imu = True
+        sys.stdout.write('\r  img: {}, imu: {}'.format(img_count, imu_count))
+      sys.stdout.write('\n')
+
+    # pylint: disable=attribute-defined-outside-init
+    self._binimg = binimg
+    self._binimu = binimu
+    self._has_img = has_img
+    self._has_imu = has_imu
+
+  def stamp_analytics(self, args):
+    outdir = args.outdir
+
+    import numpy as np
+    if self.has_img:
+      # pd.cut fails on readonly arrays
+      #   https://github.com/pandas-dev/pandas/issues/18773
+      # imgs = np.memmap(self._binimg, dtype=[
+      #     ('t', 'f8')
+      # ], mode='r')
+      imgs = np.fromfile(self._binimg, dtype=[
+          ('t', 'f8')
+      ])
+    else:
+      sys.exit("Error: there are no imgs.")
+
+    if self.has_imu:
+      imus = np.memmap(self._binimu, dtype=[
+          ('t', 'f8'),
+          ('accel_x', 'f8'), ('accel_y', 'f8'), ('accel_z', 'f8'),
+          ('gyro_x', 'f8'), ('gyro_y', 'f8'), ('gyro_z', 'f8'),
+      ], mode='r')
+    else:
+      sys.exit("Error: there are no imus.")
+
+    period_img = 1. / args.rate_img
+    period_imu = 1. / args.rate_imu
+    print('\nrate (Hz)')
+    print('  img: {}, imu: {}'.format(args.rate_img, args.rate_imu))
+    print('sample period (s)')
+    print('  img: {}, imu: {}'.format(period_img, period_imu))
+
+    imgs_t_diff = np.diff(imgs['t'])
+    imus_t_diff = np.diff(imus['t'])
+
+    print('\ndiff count')
+    print('  imgs: {}, imus: {}'.format(imgs['t'].size, imus['t'].size))
+    print('  imgs_t_diff: {}, imus_t_diff: {}'
+          .format(imgs_t_diff.size, imus_t_diff.size))
+
+    print('\ndiff where (factor={})'.format(args.factor))
+
+    where = np.argwhere(imgs_t_diff > period_img * (1 + args.factor))
+    print('  imgs where diff > {}*{} ({})'.format(period_img,
+                                                  1 + args.factor, where.size))
+    for x in where:
+      print('  {:8d}: {:.16f}'.format(x[0], imgs_t_diff[x][0]))
+
+    where = np.argwhere(imgs_t_diff < period_img * (1 - args.factor))
+    print('  imgs where diff < {}*{} ({})'.format(period_img,
+                                                  1 - args.factor, where.size))
+    for x in where:
+      print('  {:8d}: {:.16f}'.format(x[0], imgs_t_diff[x][0]))
+
+    where = np.argwhere(imus_t_diff > period_imu * (1 + args.factor))
+    print('  imus where diff > {}*{} ({})'.format(period_imu,
+                                                  1 + args.factor, where.size))
+    for x in where:
+      print('  {:8d}: {:.16f}'.format(x[0], imus_t_diff[x][0]))
+
+    where = np.argwhere(imus_t_diff < period_imu * (1 - args.factor))
+    print('  imus where diff < {}*{} ({})'.format(period_imu,
+                                                  1 - args.factor, where.size))
+    for x in where:
+      print('  {:8d}: {:.16f}'.format(x[0], imus_t_diff[x][0]))
+
+    import pandas as pd
+    bins = imgs['t']
+    bins_n = imgs['t'].size
+    bins = pd.Series(data=bins).drop_duplicates(keep='first')
+    cats = pd.cut(imus['t'], bins)
+
+    print('\nimage timestamp duplicates: {}'.format(bins_n - bins.size))
+
+    self._plot(outdir, imgs_t_diff, imus_t_diff, cats.value_counts())
+
+  def _plot(self, outdir, imgs_t_diff, imus_t_diff, imgs_t_imus):
+    import matplotlib.pyplot as plt
+    import numpy as np
+
+    fig_1 = plt.figure(1, [16, 6])
+    fig_1.suptitle('Stamp Analytics')
+    fig_1.subplots_adjust(
+        left=0.1,
+        right=0.95,
+        top=0.85,
+        bottom=0.15,
+        wspace=0.4)
+
+    ax_imgs_t_diff = fig_1.add_subplot(131)
+    ax_imgs_t_diff.set_title('Image Timestamp Diff')
+    ax_imgs_t_diff.set_xlabel('diff index')
+    ax_imgs_t_diff.set_ylabel('diff (s)')
+    ax_imgs_t_diff.axis('auto')
+
+    ax_imus_t_diff = fig_1.add_subplot(132)
+    ax_imus_t_diff.set_title('Imu Timestamp Diff')
+    ax_imus_t_diff.set_xlabel('diff index')
+    ax_imus_t_diff.set_ylabel('diff (s)')
+    ax_imus_t_diff.axis('auto')
+
+    ax_imgs_t_imus = fig_1.add_subplot(133)
+    ax_imgs_t_imus.set_title('Imu Count Per Image Intervel')
+    ax_imgs_t_imus.set_xlabel('intervel index')
+    ax_imgs_t_imus.set_ylabel('imu count')
+    ax_imgs_t_imus.axis('auto')
+
+    ax_imgs_t_diff.set_xlim([0, imgs_t_diff.size])
+    ax_imgs_t_diff.plot(imgs_t_diff)
+
+    ax_imus_t_diff.set_xlim([0, imus_t_diff.size])
+    ax_imus_t_diff.plot(imus_t_diff)
+
+    # print(imgs_t_imus.values)
+    # imgs_t_imus.plot(kind='line', ax=ax_imgs_t_imus)
+    data = imgs_t_imus.values
+    ax_imgs_t_imus.set_xlim([0, data.size])
+    ax_imgs_t_imus.set_ylim([np.min(data) - 1, np.max(data) + 1])
+    ax_imgs_t_imus.plot(data)
+
+    if outdir:
+      figpath = os.path.join(outdir, RESULT_FIGURE)
+      print('\nsave figure to:\n  {}'.format(figpath))
+      if not os.path.exists(outdir):
+        os.makedirs(outdir)
+      fig_1.savefig(figpath, dpi=100)
+
+    plt.show()
+
+  @property
+  def has_img(self):
+    return self._has_img
+
+  @property
+  def has_imu(self):
+    return self._has_imu
+
+
+def _parse_args():
+  import argparse
+  parser = argparse.ArgumentParser(
+      prog=os.path.basename(__file__),
+      formatter_class=argparse.RawTextHelpFormatter,
+      description='usage examples:'
+      '\n  python %(prog)s -i DATASET')
+  parser.add_argument(
+      '-i',
+      '--input',
+      dest='input',
+      metavar='DATASET',
+      required=True,
+      help='the input dataset path')
+  parser.add_argument(
+      '-o',
+      '--outdir',
+      dest='outdir',
+      metavar='OUTDIR',
+      help='the output directory')
+  parser.add_argument(
+      '-c',
+      '--config',
+      dest='config',
+      metavar='CONFIG',
+      help='yaml config file about input dataset')
+  parser.add_argument(
+      '-f',
+      '--factor',
+      dest='factor',
+      metavar='FACTOR',
+      default=0.1,
+      type=float,
+      help='the wave factor (default: %(default)s)')
+  parser.add_argument(
+      '--rate-img',
+      dest='rate_img',
+      metavar='RATE',
+      default=25,
+      type=int,
+      help='the img rate (default: %(default)s)')
+  parser.add_argument(
+      '--rate-imu',
+      dest='rate_imu',
+      metavar='RATE',
+      default=500,
+      type=int,
+      help='the imu rate (default: %(default)s)')
+  return parser.parse_args()
+
+
+def _main():
+  args = _parse_args()
+
+  dataset_path = args.input
+  if not dataset_path or not os.path.exists(dataset_path):
+    sys.exit('Error: the dataset path not exists, %s' % dataset_path)
+  dataset_path = os.path.normpath(dataset_path)
+
+  outdir = args.outdir
+  if not args.outdir:
+    outdir = os.path.splitext(dataset_path)[0]
+  else:
+    outdir = os.path.abspath(outdir)
+  args.outdir = outdir
+
+  print('stamp analytics ...')
+  print('  input: %s' % dataset_path)
+  print('  outdir: %s' % outdir)
+
+  def dataset_creator(path):
+    print('open dataset ...')
+    if args.config:
+      import yaml
+      config = yaml.load(file(args.config, 'r'))
+      if config['dataset'] != 'rosbag':
+        sys.exit('Error: dataset model only support rosbag now')
+      dataset = ROSBag(path, **config['rosbag'])
+    else:
+      dataset = ROSBag(path,
+                       topic_img_left='/mynteye/left',
+                       topic_imu='/mynteye/imu')
+    return dataset
+
+  dataset = BinDataset(dataset_path, dataset_creator)
+  dataset.stamp_analytics(args)
+
+  print('stamp analytics done')
+
+
+if __name__ == '__main__':
+  _main()

tools/config/mynteye/mynteye_config.yaml

@@ -0,0 +1,9 @@
+# dataset model: rosbag, euroc
+dataset: "rosbag"
+
+# rosbag config
+rosbag:
+  topic_img_left: "/mynteye/left"
+  topic_img_right: "/mynteye/right"
+  topic_imu: "/mynteye/imu"
+  topic_temp: "/mynteye/temp"

tools/internal/data/__init__.py

@@ -0,0 +1,302 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# pylint: disable=missing-docstring
+
+from __future__ import print_function
+
+
+def isiter_not_str(obj):
+  return hasattr(obj, '__iter__') and not isinstance(obj, basestring)
+
+
+class What(object):
+  img_left = "img_left"
+  img_right = "img_right"
+  imu = "imu"
+  temp = "temp"
+
+
+class DataError(Exception):
+
+  def __init__(self, message):
+    super(DataError, self).__init__()
+    self.message = message
+
+
+class Data(object):
+
+  def __init__(self):
+    self._timestamp = 0
+
+  @property
+  def timestamp(self):
+    return self._timestamp
+
+  @timestamp.setter
+  def timestamp(self, value):
+    self._timestamp = value
+
+  def __str__(self):
+    return "timestamp: {:f}".format(self.timestamp)
+
+
+class Image(Data):
+
+  def __init__(self):
+    super(Image, self).__init__()
+    self._data = None
+
+  @property
+  def data(self):
+    return self._data
+
+  @data.setter
+  def data(self, data):
+    self._data = data
+
+  @property
+  def width(self):
+    return 0
+
+  @property
+  def height(self):
+    return 0
+
+
+class IMU(Data):
+
+  def __init__(self):
+    super(IMU, self).__init__()
+    self._accel_x = 0
+    self._accel_y = 0
+    self._accel_z = 0
+    self._gyro_x = 0
+    self._gyro_y = 0
+    self._gyro_z = 0
+
+  @property
+  def accel(self):
+    return self._accel_x, self._accel_y, self._accel_z
+
+  @property
+  def accel_x(self):
+    return self._accel_x
+
+  @accel_x.setter
+  def accel_x(self, accel_x):
+    self._accel_x = accel_x
+
+  @property
+  def accel_y(self):
+    return self._accel_y
+
+  @accel_y.setter
+  def accel_y(self, accel_y):
+    self._accel_y = accel_y
+
+  @property
+  def accel_z(self):
+    return self._accel_z
+
+  @accel_z.setter
+  def accel_z(self, accel_z):
+    self._accel_z = accel_z
+
+  @property
+  def gyro(self):
+    return self._gyro_x, self._gyro_y, self._gyro_z
+
+  @property
+  def gyro_x(self):
+    return self._gyro_x
+
+  @gyro_x.setter
+  def gyro_x(self, gyro_x):
+    self._gyro_x = gyro_x
+
+  @property
+  def gyro_y(self):
+    return self._gyro_y
+
+  @gyro_y.setter
+  def gyro_y(self, gyro_y):
+    self._gyro_y = gyro_y
+
+  @property
+  def gyro_z(self):
+    return self._gyro_z
+
+  @gyro_z.setter
+  def gyro_z(self, gyro_z):
+    self._gyro_z = gyro_z
+
+  def __str__(self):
+    return super(IMU, self).__str__() + \
+        "\naccel: {:f}, {:f}, {:f}".format(*self.accel) + \
+        "\ngyro: {:f}, {:f}, {:f}".format(*self.gyro)
+
+
+class Temp(Data):
+
+  def __init__(self):
+    super(Temp, self).__init__()
+    self._value = 0
+
+  @property
+  def value(self):
+    return self._value
+
+  @value.setter
+  def value(self, value):
+    self._value = value
+
+  def __str__(self):
+    return super(Temp, self).__str__() + \
+        "\ntemp: {:f}".format(self.value)
+
+
+class Dataset(object):
+
+  def __init__(self, path):
+    self.path = path
+
+  def generate(self, *what):  # pylint: disable=unused-argument
+    raise DataError('DataError: method not implemented')
+
+  def iterate(self, action, *what):
+    for result in self.generate(*what):
+      if isinstance(result, dict):  # dict > **kwds
+        action(**result)
+      elif isiter_not_str(result):  # iterable > *args
+        action(*result)
+      else:
+        action(result)
+
+  def collect(self, *what):
+    results = {}
+    for result in self.generate(*what):
+      for key in result.keys():
+        if key not in what:
+          continue
+        if key not in results:
+          results[key] = []
+        results[key].append(result[key])
+    return results
+
+  @property
+  def timebeg(self):
+    raise DataError('DataError: method not implemented')
+
+  @property
+  def timeend(self):
+    raise DataError('DataError: method not implemented')
+
+  @property
+  def duration(self):
+    raise DataError('DataError: method not implemented')
+
+
+class ROSBag(Dataset):
+
+  def __init__(self, path, **config):
+    super(ROSBag, self).__init__(path)
+    self.topic_img_left = config['topic_img_left'] \
+        if 'topic_img_left' in config else None
+    self.topic_img_right = config['topic_img_right'] \
+        if 'topic_img_right' in config else None
+    self.topic_imu = config['topic_imu'] \
+        if 'topic_imu' in config else None
+    self.topic_temp = config['topic_temp'] \
+        if 'topic_temp' in config else None
+
+    import yaml
+    from rosbag.bag import Bag
+    # pylint: disable=protected-access
+    self._info = yaml.load(Bag(self.path, 'r')._get_yaml_info())
+
+  def generate(self, *what):
+    import rosbag
+    hit_img_left = What.img_left in what
+    hit_img_right = What.img_right in what
+    hit_imu = What.imu in what
+    hit_temp = What.temp in what
+    try:
+      # pylint: disable=unused-variable
+      for topic, msg, t in rosbag.Bag(self.path).read_messages():
+        result = {}
+        stamp = msg.header.stamp.to_sec()
+        if hit_img_left and topic == self.topic_img_left:
+          img = Image()
+          img.timestamp = stamp
+          # pylint: disable=fixme
+          # TODO: data with cv_bridge
+          result[What.img_left] = img
+        elif hit_img_right and topic == self.topic_img_right:
+          img = Image()
+          img.timestamp = stamp
+          # TODO: data with cv_bridge
+          result[What.img_right] = img
+        elif hit_imu and topic == self.topic_imu:
+          imu = IMU()
+          imu.timestamp = stamp
+          imu.accel_x = msg.linear_acceleration.x
+          imu.accel_y = msg.linear_acceleration.y
+          imu.accel_z = msg.linear_acceleration.z
+          imu.gyro_x = msg.angular_velocity.x
+          imu.gyro_y = msg.angular_velocity.y
+          imu.gyro_z = msg.angular_velocity.z
+          result[What.imu] = imu
+        elif hit_temp and topic == self.topic_temp:
+          temp = Temp()
+          temp.timestamp = stamp
+          temp.value = msg.data
+          result[What.temp] = temp
+        else:
+          # raise DataError('DataError: not proper topics in the rosbag')
+          continue
+        yield result
+    finally:
+      pass
+
+  @property
+  def info(self):
+    return self._info
+
+  @property
+  def timebeg(self):
+    return self._info['start']
+
+  @property
+  def timeend(self):
+    return self._info['end']
+
+  @property
+  def duration(self):
+    return self._info['duration']
+
+
+if __name__ == '__main__':
+
+  class DataA(Dataset):
+
+    def generate(self, *what):
+      yield 'a'
+      yield 'b'
+
+  class DataB(Dataset):
+
+    def generate(self, *what):
+      yield 'a1', 'a2', 'a3'
+      yield 'b1', 'b2', 'b3'
+
+  print('DataA, generate')
+  for x in DataA('path').generate("what"):
+    print(x)
+  print('\nDataA, iterate')
+  DataA('path').iterate(print, "what")
+
+  print('\nDataB, generate')
+  for x in DataB('path').generate("what"):
+    print(', '.join(x))
+  print('\nDataB, iterate')
+  DataB('path').iterate(lambda *x: print(', '.join(x)), "what")

tools/requirements.txt

@@ -0,0 +1,3 @@
+matplotlib>=1.5.1
+numpy>=1.11.0
+pandas>=0.22.0