MYNT-EYE-S-SDK/tools/analytics/stamp_analytics.py
2018-08-14 10:17:33 +08:00

368 lines
11 KiB
Python

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2018 Slightech Co., Ltd. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# 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, MYNTEYE, 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.flag,
imu.accel_x, imu.accel_y, imu.accel_z,
imu.gyro_x, imu.gyro_y, imu.gyro_z
)], dtype="f8, i4, 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'), ('flag', 'i4'),
('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'])
accel = imus[imus['flag'] == 1]
accel_t_diff = np.diff(accel['t'])
gyro = imus[imus['flag'] == 2]
gyro_t_diff = np.diff(gyro['t'])
print('\ncount')
print(' imgs: {}, imus: {}, accel: {}, gyro: {}'.format(
imgs.size, imus.size, accel.size, gyro.size))
print('\ndiff count')
print(' imgs_t_diff: {}, accel_t_diff: {}, gyro_t_diff: {}'.format(
imgs_t_diff.size, accel_t_diff.size, gyro_t_diff.size))
print('\ndiff where (factor={})'.format(args.factor))
self._print_t_diff_where('imgs', imgs_t_diff, period_img, args.factor)
# self._print_t_diff_where('imus', imus_t_diff, period_imu, args.factor)
self._print_t_diff_where('accel', accel_t_diff, period_imu, args.factor)
self._print_t_diff_where('gyro', gyro_t_diff, period_imu, args.factor)
import pandas as pd
bins = imgs['t']
bins_n = imgs['t'].size
bins = pd.Series(data=bins).drop_duplicates(keep='first')
print('\nimage timestamp duplicates: {}'.format(bins_n - bins.size))
def _cut_by_imgs_t(imus_t):
cats = pd.cut(imus_t, bins)
return cats.value_counts()
self._plot(
outdir,
imgs_t_diff,
accel_t_diff,
_cut_by_imgs_t(
accel['t']),
gyro_t_diff,
_cut_by_imgs_t(
gyro['t']))
def _print_t_diff_where(self, name, t_diff, period, factor):
import numpy as np
where = np.argwhere(t_diff > period * (1 + factor))
print(' {} where diff > {}*{} ({})'.format(
name, period, 1 + factor, where.size))
for x in where:
print(' {:8d}: {:.16f}'.format(x[0], t_diff[x][0]))
where = np.argwhere(t_diff < period * (1 - factor))
print(' {} where diff < {}*{} ({})'.format(
name, period, 1 - factor, where.size))
for x in where:
print(' {:8d}: {:.16f}'.format(x[0], t_diff[x][0]))
def _plot(self, outdir, imgs_t_diff,
accel_t_diff, accel_counts, gyro_t_diff, gyro_counts):
import matplotlib.pyplot as plt
import numpy as np
fig_1 = plt.figure(1, [16, 12])
fig_1.suptitle('Stamp Analytics')
fig_1.subplots_adjust(
left=0.1,
right=0.95,
top=0.85,
bottom=0.15,
wspace=0.4,
hspace=0.4)
ax_imgs_t_diff = fig_1.add_subplot(231)
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_imgs_t_diff.set_xlim([0, imgs_t_diff.size])
ax_imgs_t_diff.plot(imgs_t_diff)
def _plot_imus(name, t_diff, counts, pos_offset=0):
ax_imus_t_diff = fig_1.add_subplot(232 + pos_offset)
ax_imus_t_diff.set_title('{} Timestamp Diff'.format(name))
ax_imus_t_diff.set_xlabel('diff index')
ax_imus_t_diff.set_ylabel('diff (s)')
ax_imus_t_diff.axis('auto')
ax_imus_t_diff.set_xlim([0, t_diff.size - 1])
ax_imus_t_diff.plot(t_diff)
ax_imus_counts = fig_1.add_subplot(233 + pos_offset)
ax_imus_counts.set_title('{} Count Per Image Intervel'.format(name))
ax_imus_counts.set_xlabel('intervel index')
ax_imus_counts.set_ylabel('imu count')
ax_imus_counts.axis('auto')
# print(counts.values)
# counts.plot(kind='line', ax=ax_imus_counts)
data = counts.values
ax_imus_counts.set_xlim([0, data.size])
ax_imus_counts.set_ylim([np.min(data) - 1, np.max(data) + 1])
ax_imus_counts.plot(data)
_plot_imus('Accel', accel_t_diff, accel_counts)
_plot_imus('Gyro', gyro_t_diff, gyro_counts, 3)
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=60,
type=int,
help='the img rate (default: %(default)s)')
parser.add_argument(
'--rate-imu',
dest='rate_imu',
metavar='RATE',
default=200,
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'))
model = config['dataset']
if model == 'rosbag':
dataset = ROSBag(path, **config['rosbag'])
elif model == 'mynteye':
dataset = MYNTEYE(path)
else:
sys.exit('Error: dataset model not supported {}'.format(model))
else:
dataset = ROSBag(path,
topic_img_left='/mynteye/left/image_raw',
topic_imu='/mynteye/imu/data_raw')
return dataset
dataset = BinDataset(dataset_path, dataset_creator)
dataset.stamp_analytics(args)
print('stamp analytics done')
if __name__ == '__main__':
_main()