tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add analytics tools

Browse Source
This commit is contained in:
John Zhao 2018-04-14 21:17:00 +08:00
parent 837fd697ae
commit cdfc2967a3
7 changed files with 1343 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored
View File

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

40
tools/README.md Normal file
View File

@ -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
```

653
tools/analytics/imu_analytics.py Normal file
View File

@ -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()

330
tools/analytics/stamp_analytics.py Normal file
View File

@ -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()

9
tools/config/mynteye/mynteye_config.yaml Normal file
View File

@ -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"

302
tools/internal/data/__init__.py Normal file
View File

@ -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")

3
tools/requirements.txt Normal file
View File

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