From cdfc2967a315d1331f57c98998fa73bc8c7c92a7 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 14 Apr 2018 21:17:00 +0800 Subject: [PATCH] Add analytics tools --- .gitignore | 6 + tools/README.md | 40 ++ tools/analytics/imu_analytics.py | 653 +++++++++++++++++++++++ tools/analytics/stamp_analytics.py | 330 ++++++++++++ tools/config/mynteye/mynteye_config.yaml | 9 + tools/internal/data/__init__.py | 302 +++++++++++ tools/requirements.txt | 3 + 7 files changed, 1343 insertions(+) create mode 100644 tools/README.md create mode 100644 tools/analytics/imu_analytics.py create mode 100644 tools/analytics/stamp_analytics.py create mode 100644 tools/config/mynteye/mynteye_config.yaml create mode 100644 tools/internal/data/__init__.py create mode 100644 tools/requirements.txt diff --git a/.gitignore b/.gitignore index cedf852..2a3aec5 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,9 @@ _output/ /wrappers/ros/install /wrappers/ros/.catkin_workspace /wrappers/ros/src/CMakeLists.txt + +# tools + +*.pyc +/mynteye/ +/mynteye.bag \ No newline at end of file diff --git a/tools/README.md b/tools/README.md new file mode 100644 index 0000000..0eb6473 --- /dev/null +++ b/tools/README.md @@ -0,0 +1,40 @@ +# Samples for MYNT® EYE tools + +## Prerequisites + +Ubuntu 16.04, ROS Kinetic. + +```bash +cd /tools/ +sudo pip install -r requirements.txt +``` + +## Record data + +```bash +cd +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 +``` diff --git a/tools/analytics/imu_analytics.py b/tools/analytics/imu_analytics.py new file mode 100644 index 0000000..afdf446 --- /dev/null +++ b/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() diff --git a/tools/analytics/stamp_analytics.py b/tools/analytics/stamp_analytics.py new file mode 100644 index 0000000..b0eb0ba --- /dev/null +++ b/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() diff --git a/tools/config/mynteye/mynteye_config.yaml b/tools/config/mynteye/mynteye_config.yaml new file mode 100644 index 0000000..d2e0dc3 --- /dev/null +++ b/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" diff --git a/tools/internal/data/__init__.py b/tools/internal/data/__init__.py new file mode 100644 index 0000000..c183dae --- /dev/null +++ b/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") diff --git a/tools/requirements.txt b/tools/requirements.txt new file mode 100644 index 0000000..98c1a38 --- /dev/null +++ b/tools/requirements.txt @@ -0,0 +1,3 @@ +matplotlib>=1.5.1 +numpy>=1.11.0 +pandas>=0.22.0