Merge branch 'release/v2.4.2' into develop
This commit is contained in:
TinyO
commit
d76903f1cd
19
Makefile
19
Makefile
|
|
@ -43,7 +43,6 @@ help:
|
|||
@echo " make install install project"
|
||||
@echo " make test build test and run"
|
||||
@echo " make samples build samples"
|
||||
@echo " make tools build tools"
|
||||
@echo " make pkg package sdk"
|
||||
@echo " make ros build ros wrapper"
|
||||
@echo " make py build python wrapper"
|
||||
|
|
@ -51,7 +50,7 @@ help:
|
|||
|
||||
.PHONY: help
|
||||
|
||||
all: init samples tools ros
|
||||
all: init samples ros
|
||||
|
||||
.PHONY: all
|
||||
|
||||
|
|
@ -159,20 +158,6 @@ samples: install
|
|||
|
||||
.PHONY: samples
|
||||
|
||||
# tools
|
||||
|
||||
tools: install
|
||||
@$(call echo,Make $@)
|
||||
ifeq ($(HOST_OS),Mac)
|
||||
$(error "Can't make tools on $(HOST_OS)")
|
||||
else
|
||||
@$(call cmake_build,./tools/_build)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
.PHONY: tools
|
||||
|
||||
# pkg
|
||||
|
||||
pkg: clean
|
||||
|
|
@ -262,8 +247,6 @@ clean:
|
|||
@$(call rm,./_install/)
|
||||
@$(call rm,./samples/_build/)
|
||||
@$(call rm,./samples/_output/)
|
||||
@$(call rm,./tools/_build/)
|
||||
@$(call rm,./tools/_output/)
|
||||
@$(call rm,./test/_build/)
|
||||
@$(call rm,./test/_output/)
|
||||
@$(MAKE) cleanlog
|
||||
|
|
|
|||
|
|
@ -1,72 +0,0 @@
|
|||
# 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.
|
||||
|
||||
cmake_minimum_required(VERSION 3.0)
|
||||
|
||||
project(mynteye_tools VERSION 2.0.0 LANGUAGES C CXX)
|
||||
|
||||
get_filename_component(PRO_DIR ${PROJECT_SOURCE_DIR} DIRECTORY)
|
||||
|
||||
include(${PRO_DIR}/cmake/Common.cmake)
|
||||
include(${PRO_DIR}/cmake/Utils.cmake)
|
||||
|
||||
# flags
|
||||
|
||||
if(OS_WIN)
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
|
||||
else()
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
|
||||
endif()
|
||||
|
||||
include(${PRO_DIR}/cmake/DetectCXX11.cmake)
|
||||
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
|
||||
|
||||
string(STRIP "${CMAKE_C_FLAGS}" CMAKE_C_FLAGS)
|
||||
string(STRIP "${CMAKE_CXX_FLAGS}" CMAKE_CXX_FLAGS)
|
||||
message(STATUS "C_FLAGS: ${CMAKE_C_FLAGS}")
|
||||
message(STATUS "CXX_FLAGS: ${CMAKE_CXX_FLAGS}")
|
||||
|
||||
# packages
|
||||
|
||||
LIST(APPEND CMAKE_PREFIX_PATH ${PRO_DIR}/_install/lib/cmake)
|
||||
find_package(mynteye REQUIRED)
|
||||
message(STATUS "Found mynteye: ${mynteye_VERSION}")
|
||||
|
||||
include(${PRO_DIR}/cmake/DetectOpenCV.cmake)
|
||||
|
||||
if(mynteye_WITH_GLOG)
|
||||
include(${PRO_DIR}/cmake/DetectGLog.cmake)
|
||||
endif()
|
||||
|
||||
#LIST(APPEND CMAKE_MODULE_PATH ${PRO_DIR}/cmake)
|
||||
|
||||
# targets
|
||||
|
||||
set(OUT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/_output")
|
||||
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
||||
# dataset
|
||||
|
||||
add_subdirectory(dataset)
|
||||
|
||||
# writer
|
||||
|
||||
add_subdirectory(writer)
|
||||
135
tools/README.md
135
tools/README.md
|
|
@ -1,135 +0,0 @@
|
|||
# Tools for MYNT® EYE cameras
|
||||
|
||||
## Prerequisites
|
||||
|
||||
[OpenCV](https://opencv.org/),
|
||||
|
||||
```bash
|
||||
# Linux, macOS
|
||||
export OpenCV_DIR=~/opencv
|
||||
|
||||
# Windows
|
||||
set OpenCV_DIR=C:\opencv
|
||||
```
|
||||
|
||||
Python packages,
|
||||
|
||||
```bash
|
||||
cd <sdk>/tools/
|
||||
sudo pip install -r requirements.txt
|
||||
```
|
||||
|
||||
[ROS](http://www.ros.org/) if using rosbag.
|
||||
|
||||
## Build
|
||||
|
||||
```bash
|
||||
cd <sdk>
|
||||
make tools
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Record data (mynteye dataset)
|
||||
|
||||
```bash
|
||||
./tools/_output/bin/dataset/record
|
||||
|
||||
# Windows
|
||||
.\tools\_output\bin\dataset\record.bat
|
||||
```
|
||||
|
||||
## Analytics data (mynteye dataset)
|
||||
|
||||
### imu_analytics.py
|
||||
|
||||
```bash
|
||||
python tools/analytics/imu_analytics.py -i dataset -c tools/config/mynteye/mynteye_config.yaml \
|
||||
-al=-1.2,1.2 -gl= -gdu=d -gsu=d -kl=
|
||||
```
|
||||
|
||||
### stamp_analytics.py
|
||||
|
||||
```bash
|
||||
python tools/analytics/stamp_analytics.py -i dataset -c tools/config/mynteye/mynteye_config.yaml
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Record data (rosbag)
|
||||
|
||||
```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/image_raw /mynteye/imu/data_raw /mynteye/temperature/data_raw
|
||||
```
|
||||
|
||||
## Analytics data (rosbag)
|
||||
|
||||
### 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
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Writer
|
||||
|
||||
### device_info_writer.cc
|
||||
|
||||
```bash
|
||||
./tools/_output/bin/writer/device_info_writer tools/writer/config/device.info
|
||||
|
||||
# Windows
|
||||
.\tools\_output\bin\writer\device_info_writer.bat tools\writer\config\device.info
|
||||
```
|
||||
|
||||
### img_params_writer.cc
|
||||
|
||||
```bash
|
||||
./tools/_output/bin/writer/img_params_writer tools/writer/config/img.params
|
||||
|
||||
# Windows
|
||||
.\tools\_output\bin\writer\img_params_writer.bat tools\writer\config\img.params
|
||||
```
|
||||
|
||||
### imu_params_writer.cc
|
||||
|
||||
```bash
|
||||
./tools/_output/bin/writer/imu_params_writer tools/writer/config/imu.params
|
||||
|
||||
# Windows
|
||||
.\tools\_output\bin\writer\imu_params_writer.bat tools\writer\config\imu.params
|
||||
```
|
||||
|
||||
### save_all_infos.cc
|
||||
|
||||
```bash
|
||||
./tools/_output/bin/writer/save_all_infos
|
||||
|
||||
# Windows
|
||||
.\tools\_output\bin\writer\save_all_infos.bat
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Checksum
|
||||
|
||||
```bash
|
||||
./tools/checksum/md5sum.sh <file or directory>
|
||||
```
|
||||
|
|
@ -1,671 +0,0 @@
|
|||
#!/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 DataError, Dataset, ROSBag, MYNTEYE, 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'))
|
||||
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_imu='/mynteye/imu/data_raw',
|
||||
topic_temp='/mynteye/temp/data_raw')
|
||||
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()
|
||||
|
|
@ -1,370 +0,0 @@
|
|||
#!/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'
|
||||
|
||||
|
||||
IMU_ALL = 0
|
||||
IMU_ACCEL = 1
|
||||
IMU_GYRO = 2
|
||||
|
||||
|
||||
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'] == IMU_ALL) | (imus['flag'] == IMU_ACCEL)]
|
||||
accel_t_diff = np.diff(accel['t'])
|
||||
gyro = imus[(imus['flag'] == IMU_ALL) | (imus['flag'] == IMU_GYRO)]
|
||||
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()
|
||||
|
|
@ -1,78 +0,0 @@
|
|||
#!/usr/bin/env bash
|
||||
# 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.
|
||||
|
||||
BASE_DIR=$(cd "$(dirname "$0")" && pwd)
|
||||
ROOT_DIR=$(realpath "$BASE_DIR/../..")
|
||||
SCRIPTS_DIR="$ROOT_DIR/scripts"
|
||||
|
||||
source "$SCRIPTS_DIR/common/echo.sh"
|
||||
source "$SCRIPTS_DIR/common/host.sh"
|
||||
|
||||
if [ "$HOST_OS" = "Linux" ]; then
|
||||
_md5sum() { md5sum "$1"; }
|
||||
elif [ "$HOST_OS" = "Mac" ]; then
|
||||
_md5sum() { md5 -q "$1"; }
|
||||
elif [ "$HOST_OS" = "Win" ]; then
|
||||
_md5sum() { certutil -hashfile "$1" MD5; }
|
||||
else # unexpected
|
||||
_echo_e "Unknown host os :("
|
||||
exit 1
|
||||
fi
|
||||
|
||||
PYTHON="python"
|
||||
if [ "$HOST_OS" = "Win" ]; then
|
||||
# default python on MSYS
|
||||
PYTHON="python2"
|
||||
fi
|
||||
|
||||
_get_size() {
|
||||
PYTHON_ARG="$1" $PYTHON - <<EOF
|
||||
import math
|
||||
from os.path import getsize
|
||||
|
||||
def convert_size(size_bytes):
|
||||
if size_bytes == 0:
|
||||
return "0B"
|
||||
size_name = ("B", "KB", "MB", "GB", "TB", "PB", "EB", "ZB", "YB")
|
||||
i = int(math.floor(math.log(size_bytes, 1024)))
|
||||
p = math.pow(1024, i)
|
||||
s = round(size_bytes / p, 2)
|
||||
return "%s %s" % (s, size_name[i])
|
||||
|
||||
import os
|
||||
print(convert_size(getsize(os.environ["PYTHON_ARG"])))
|
||||
EOF
|
||||
}
|
||||
|
||||
_print_info() {
|
||||
file="$1"
|
||||
_echo "File: $file"
|
||||
_echo "Size: `_get_size "$file"`"
|
||||
_echo "MD5: `_md5sum "$file"`"
|
||||
_echo
|
||||
}
|
||||
|
||||
if [ $# -eq 0 ]; then
|
||||
_echo_e "Usage: ./tools/checksum/md5sum.sh <file or directory>"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [ -d "$1" ]; then
|
||||
find "$1" -type f | while read -r f; do
|
||||
_print_info "$f"
|
||||
done
|
||||
else
|
||||
_print_info "$1"
|
||||
fi
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
# dataset model: rosbag, mynteye
|
||||
dataset: "mynteye"
|
||||
|
||||
# rosbag config
|
||||
rosbag:
|
||||
topic_img_left: "/mynteye/left/image_raw"
|
||||
topic_img_right: "/mynteye/right/image_raw"
|
||||
topic_imu: "/mynteye/imu/data_raw"
|
||||
topic_temp: "/mynteye/temp/data_raw"
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
# 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.
|
||||
|
||||
get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
|
||||
|
||||
set_outdir(
|
||||
"${OUT_DIR}/lib/${DIR_NAME}"
|
||||
"${OUT_DIR}/lib/${DIR_NAME}"
|
||||
"${OUT_DIR}/bin/${DIR_NAME}"
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${PRO_DIR}/src
|
||||
)
|
||||
|
||||
## record
|
||||
|
||||
# make_executable(record
|
||||
# SRCS record.cc dataset.cc
|
||||
# LINK_LIBS mynteye ${OpenCV_LIBS}
|
||||
# DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
#)
|
||||
|
||||
make_executable(record2
|
||||
SRCS record2.cc dataset.cc
|
||||
LINK_LIBS mynteye ${OpenCV_LIBS}
|
||||
DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
)
|
||||
|
|
@ -1,203 +0,0 @@
|
|||
// 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.
|
||||
#include "dataset/dataset.h"
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/util/files.h"
|
||||
|
||||
#define FULL_PRECISION \
|
||||
std::fixed << std::setprecision(std::numeric_limits<double>::max_digits10)
|
||||
|
||||
#define IMAGE_FILENAME_WIDTH 6
|
||||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
namespace tools {
|
||||
|
||||
Dataset::Dataset(std::string outdir) : outdir_(std::move(outdir)) {
|
||||
VLOG(2) << __func__;
|
||||
if (!files::mkdir(outdir_)) {
|
||||
LOG(FATAL) << "Create directory failed: " << outdir_;
|
||||
}
|
||||
}
|
||||
|
||||
Dataset::~Dataset() {
|
||||
VLOG(2) << __func__;
|
||||
for (auto &&it = stream_writers_.begin(); it != stream_writers_.end(); it++) {
|
||||
if (it->second) {
|
||||
it->second->ofs.flush();
|
||||
it->second->ofs.close();
|
||||
}
|
||||
}
|
||||
if (motion_writer_) {
|
||||
motion_writer_->ofs.flush();
|
||||
motion_writer_->ofs.close();
|
||||
}
|
||||
}
|
||||
|
||||
void Dataset::SaveStreamData(
|
||||
const Stream &stream, const device::StreamData &data) {
|
||||
auto &&writer = GetStreamWriter(stream);
|
||||
auto seq = stream_counts_[stream];
|
||||
writer->ofs << seq << ", " << data.img->frame_id << ", "
|
||||
<< data.img->timestamp << ", " << data.img->exposure_time
|
||||
<< std::endl;
|
||||
if (data.frame) {
|
||||
std::stringstream ss;
|
||||
ss << writer->outdir << MYNTEYE_OS_SEP << std::dec
|
||||
<< std::setw(IMAGE_FILENAME_WIDTH) << std::setfill('0') << seq << ".png";
|
||||
if (data.frame->format() == Format::GREY) {
|
||||
cv::Mat img(
|
||||
data.frame->height(), data.frame->width(), CV_8UC1,
|
||||
data.frame->data());
|
||||
cv::imwrite(ss.str(), img);
|
||||
} else if (data.frame->format() == Format::YUYV) {
|
||||
cv::Mat img(
|
||||
data.frame->height(), data.frame->width(), CV_8UC2,
|
||||
data.frame->data());
|
||||
cv::cvtColor(img, img, cv::COLOR_YUV2BGR_YUY2);
|
||||
cv::imwrite(ss.str(), img);
|
||||
} else if (data.frame->format() == Format::BGR888) {
|
||||
cv::Mat img(
|
||||
data.frame->height(), data.frame->width(), CV_8UC3,
|
||||
data.frame->data());
|
||||
cv::imwrite(ss.str(), img);
|
||||
} else {
|
||||
cv::Mat img(
|
||||
data.frame->height(), data.frame->width(), CV_8UC1,
|
||||
data.frame->data());
|
||||
cv::imwrite(ss.str(), img);
|
||||
}
|
||||
}
|
||||
++stream_counts_[stream];
|
||||
}
|
||||
|
||||
void Dataset::SaveMotionData(const device::MotionData &data) {
|
||||
auto &&writer = GetMotionWriter();
|
||||
// auto seq = data.imu->serial_number;
|
||||
auto seq = motion_count_;
|
||||
writer->ofs << seq << ", " << static_cast<int>(data.imu->flag) << ", "
|
||||
<< data.imu->timestamp << ", " << data.imu->accel[0] << ", "
|
||||
<< data.imu->accel[1] << ", " << data.imu->accel[2] << ", "
|
||||
<< data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", "
|
||||
<< data.imu->gyro[2] << ", " << data.imu->temperature
|
||||
<< std::endl;
|
||||
++motion_count_;
|
||||
/*
|
||||
if(motion_count_ != seq) {
|
||||
LOG(INFO) << "motion_count_ != seq !" << " motion_count_: " << motion_count_
|
||||
<< " seq: " << seq;
|
||||
motion_count_ = seq;
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
void Dataset::SaveStreamData(
|
||||
const Stream &stream, const api::StreamData &data) {
|
||||
auto &&writer = GetStreamWriter(stream);
|
||||
auto seq = stream_counts_[stream];
|
||||
writer->ofs << seq << ", " << data.img->frame_id << ", "
|
||||
<< data.img->timestamp << ", " << data.img->exposure_time
|
||||
<< std::endl;
|
||||
if (!data.frame.empty()) {
|
||||
std::stringstream ss;
|
||||
ss << writer->outdir << MYNTEYE_OS_SEP << std::dec
|
||||
<< std::setw(IMAGE_FILENAME_WIDTH) << std::setfill('0') << seq << ".png";
|
||||
cv::imwrite(ss.str(), data.frame);
|
||||
}
|
||||
++stream_counts_[stream];
|
||||
}
|
||||
|
||||
void Dataset::SaveMotionData(const api::MotionData &data) {
|
||||
auto &&writer = GetMotionWriter();
|
||||
// auto seq = data.imu->serial_number;
|
||||
auto seq = motion_count_;
|
||||
if (data.imu->flag == 1 || data.imu->flag == 2) {
|
||||
writer->ofs << seq << ", " << static_cast<int>(data.imu->flag) << ", "
|
||||
<< data.imu->timestamp << ", " << data.imu->accel[0] << ", "
|
||||
<< data.imu->accel[1] << ", " << data.imu->accel[2] << ", "
|
||||
<< data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", "
|
||||
<< data.imu->gyro[2] << ", " << data.imu->temperature
|
||||
<< std::endl;
|
||||
++motion_count_;
|
||||
}
|
||||
/*
|
||||
if(motion_count_ != seq) {
|
||||
LOG(INFO) << "motion_count_ != seq !" << " motion_count_: " << motion_count_
|
||||
<< " seq: " << seq;
|
||||
motion_count_ = seq;
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
Dataset::writer_t Dataset::GetStreamWriter(const Stream &stream) {
|
||||
try {
|
||||
return stream_writers_.at(stream);
|
||||
} catch (const std::out_of_range &e) {
|
||||
writer_t writer = std::make_shared<Writer>();
|
||||
switch (stream) {
|
||||
case Stream::LEFT: {
|
||||
writer->outdir = outdir_ + MYNTEYE_OS_SEP "left";
|
||||
} break;
|
||||
case Stream::RIGHT: {
|
||||
writer->outdir = outdir_ + MYNTEYE_OS_SEP "right";
|
||||
} break;
|
||||
default:
|
||||
LOG(FATAL) << "Unsupported stream: " << stream;
|
||||
}
|
||||
writer->outfile = writer->outdir + MYNTEYE_OS_SEP "stream.txt";
|
||||
|
||||
files::mkdir(writer->outdir);
|
||||
writer->ofs.open(writer->outfile, std::ofstream::out);
|
||||
writer->ofs << "seq, frame_id, timestamp, exposure_time" << std::endl;
|
||||
writer->ofs << FULL_PRECISION;
|
||||
|
||||
stream_writers_[stream] = writer;
|
||||
stream_counts_[stream] = 0;
|
||||
return writer;
|
||||
}
|
||||
}
|
||||
|
||||
Dataset::writer_t Dataset::GetMotionWriter() {
|
||||
if (motion_writer_ == nullptr) {
|
||||
writer_t writer = std::make_shared<Writer>();
|
||||
writer->outdir = outdir_;
|
||||
writer->outfile = writer->outdir + MYNTEYE_OS_SEP "motion.txt";
|
||||
|
||||
files::mkdir(writer->outdir);
|
||||
writer->ofs.open(writer->outfile, std::ofstream::out);
|
||||
writer->ofs << "seq, flag, timestamp, accel_x, accel_y, accel_z, "
|
||||
"gyro_x, gyro_y, gyro_z, temperature"
|
||||
<< std::endl;
|
||||
writer->ofs << FULL_PRECISION;
|
||||
|
||||
motion_writer_ = writer;
|
||||
motion_count_ = 0;
|
||||
accel_count_ = 0;
|
||||
gyro_count_ = 0;
|
||||
}
|
||||
return motion_writer_;
|
||||
}
|
||||
|
||||
} // namespace tools
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
// 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.
|
||||
#ifndef MYNTEYE_TOOLS_DATASET_H_ // NOLINT
|
||||
#define MYNTEYE_TOOLS_DATASET_H_
|
||||
#pragma once
|
||||
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "mynteye/mynteye.h"
|
||||
#include "mynteye/api/api.h"
|
||||
#include "mynteye/device/callbacks.h"
|
||||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
namespace tools {
|
||||
|
||||
class Dataset {
|
||||
public:
|
||||
struct Writer {
|
||||
std::ofstream ofs;
|
||||
std::string outdir;
|
||||
std::string outfile;
|
||||
};
|
||||
|
||||
using writer_t = std::shared_ptr<Writer>;
|
||||
|
||||
explicit Dataset(std::string outdir);
|
||||
~Dataset();
|
||||
|
||||
void SaveStreamData(const Stream &stream, const device::StreamData &data);
|
||||
void SaveMotionData(const device::MotionData &data);
|
||||
|
||||
void SaveStreamData(const Stream &stream, const api::StreamData &data);
|
||||
void SaveMotionData(const api::MotionData &data);
|
||||
|
||||
private:
|
||||
writer_t GetStreamWriter(const Stream &stream);
|
||||
writer_t GetMotionWriter();
|
||||
|
||||
std::string outdir_;
|
||||
|
||||
std::map<Stream, writer_t> stream_writers_;
|
||||
writer_t motion_writer_;
|
||||
|
||||
std::map<Stream, std::size_t> stream_counts_;
|
||||
std::size_t motion_count_;
|
||||
std::size_t accel_count_;
|
||||
std::size_t gyro_count_;
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
||||
#endif // MYNTEYE_TOOLS_DATASET_H_ NOLINT
|
||||
|
|
@ -1,134 +0,0 @@
|
|||
// 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.
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/api/api.h"
|
||||
#include "mynteye/util/times.h"
|
||||
|
||||
#include "dataset/dataset.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
auto &&api = API::Create(argc, argv);
|
||||
if (!api) return 1;
|
||||
|
||||
bool ok;
|
||||
auto &&request = api->SelectStreamRequest(&ok);
|
||||
if (!ok) return 1;
|
||||
api->ConfigStreamRequest(request);
|
||||
|
||||
api->LogOptionInfos();
|
||||
|
||||
// Enable this will cache the motion datas until you get them.
|
||||
api->EnableMotionDatas();
|
||||
api->Start(Source::ALL);
|
||||
|
||||
const char *outdir;
|
||||
if (argc >= 2) {
|
||||
outdir = argv[1];
|
||||
} else {
|
||||
outdir = "./dataset";
|
||||
}
|
||||
tools::Dataset dataset(outdir);
|
||||
|
||||
cv::namedWindow("frame");
|
||||
|
||||
std::size_t img_count = 0;
|
||||
std::size_t imu_count = 0;
|
||||
auto &&time_beg = times::now();
|
||||
while (true) {
|
||||
api->WaitForStreams();
|
||||
|
||||
auto &&left_datas = api->GetStreamDatas(Stream::LEFT);
|
||||
auto &&right_datas = api->GetStreamDatas(Stream::RIGHT);
|
||||
img_count += left_datas.size();
|
||||
|
||||
auto &&motion_datas = api->GetMotionDatas();
|
||||
imu_count += motion_datas.size();
|
||||
|
||||
auto &&left_frame = left_datas.back().frame;
|
||||
auto &&right_frame = right_datas.back().frame;
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
if (left_frame->format() == Format::GREY) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC1,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC1,
|
||||
right_frame->data());
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else if (left_frame->format() == Format::YUYV) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC2,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC2,
|
||||
right_frame->data());
|
||||
cv::cvtColor(left_img, left_img, cv::COLOR_YUV2BGR_YUY2);
|
||||
cv::cvtColor(right_img, right_img, cv::COLOR_YUV2BGR_YUY2);
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else if (left_frame->format() == Format::BGR888) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC3,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC3,
|
||||
right_frame->data());
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
cv::imshow("frame", img);
|
||||
if (img_count > 10 && imu_count > 50) { // save
|
||||
for (auto &&left : left_datas) {
|
||||
dataset.SaveStreamData(Stream::LEFT, left);
|
||||
}
|
||||
for (auto &&right : right_datas) {
|
||||
dataset.SaveStreamData(Stream::RIGHT, right);
|
||||
}
|
||||
|
||||
for (auto &&motion : motion_datas) {
|
||||
dataset.SaveMotionData(motion);
|
||||
}
|
||||
|
||||
std::cout << "\rSaved " << img_count << " imgs"
|
||||
<< ", " << imu_count << " imus" << std::flush;
|
||||
}
|
||||
|
||||
char key = static_cast<char>(cv::waitKey(1));
|
||||
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
|
||||
break;
|
||||
}
|
||||
}
|
||||
std::cout << " to " << outdir << std::endl;
|
||||
auto &&time_end = times::now();
|
||||
|
||||
api->Stop(Source::ALL);
|
||||
|
||||
float elapsed_ms =
|
||||
times::count<times::microseconds>(time_end - time_beg) * 0.001f;
|
||||
LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
|
||||
<< ", end: " << times::to_local_string(time_end)
|
||||
<< ", cost: " << elapsed_ms << "ms";
|
||||
LOG(INFO) << "Img count: " << img_count
|
||||
<< ", fps: " << (1000.f * img_count / elapsed_ms);
|
||||
LOG(INFO) << "Imu count: " << imu_count
|
||||
<< ", hz: " << (1000.f * imu_count / elapsed_ms);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,137 +0,0 @@
|
|||
// 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.
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/device/device.h"
|
||||
#include "mynteye/device/utils.h"
|
||||
#include "mynteye/util/times.h"
|
||||
|
||||
#include "dataset/dataset.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
glog_init _(argc, argv);
|
||||
|
||||
auto &&device = device::select();
|
||||
if (!device) return 1;
|
||||
|
||||
bool ok;
|
||||
auto &&request = device::select_request(device, &ok);
|
||||
if (!ok) return 1;
|
||||
device->ConfigStreamRequest(request);
|
||||
|
||||
device->LogOptionInfos();
|
||||
|
||||
// Enable this will cache the motion datas until you get them.
|
||||
device->EnableMotionDatas();
|
||||
device->Start(Source::ALL);
|
||||
|
||||
const char *outdir;
|
||||
if (argc >= 2) {
|
||||
outdir = argv[1];
|
||||
} else {
|
||||
outdir = "./dataset";
|
||||
}
|
||||
tools::Dataset dataset(outdir);
|
||||
|
||||
cv::namedWindow("frame");
|
||||
|
||||
std::size_t img_count = 0;
|
||||
std::size_t imu_count = 0;
|
||||
auto &&time_beg = times::now();
|
||||
while (true) {
|
||||
device->WaitForStreams();
|
||||
|
||||
auto &&left_datas = device->GetStreamDatas(Stream::LEFT);
|
||||
auto &&right_datas = device->GetStreamDatas(Stream::RIGHT);
|
||||
img_count += left_datas.size();
|
||||
|
||||
auto &&motion_datas = device->GetMotionDatas();
|
||||
imu_count += motion_datas.size();
|
||||
|
||||
auto &&left_frame = left_datas.back().frame;
|
||||
auto &&right_frame = right_datas.back().frame;
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
if (left_frame->format() == Format::GREY) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC1,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC1,
|
||||
right_frame->data());
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else if (left_frame->format() == Format::YUYV) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC2,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC2,
|
||||
right_frame->data());
|
||||
cv::cvtColor(left_img, left_img, cv::COLOR_YUV2BGR_YUY2);
|
||||
cv::cvtColor(right_img, right_img, cv::COLOR_YUV2BGR_YUY2);
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else if (left_frame->format() == Format::BGR888) {
|
||||
cv::Mat left_img(
|
||||
left_frame->height(), left_frame->width(), CV_8UC3,
|
||||
left_frame->data());
|
||||
cv::Mat right_img(
|
||||
right_frame->height(), right_frame->width(), CV_8UC3,
|
||||
right_frame->data());
|
||||
cv::hconcat(left_img, right_img, img);
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
cv::imshow("frame", img);
|
||||
if (img_count > 10 && imu_count > 50) { // save
|
||||
for (auto &&left : left_datas) {
|
||||
dataset.SaveStreamData(Stream::LEFT, left);
|
||||
}
|
||||
for (auto &&right : right_datas) {
|
||||
dataset.SaveStreamData(Stream::RIGHT, right);
|
||||
}
|
||||
|
||||
for (auto &&motion : motion_datas) {
|
||||
dataset.SaveMotionData(motion);
|
||||
}
|
||||
|
||||
std::cout << "\rSaved " << img_count << " imgs"
|
||||
<< ", " << imu_count << " imus" << std::flush;
|
||||
}
|
||||
|
||||
char key = static_cast<char>(cv::waitKey(1));
|
||||
if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q
|
||||
break;
|
||||
}
|
||||
}
|
||||
std::cout << " to " << outdir << std::endl;
|
||||
auto &&time_end = times::now();
|
||||
|
||||
device->Stop(Source::ALL);
|
||||
|
||||
float elapsed_ms =
|
||||
times::count<times::microseconds>(time_end - time_beg) * 0.001f;
|
||||
LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
|
||||
<< ", end: " << times::to_local_string(time_end)
|
||||
<< ", cost: " << elapsed_ms << "ms";
|
||||
LOG(INFO) << "Img count: " << img_count
|
||||
<< ", fps: " << (1000.f * img_count / elapsed_ms);
|
||||
LOG(INFO) << "Imu count: " << imu_count
|
||||
<< ", hz: " << (1000.f * imu_count / elapsed_ms);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,442 +0,0 @@
|
|||
#!/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
|
||||
|
||||
|
||||
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._flag = 0
|
||||
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 flag(self):
|
||||
return self._flag
|
||||
|
||||
@flag.setter
|
||||
def flag(self, flag):
|
||||
self._flag = flag
|
||||
|
||||
@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']
|
||||
|
||||
|
||||
class MYNTEYE(Dataset):
|
||||
# pylint: disable=no-member
|
||||
|
||||
def __init__(self, path):
|
||||
super(MYNTEYE, self).__init__(path)
|
||||
self._info = self._get_info()
|
||||
|
||||
def _get_info(self):
|
||||
import os
|
||||
import sys
|
||||
from os import path
|
||||
|
||||
info = type('', (), {})()
|
||||
|
||||
info.img_left_dir = path.join(self.path, 'left')
|
||||
info.img_left_txt = path.join(info.img_left_dir, 'stream.txt')
|
||||
info.has_img_left = path.isfile(info.img_left_txt)
|
||||
|
||||
info.img_right_dir = path.join(self.path, 'right')
|
||||
info.img_right_txt = path.join(info.img_right_dir, 'stream.txt')
|
||||
info.has_img_right = path.isfile(info.img_right_txt)
|
||||
|
||||
info.imu_txt = path.join(self.path, 'motion.txt')
|
||||
info.has_imu = path.isfile(info.imu_txt)
|
||||
|
||||
if info.has_img_left:
|
||||
info_txt = info.img_left_txt
|
||||
elif info.has_img_right:
|
||||
info_txt = info.img_right_txt
|
||||
elif info.has_imu:
|
||||
info_txt = info.img_left_txt
|
||||
else:
|
||||
sys.exit('Error: Dataset is empty or unexpected format')
|
||||
with open(info_txt, 'rb') as f:
|
||||
fields = [_.strip() for _ in f.readline().split(',')]
|
||||
|
||||
first = f.readline()
|
||||
f.seek(-2, os.SEEK_END)
|
||||
while f.read(1) != b'\n':
|
||||
f.seek(-2, os.SEEK_CUR)
|
||||
last = f.readline()
|
||||
|
||||
index = -1
|
||||
for i, field in enumerate(fields):
|
||||
if field == 'timestamp':
|
||||
index = i
|
||||
break
|
||||
if index == -1:
|
||||
sys.exit('Error: Dataset is unexpected format, timestamp not found')
|
||||
|
||||
# unit from 1us to 1s
|
||||
info.timebeg = float(first.split(',')[index].strip()) * 0.000001
|
||||
info.timeend = float(last.split(',')[index].strip()) * 0.000001
|
||||
# print('time: [{}, {}]'.format(info.timebeg, info.timeend))
|
||||
|
||||
return info
|
||||
|
||||
def generate(self, *what):
|
||||
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
|
||||
|
||||
def get_fields(f):
|
||||
fields = {}
|
||||
for i, field in enumerate(_.strip() for _ in f.readline().split(',')):
|
||||
fields[field] = i
|
||||
return fields
|
||||
|
||||
if hit_img_left and self._info.has_img_left:
|
||||
with open(self._info.img_left_txt) as f:
|
||||
fields = get_fields(f)
|
||||
for line in f:
|
||||
values = [_.strip() for _ in line.split(',')]
|
||||
img = Image()
|
||||
img.timestamp = float(values[fields['timestamp']]) * 0.000001
|
||||
yield {What.img_left: img}
|
||||
if hit_img_right and self._info.has_img_right:
|
||||
with open(self._info.img_right_txt) as f:
|
||||
fields = get_fields(f)
|
||||
for line in f:
|
||||
values = [_.strip() for _ in line.split(',')]
|
||||
img = Image()
|
||||
img.timestamp = float(values[fields['timestamp']]) * 0.000001
|
||||
yield {What.img_right: img}
|
||||
if (hit_imu or hit_temp) and self._info.has_imu:
|
||||
with open(self._info.imu_txt) as f:
|
||||
fields = get_fields(f)
|
||||
for line in f:
|
||||
values = [_.strip() for _ in line.split(',')]
|
||||
imu = IMU()
|
||||
imu.timestamp = float(values[fields['timestamp']]) * 0.000001
|
||||
imu.flag = values[fields['flag']]
|
||||
imu.accel_x = float(values[fields['accel_x']])
|
||||
imu.accel_y = float(values[fields['accel_y']])
|
||||
imu.accel_z = float(values[fields['accel_z']])
|
||||
imu.gyro_x = float(values[fields['gyro_x']])
|
||||
imu.gyro_y = float(values[fields['gyro_y']])
|
||||
imu.gyro_z = float(values[fields['gyro_z']])
|
||||
temp = Temp()
|
||||
temp.timestamp = imu.timestamp
|
||||
temp.value = float(values[fields['temperature']])
|
||||
yield {What.imu: imu, What.temp: temp}
|
||||
|
||||
@property
|
||||
def timebeg(self):
|
||||
return self._info.timebeg
|
||||
|
||||
@property
|
||||
def timeend(self):
|
||||
return self._info.timeend
|
||||
|
||||
@property
|
||||
def duration(self):
|
||||
return self.timeend - self.timebeg
|
||||
|
||||
|
||||
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")
|
||||
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 1c7828bedb786c9f4fcf6f31f619bfb71d7b1cd3
|
||||
|
|
@ -1,4 +0,0 @@
|
|||
matplotlib>=1.5.1
|
||||
numpy>=1.11.0
|
||||
pandas>=0.22.0
|
||||
PyYAML>=3.11
|
||||
|
|
@ -1,58 +0,0 @@
|
|||
# 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.
|
||||
|
||||
get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
|
||||
|
||||
set_outdir(
|
||||
"${OUT_DIR}/lib/${DIR_NAME}"
|
||||
"${OUT_DIR}/lib/${DIR_NAME}"
|
||||
"${OUT_DIR}/bin/${DIR_NAME}"
|
||||
)
|
||||
|
||||
## device_writer
|
||||
|
||||
add_library(device_writer STATIC device_writer.cc)
|
||||
target_link_libraries(device_writer mynteye ${OpenCV_LIBS})
|
||||
|
||||
## device_info_writer
|
||||
|
||||
make_executable(device_info_writer
|
||||
SRCS device_info_writer.cc
|
||||
LINK_LIBS device_writer
|
||||
DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
)
|
||||
|
||||
## img_params_writer
|
||||
|
||||
make_executable(img_params_writer
|
||||
SRCS img_params_writer.cc
|
||||
LINK_LIBS device_writer
|
||||
DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
)
|
||||
|
||||
## imu_params_writer
|
||||
|
||||
make_executable(imu_params_writer
|
||||
SRCS imu_params_writer.cc
|
||||
LINK_LIBS device_writer
|
||||
DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
)
|
||||
|
||||
## save_all_infos
|
||||
|
||||
make_executable(save_all_infos
|
||||
SRCS save_all_infos.cc
|
||||
LINK_LIBS device_writer
|
||||
DLL_SEARCH_PATHS ${PRO_DIR}/_install/bin ${OpenCV_LIB_SEARCH_PATH}
|
||||
)
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
M1: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 7.3638305001095546e+02, 0., 3.5691961817119693e+02, 0.,
|
||||
7.2350066150722432e+02, 2.1727271340923883e+02, 0., 0., 1. ]
|
||||
D1: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 14
|
||||
dt: d
|
||||
data: [ -5.4898645145016478e-01, 5.2837141203888638e-01, 0., 0., 0.,
|
||||
0., 0., 4.3563985766435476e-01, 0., 0., 0., 0., 0., 0. ]
|
||||
M2: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 7.3638305001095546e+02, 0., 4.5668367112303980e+02, 0.,
|
||||
7.2350066150722432e+02, 2.5070083335536796e+02, 0., 0., 1. ]
|
||||
D2: !!opencv-matrix
|
||||
rows: 1
|
||||
cols: 14
|
||||
dt: d
|
||||
data: [ -5.1012886039889305e-01, 3.8764476500996770e-01, 0., 0., 0.,
|
||||
0., 0., 2.5629798245273044e-01, 0., 0., 0., 0., 0., 0. ]
|
||||
R: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 9.9701893306553813e-01, -9.5378124886236681e-04,
|
||||
-7.7151392794850615e-02, 1.4493996762830500e-03,
|
||||
9.9997867219985104e-01, 6.3682325649414354e-03,
|
||||
7.7143673424555026e-02, -6.4610716411527686e-03,
|
||||
9.9699905125522237e-01 ]
|
||||
T: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 1
|
||||
dt: d
|
||||
data: [ -1.1888991734400047e+02, -4.5605803870530912e-02,
|
||||
-3.9531373691193386e+00 ]
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
in_left:
|
||||
width: 752
|
||||
height: 480
|
||||
fx: 3.6220059643202876e+02
|
||||
fy: 3.6350065250745848e+02
|
||||
cx: 4.0658699068023441e+02
|
||||
cy: 2.3435161110061483e+02
|
||||
model: 0
|
||||
coeffs: [ -2.5034765682756088e-01, 5.0579399202897619e-02,
|
||||
-7.0536676161976066e-04, -8.5255451307033846e-03, 0. ]
|
||||
in_right:
|
||||
width: 752
|
||||
height: 480
|
||||
fx: 3.6514014888558478e+02
|
||||
fy: 3.6513385298966961e+02
|
||||
cx: 3.8932395100630907e+02
|
||||
cy: 2.3495160212312547e+02
|
||||
model: 0
|
||||
coeffs: [ -3.0377346762098512e-01, 7.9929693673999838e-02,
|
||||
5.1547517530716883e-05, -6.7345903740579250e-04, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9867908939669447e-01, -6.3445566137485428e-03,
|
||||
5.0988459509619687e-02, 5.9890316389333252e-03,
|
||||
9.9995670037792639e-01, 7.1224201868366971e-03,
|
||||
-5.1031440326695092e-02, -6.8076406092671274e-03,
|
||||
9.9867384471984544e-01 ]
|
||||
translation: [ -1.2002489764113250e+02, -1.1782637409050747e+00,
|
||||
-5.2058205159996538e+00 ]
|
||||
|
|
@ -1,33 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.0"
|
||||
img_params_map:
|
||||
-
|
||||
in_left:
|
||||
width: 752
|
||||
height: 480
|
||||
fx: 3.6220059643202876e+02
|
||||
fy: 3.6350065250745848e+02
|
||||
cx: 4.0658699068023441e+02
|
||||
cy: 2.3435161110061483e+02
|
||||
model: 0
|
||||
coeffs: [ -2.5034765682756088e-01, 5.0579399202897619e-02,
|
||||
-7.0536676161976066e-04, -8.5255451307033846e-03, 0. ]
|
||||
in_right:
|
||||
width: 752
|
||||
height: 480
|
||||
fx: 3.6514014888558478e+02
|
||||
fy: 3.6513385298966961e+02
|
||||
cx: 3.8932395100630907e+02
|
||||
cy: 2.3495160212312547e+02
|
||||
model: 0
|
||||
coeffs: [ -3.0377346762098512e-01, 7.9929693673999838e-02,
|
||||
5.1547517530716883e-05, -6.7345903740579250e-04, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9867908939669447e-01, -6.3445566137485428e-03,
|
||||
5.0988459509619687e-02, 5.9890316389333252e-03,
|
||||
9.9995670037792639e-01, 7.1224201868366971e-03,
|
||||
-5.1031440326695092e-02, -6.8076406092671274e-03,
|
||||
9.9867384471984544e-01 ]
|
||||
translation: [ -1.2002489764113250e+02, -1.1782637409050747e+00,
|
||||
-5.2058205159996538e+00 ]
|
||||
|
|
@ -1,24 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
in_accel:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
|
||||
1.7452487504590969e-02 ]
|
||||
bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
|
||||
5.4490537096493644e-04 ]
|
||||
in_gyro:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
|
||||
1.1003229919806443e-03 ]
|
||||
bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
|
||||
1.4970418056326829e-05 ]
|
||||
ex_left_to_imu:
|
||||
rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
|
||||
-7.6356499999999999e-03, 9.9997908999999996e-01,
|
||||
-6.4656599999999998e-03, -9.5580000000000003e-05,
|
||||
4.6199999999999998e-05, -7.6361099999999998e-03,
|
||||
9.9997084000000003e-01 ]
|
||||
translation: [ 5.3364600000000003e-03, -4.3029220000000000e-02,
|
||||
2.3031240000000001e-02 ]
|
||||
|
|
@ -1,10 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
device_name: MYNT-EYE-S1030
|
||||
serial_number: "02811F3B00090807"
|
||||
firmware_version: "2.2"
|
||||
hardware_version: "2.0"
|
||||
spec_version: "1.0"
|
||||
lens_type: "0000"
|
||||
imu_type: "0000"
|
||||
nominal_baseline: 120
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
img_params:
|
||||
-
|
||||
model: 1
|
||||
width: 752
|
||||
height: 480
|
||||
in_left:
|
||||
# coeffs: k2,k3,k4,k5,mu,mv,u0,v0
|
||||
coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02,
|
||||
5.3814579203036512e-02, -3.6663721418756977e-02,
|
||||
3.6797709792391299e+02, 3.6808712539453859e+02,
|
||||
3.7414963027144353e+02, 2.3125000326472903e+02 ]
|
||||
in_right:
|
||||
# coeffs: k2,k3,k4,k5,mu,mv,u0,v0
|
||||
coeffs: [ -1.8964880945048872e-02, -2.6172447729615463e-02,
|
||||
4.0863326732708288e-02, -2.1522718578606163e-02,
|
||||
3.6807315003638877e+02, 3.6806204885853174e+02,
|
||||
3.8499216569965887e+02, 2.3183027347834383e+02 ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9867908939669447e-01, -6.3445566137485428e-03,
|
||||
5.0988459509619687e-02, 5.9890316389333252e-03,
|
||||
9.9995670037792639e-01, 7.1224201868366971e-03,
|
||||
-5.1031440326695092e-02, -6.8076406092671274e-03,
|
||||
9.9867384471984544e-01 ]
|
||||
translation: [ -1.2002489764113250e+02, -1.1782637409050747e+00,
|
||||
-5.2058205159996538e+00 ]
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
img_params:
|
||||
-
|
||||
model: 0
|
||||
width: 752
|
||||
height: 480
|
||||
in_left:
|
||||
# [fx 0 cx]
|
||||
# [ 0 fy cy]
|
||||
# [ 0 0 1]
|
||||
fx: 3.6220059643202876e+02
|
||||
fy: 3.6350065250745848e+02
|
||||
cx: 4.0658699068023441e+02
|
||||
cy: 2.3435161110061483e+02
|
||||
# coeffs: k1,k2,p1,p2,k3
|
||||
coeffs: [ -2.5034765682756088e-01, 5.0579399202897619e-02,
|
||||
-7.0536676161976066e-04, -8.5255451307033846e-03, 0. ]
|
||||
in_right:
|
||||
# [fx 0 cx]
|
||||
# [ 0 fy cy]
|
||||
# [ 0 0 1]
|
||||
fx: 3.6514014888558478e+02
|
||||
fy: 3.6513385298966961e+02
|
||||
cx: 3.8932395100630907e+02
|
||||
cy: 2.3495160212312547e+02
|
||||
# coeffs: k1,k2,p1,p2,k3
|
||||
coeffs: [ -3.0377346762098512e-01, 7.9929693673999838e-02,
|
||||
5.1547517530716883e-05, -6.7345903740579250e-04, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9867908939669447e-01, -6.3445566137485428e-03,
|
||||
5.0988459509619687e-02, 5.9890316389333252e-03,
|
||||
9.9995670037792639e-01, 7.1224201868366971e-03,
|
||||
-5.1031440326695092e-02, -6.8076406092671274e-03,
|
||||
9.9867384471984544e-01 ]
|
||||
translation: [ -1.2002489764113250e+02, -1.1782637409050747e+00,
|
||||
-5.2058205159996538e+00 ]
|
||||
|
|
@ -1,33 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
in_accel:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
|
||||
1.7452487504590969e-02 ]
|
||||
bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
|
||||
5.4490537096493644e-04 ]
|
||||
x: [ 0.0, 0.0 ]
|
||||
y: [ 0.0, 0.0 ]
|
||||
z: [ 0.0, 0.0 ]
|
||||
in_gyro:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
|
||||
1.1003229919806443e-03 ]
|
||||
bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
|
||||
1.4970418056326829e-05 ]
|
||||
x: [ 0.0, 0.0 ]
|
||||
y: [ 0.0, 0.0 ]
|
||||
z: [ 0.0, 0.0 ]
|
||||
ex_left_to_imu:
|
||||
rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
|
||||
-7.6356499999999999e-03, 9.9997908999999996e-01,
|
||||
-6.4656599999999998e-03, -9.5580000000000003e-05,
|
||||
4.6199999999999998e-05, -7.6361099999999998e-03,
|
||||
9.9997084000000003e-01 ]
|
||||
translation: [ 5.3364600000000003e-03, -4.3029220000000000e-02,
|
||||
2.3031240000000001e-02 ]
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.1"
|
||||
img_params_map:
|
||||
-
|
||||
in_left:
|
||||
width: 640
|
||||
height: 400
|
||||
fx: 1.9739641213416058e+02
|
||||
fy: 1.9772337597617189e+02
|
||||
cx: 3.2611983633916327e+02
|
||||
cy: 1.9986969132833946e+02
|
||||
model: 0
|
||||
coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02,
|
||||
2.4914898631983504e-03, -3.7752063658256863e-03, 0. ]
|
||||
in_right:
|
||||
width: 640
|
||||
height: 400
|
||||
fx: 2.0335498653655989e+02
|
||||
fy: 2.0453858622699008e+02
|
||||
cx: 3.1589962248180814e+02
|
||||
cy: 2.1871688038954812e+02
|
||||
model: 0
|
||||
coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02,
|
||||
3.9725942760981507e-03, -3.9689073214945591e-03, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
-
|
||||
in_left:
|
||||
width: 1280
|
||||
height: 800
|
||||
fx: 1.9739641213416058e+02
|
||||
fy: 1.9772337597617189e+02
|
||||
cx: 3.2611983633916327e+02
|
||||
cy: 1.9986969132833946e+02
|
||||
model: 0
|
||||
coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02,
|
||||
2.4914898631983504e-03, -3.7752063658256863e-03, 0. ]
|
||||
in_right:
|
||||
width: 1280
|
||||
height: 800
|
||||
fx: 2.0335498653655989e+02
|
||||
fy: 2.0453858622699008e+02
|
||||
cx: 3.1589962248180814e+02
|
||||
cy: 2.1871688038954812e+02
|
||||
model: 0
|
||||
coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02,
|
||||
3.9725942760981507e-03, -3.9689073214945591e-03, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
in_accel:
|
||||
scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 0., 0., 0. ]
|
||||
bias: [ 0., 0., 0. ]
|
||||
in_gyro:
|
||||
scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 0., 0., 0. ]
|
||||
bias: [ 0., 0., 0. ]
|
||||
ex_left_to_imu:
|
||||
rotation: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ]
|
||||
translation: [ 0., 0., 0. ]
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
device_name: MYNT-EYE-S210A
|
||||
serial_number: "07C40D1C0009071F"
|
||||
firmware_version: "1.1"
|
||||
hardware_version: "1.0"
|
||||
spec_version: "1.2"
|
||||
lens_type: "0001"
|
||||
imu_type: "0001"
|
||||
nominal_baseline: 80
|
||||
auxiliary_chip_version: "1.0"
|
||||
isp_version: "1.0"
|
||||
|
|
@ -1,51 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
img_params:
|
||||
# There are two params of resolution [640x400], [1280x800].
|
||||
# If you only set one of them, write will keep another params if it exists.
|
||||
-
|
||||
model: 1
|
||||
width: 640
|
||||
height: 400
|
||||
in_left:
|
||||
# coeffs: k2,k3,k4,k5,mu,mv,u0,v0
|
||||
coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02,
|
||||
5.3814579203036512e-02, -3.6663721418756977e-02,
|
||||
3.6797709792391299e+02, 3.6808712539453859e+02,
|
||||
3.7414963027144353e+02, 2.3125000326472903e+02 ]
|
||||
in_right:
|
||||
coeffs: [ -1.8964880945048872e-02, -2.6172447729615463e-02,
|
||||
4.0863326732708288e-02, -2.1522718578606163e-02,
|
||||
3.6807315003638877e+02, 3.6806204885853174e+02,
|
||||
3.8499216569965887e+02, 2.3183027347834383e+02 ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
-
|
||||
model: 1
|
||||
width: 1280
|
||||
height: 800
|
||||
in_left:
|
||||
coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02,
|
||||
5.3814579203036512e-02, -3.6663721418756977e-02,
|
||||
3.6797709792391299e+02, 3.6808712539453859e+02,
|
||||
3.7414963027144353e+02, 2.3125000326472903e+02 ]
|
||||
in_right:
|
||||
coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02,
|
||||
5.3814579203036512e-02, -3.6663721418756977e-02,
|
||||
3.6797709792391299e+02, 3.6808712539453859e+02,
|
||||
3.7414963027144353e+02, 2.3125000326472903e+02 ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
img_params:
|
||||
# There are two params of resolution [640x400], [1280x800].
|
||||
# If you only set one of them, write will keep another params if it exists.
|
||||
-
|
||||
model: 0
|
||||
width: 640
|
||||
height: 400
|
||||
in_left:
|
||||
# [fx 0 cx]
|
||||
# [ 0 fy cy]
|
||||
# [ 0 0 1]
|
||||
fx: 1.9739641213416058e+02
|
||||
fy: 1.9772337597617189e+02
|
||||
cx: 3.2611983633916327e+02
|
||||
cy: 1.9986969132833946e+02
|
||||
# coeffs: k1,k2,p1,p2,k3
|
||||
coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02,
|
||||
2.4914898631983504e-03, -3.7752063658256863e-03, 0. ]
|
||||
in_right:
|
||||
fx: 2.0335498653655989e+02
|
||||
fy: 2.0453858622699008e+02
|
||||
cx: 3.1589962248180814e+02
|
||||
cy: 2.1871688038954812e+02
|
||||
coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02,
|
||||
3.9725942760981507e-03, -3.9689073214945591e-03, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
-
|
||||
model: 0
|
||||
width: 1280
|
||||
height: 800
|
||||
in_left:
|
||||
fx: 1.9739641213416058e+02
|
||||
fy: 1.9772337597617189e+02
|
||||
cx: 3.2611983633916327e+02
|
||||
cy: 1.9986969132833946e+02
|
||||
coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02,
|
||||
2.4914898631983504e-03, -3.7752063658256863e-03, 0. ]
|
||||
in_right:
|
||||
fx: 2.0335498653655989e+02
|
||||
fy: 2.0453858622699008e+02
|
||||
cx: 3.1589962248180814e+02
|
||||
cy: 2.1871688038954812e+02
|
||||
coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02,
|
||||
3.9725942760981507e-03, -3.9689073214945591e-03, 0. ]
|
||||
ex_right_to_left:
|
||||
rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
|
||||
-4.3917309443490191e-03, 1.8166060642710027e-03,
|
||||
9.9968925981619028e-01, -2.4861290203142431e-02,
|
||||
4.4382582477776426e-03, 2.4853026274046636e-02,
|
||||
9.9968126367795229e-01 ]
|
||||
translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00,
|
||||
2.2588034344482368e+00 ]
|
||||
|
|
@ -1,33 +0,0 @@
|
|||
%YAML:1.0
|
||||
---
|
||||
version: "1.2"
|
||||
in_accel:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
|
||||
1.7452487504590969e-02 ]
|
||||
bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
|
||||
5.4490537096493644e-04 ]
|
||||
x: [ 0.0, 0.0 ]
|
||||
y: [ 0.0, 0.0 ]
|
||||
z: [ 0.0, 0.0 ]
|
||||
in_gyro:
|
||||
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
|
||||
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
|
||||
drift: [ 0., 0., 0. ]
|
||||
noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
|
||||
1.1003229919806443e-03 ]
|
||||
bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
|
||||
1.4970418056326829e-05 ]
|
||||
x: [ 0.0, 0.0 ]
|
||||
y: [ 0.0, 0.0 ]
|
||||
z: [ 0.0, 0.0 ]
|
||||
ex_left_to_imu:
|
||||
rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
|
||||
-7.6356499999999999e-03, 9.9997908999999996e-01,
|
||||
-6.4656599999999998e-03, -9.5580000000000003e-05,
|
||||
4.6199999999999998e-05, -7.6361099999999998e-03,
|
||||
9.9997084000000003e-01 ]
|
||||
translation: [ 5.3364600000000003e-03, -4.3029220000000000e-02,
|
||||
2.3031240000000001e-02 ]
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
// 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.
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/device/device.h"
|
||||
#include "mynteye/device/utils.h"
|
||||
|
||||
#include "writer/device_writer.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
glog_init _(argc, argv);
|
||||
|
||||
const char *filepath;
|
||||
if (argc >= 2) {
|
||||
filepath = argv[1];
|
||||
} else {
|
||||
LOG(ERROR) << "Usage: ./device_info_writer <filepath>";
|
||||
return 2;
|
||||
}
|
||||
|
||||
auto &&device = device::select();
|
||||
if (!device)
|
||||
return 1;
|
||||
|
||||
tools::DeviceWriter writer(device);
|
||||
writer.WriteDeviceInfo(filepath);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,590 +0,0 @@
|
|||
// 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.
|
||||
#include "writer/device_writer.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/util/files.h"
|
||||
|
||||
#define SAVE_LATEST_VERSION Version(1, 2)
|
||||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
namespace tools {
|
||||
|
||||
DeviceWriter::DeviceWriter(std::shared_ptr<Device> device) : device_(device) {
|
||||
VLOG(2) << __func__;
|
||||
}
|
||||
|
||||
DeviceWriter::~DeviceWriter() {
|
||||
VLOG(2) << __func__;
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) {
|
||||
// Update device info
|
||||
auto &&dev_info = device_->GetInfo();
|
||||
dev_info->lens_type = Type(info.lens_type);
|
||||
dev_info->imu_type = Type(info.imu_type);
|
||||
dev_info->nominal_baseline = info.nominal_baseline;
|
||||
|
||||
if (device_->SetFiles(dev_info.get(), nullptr, nullptr)) {
|
||||
LOG(INFO) << "Write device info success";
|
||||
LOG(INFO) << "Device info: {name: " << dev_info->name
|
||||
<< ", serial_number: " << dev_info->serial_number
|
||||
<< ", firmware_version: "
|
||||
<< dev_info->firmware_version.to_string()
|
||||
<< ", hardware_version: "
|
||||
<< dev_info->hardware_version.to_string()
|
||||
<< ", spec_version: " << dev_info->spec_version.to_string()
|
||||
<< ", lens_type: " << dev_info->lens_type.to_string()
|
||||
<< ", imu_type: " << dev_info->imu_type.to_string()
|
||||
<< ", nominal_baseline: " << dev_info->nominal_baseline
|
||||
<< ", auxiliary_chip_version: "
|
||||
<< dev_info->auxiliary_chip_version.to_string()
|
||||
<< ", isp_version: "
|
||||
<< dev_info->isp_version.to_string()<< "}";
|
||||
return true;
|
||||
} else {
|
||||
LOG(ERROR) << "Write device info failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) {
|
||||
return WriteDeviceInfo(LoadDeviceInfo(filepath));
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteImgParams(const img_params_map_t &img_params_map) {
|
||||
img_params_map_t *img_params_new =
|
||||
const_cast<img_params_map_t *>(&img_params_map);
|
||||
// Update image params with raw
|
||||
auto &&img_params_raw = device_->GetImgParams();
|
||||
for (auto entry_raw : img_params_raw) {
|
||||
// Add raw params if not load this resolution
|
||||
if (img_params_new->find(entry_raw.first) == img_params_new->end()) {
|
||||
(*img_params_new)[entry_raw.first] = entry_raw.second;
|
||||
}
|
||||
}
|
||||
|
||||
if (device_->SetFiles(nullptr, img_params_new, nullptr)) {
|
||||
LOG(INFO) << "Write img params success";
|
||||
for (auto it = img_params_new->begin(); it != img_params_new->end(); it++) {
|
||||
LOG(INFO) << "Resolution: {width: " << (*it).first.width
|
||||
<< ", height: " << (*it).first.height << "}";
|
||||
LOG(INFO) << "Intrinsics left: {" << *(*it).second.in_left << "}";
|
||||
LOG(INFO) << "Intrinsics right: {" << *(*it).second.in_right << "}";
|
||||
LOG(INFO) << "Extrinsics right to left: {"
|
||||
<< (*it).second.ex_right_to_left << "}";
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
LOG(ERROR) << "Write img params failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteImgParams(const std::string &filepath) {
|
||||
return WriteImgParams(LoadImgParams(filepath));
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteImuParams(const imu_params_t ¶ms) {
|
||||
if (device_->SetFiles(
|
||||
nullptr, nullptr, const_cast<imu_params_t *>(¶ms))) {
|
||||
LOG(INFO) << "Write imu params success";
|
||||
LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";
|
||||
LOG(INFO) << "Imu intrinsics gyro: {" << params.in_gyro << "}";
|
||||
LOG(INFO) << "Imu extrinsics left to imu: {" << params.ex_left_to_imu
|
||||
<< "}";
|
||||
return true;
|
||||
} else {
|
||||
LOG(ERROR) << "Write imu params failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool DeviceWriter::WriteImuParams(const std::string &filepath) {
|
||||
return WriteImuParams(LoadImuParams(filepath));
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
cv::FileStorage &operator<<(cv::FileStorage &fs, const IntrinsicsPinhole &in) {
|
||||
fs << "{"
|
||||
<< "fx" << in.fx << "fy" << in.fy
|
||||
<< "cx" << in.cx << "cy" << in.cy
|
||||
<< "coeffs" << std::vector<double>(in.coeffs, in.coeffs + 5) << "}";
|
||||
return fs;
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(cv::FileStorage &fs,
|
||||
const IntrinsicsEquidistant &in) {
|
||||
fs << "{"
|
||||
<< "coeffs" << std::vector<double>(in.coeffs, in.coeffs + 8) << "}";
|
||||
return fs;
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(cv::FileStorage &fs,
|
||||
const std::shared_ptr<IntrinsicsBase> &in) {
|
||||
switch (in->calib_model()) {
|
||||
case CalibrationModel::PINHOLE:
|
||||
return fs << *std::dynamic_pointer_cast<IntrinsicsPinhole>(in);
|
||||
case CalibrationModel::KANNALA_BRANDT:
|
||||
return fs << *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in);
|
||||
default:
|
||||
LOG(FATAL) << "Unknown calib model: " << in->calib_model();
|
||||
return fs;
|
||||
}
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) {
|
||||
std::vector<double> scales;
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
scales.push_back(in.scale[i][j]);
|
||||
}
|
||||
}
|
||||
std::vector<double> assembly;
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
assembly.push_back(in.assembly[i][j]);
|
||||
}
|
||||
}
|
||||
fs << "{"
|
||||
<< "scale" << scales << "assembly" << assembly << "drift"
|
||||
<< std::vector<double>(in.drift, in.drift + 3) << "noise"
|
||||
<< std::vector<double>(in.noise, in.noise + 3) << "bias"
|
||||
<< std::vector<double>(in.bias, in.bias + 3) << "x"
|
||||
<< std::vector<double>(in.x, in.x + 2) << "y"
|
||||
<< std::vector<double>(in.y, in.y + 2) << "z"
|
||||
<< std::vector<double>(in.z, in.z + 2) << "}";
|
||||
return fs;
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) {
|
||||
std::vector<double> rotations;
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
rotations.push_back(ex.rotation[i][j]);
|
||||
}
|
||||
}
|
||||
fs << "{"
|
||||
<< "rotation" << rotations << "translation"
|
||||
<< std::vector<double>(ex.translation, ex.translation + 3) << "}";
|
||||
return fs;
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(
|
||||
cv::FileStorage &fs, const device::img_params_t ¶ms) {
|
||||
fs << "{"
|
||||
<< "model" << static_cast<std::uint8_t>(params.in_left->calib_model())
|
||||
<< "width" << params.in_left->width
|
||||
<< "height" << params.in_left->height
|
||||
<< "in_left" << params.in_left
|
||||
<< "in_right" << params.in_right
|
||||
<< "ex_right_to_left" << params.ex_right_to_left << "}";
|
||||
return fs;
|
||||
}
|
||||
|
||||
cv::FileStorage &operator<<(
|
||||
cv::FileStorage &fs, const DeviceWriter::img_params_map_t &img_params_map) {
|
||||
fs << "[";
|
||||
std::map<Resolution, device::img_params_t>::const_iterator it;
|
||||
for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
|
||||
fs << (*it).second;
|
||||
}
|
||||
fs << "]";
|
||||
return fs;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool DeviceWriter::SaveDeviceInfo(
|
||||
const dev_info_t &info, const std::string &filepath) {
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::WRITE);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(ERROR) << "Failed to save file: " << filepath;
|
||||
return false;
|
||||
}
|
||||
fs << "device_name" << info.name;
|
||||
fs << "serial_number" << info.serial_number;
|
||||
fs << "firmware_version" << info.firmware_version.to_string();
|
||||
fs << "hardware_version" << info.hardware_version.to_string();
|
||||
fs << "spec_version" << info.spec_version.to_string();
|
||||
fs << "lens_type" << info.lens_type.to_string();
|
||||
fs << "imu_type" << info.imu_type.to_string();
|
||||
fs << "nominal_baseline" << info.nominal_baseline;
|
||||
fs << "auxiliary_chip_version" << info.auxiliary_chip_version.to_string();
|
||||
fs << "isp_version" << info.isp_version.to_string();
|
||||
// save other infos according to spec_version
|
||||
fs.release();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DeviceWriter::SaveImgParams(
|
||||
const img_params_map_t &img_params_map,
|
||||
const std::string &filepath) {
|
||||
if (img_params_map.empty()) {
|
||||
return false;
|
||||
}
|
||||
std::string version = img_params_map.begin()->second.version;
|
||||
if (Version(version) > SAVE_LATEST_VERSION) {
|
||||
LOG(ERROR) << "Failed to save img params of version " << version
|
||||
<< ", please use latest SDK.";
|
||||
return false;
|
||||
}
|
||||
|
||||
// always save img params with latest version format
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::WRITE);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(ERROR) << "Failed to save file: " << filepath;
|
||||
return false;
|
||||
}
|
||||
fs << "version" << SAVE_LATEST_VERSION.to_string()
|
||||
<< "img_params" << img_params_map;
|
||||
fs.release();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DeviceWriter::SaveImuParams(
|
||||
const imu_params_t ¶ms, const std::string &filepath) {
|
||||
if (!params.ok) return false;
|
||||
std::string version = params.version;
|
||||
if (Version(version) > SAVE_LATEST_VERSION) {
|
||||
LOG(ERROR) << "Failed to save imu params of version " << version
|
||||
<< ", please use latest SDK.";
|
||||
return false;
|
||||
}
|
||||
|
||||
// always save imu params with latest version format
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::WRITE);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(ERROR) << "Failed to save file: " << filepath;
|
||||
return false;
|
||||
}
|
||||
fs << "version" << SAVE_LATEST_VERSION.to_string()
|
||||
<< "in_accel" << params.in_accel
|
||||
<< "in_gyro" << params.in_gyro
|
||||
<< "ex_left_to_imu" << params.ex_left_to_imu;
|
||||
fs.release();
|
||||
return true;
|
||||
}
|
||||
|
||||
void DeviceWriter::SaveAllInfos(const std::string &dir) {
|
||||
if (!files::mkdir(dir)) {
|
||||
LOG(FATAL) << "Create directory failed: " << dir;
|
||||
}
|
||||
SaveDeviceInfo(*device_->GetInfo(), dir + MYNTEYE_OS_SEP "device.info");
|
||||
SaveImgParams(device_->GetImgParams(), dir + MYNTEYE_OS_SEP "img.params");
|
||||
SaveImuParams(device_->GetImuParams(), dir + MYNTEYE_OS_SEP "imu.params");
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
// old
|
||||
void to_intrinsics(
|
||||
const std::uint16_t &width, const std::uint16_t &height,
|
||||
const std::uint8_t &model, const cv::Mat &M, const cv::Mat &D,
|
||||
IntrinsicsPinhole *in) {
|
||||
in->width = width;
|
||||
in->height = height;
|
||||
/*
|
||||
fx, 0, cx,
|
||||
0, fy, cy,
|
||||
0, 0, 1
|
||||
*/
|
||||
in->fx = M.at<double>(0, 0);
|
||||
in->fy = M.at<double>(1, 1);
|
||||
in->cx = M.at<double>(0, 2);
|
||||
in->cy = M.at<double>(1, 2);
|
||||
/* k1, k2, p1, p2, k3 */
|
||||
in->model = model;
|
||||
LOG_IF(FATAL, D.cols < 5) << "Distortion coefficients must >= 5 columns";
|
||||
for (std::size_t i = 0; i < 5; i++) {
|
||||
in->coeffs[i] = D.at<double>(i);
|
||||
}
|
||||
}
|
||||
|
||||
// old
|
||||
void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) {
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
ex->rotation[i][j] = R.at<double>(i, j);
|
||||
}
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
ex->translation[i] = T.at<double>(i);
|
||||
}
|
||||
}
|
||||
// old
|
||||
void operator>>(const cv::FileNode &n, IntrinsicsPinhole &in) {
|
||||
n["width"] >> in.width;
|
||||
n["height"] >> in.height;
|
||||
n["fx"] >> in.fx;
|
||||
n["fy"] >> in.fy;
|
||||
n["cx"] >> in.cx;
|
||||
n["cy"] >> in.cy;
|
||||
n["model"] >> in.model;
|
||||
for (std::size_t i = 0; i < 5; i++) {
|
||||
in.coeffs[i] = n["coeffs"][i];
|
||||
}
|
||||
}
|
||||
|
||||
void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
in.scale[i][j] = n["scale"][3 * i + j];
|
||||
}
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
in.assembly[i][j] = n["assembly"][3 * i + j];
|
||||
}
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
in.drift[i] = n["drift"][i];
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
in.noise[i] = n["noise"][i];
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
in.bias[i] = n["bias"][i];
|
||||
}
|
||||
for (std::size_t i = 0; i < 2; i++) {
|
||||
in.x[i] = n["x"][i];
|
||||
}
|
||||
for (std::size_t i = 0; i < 2; i++) {
|
||||
in.y[i] = n["y"][i];
|
||||
}
|
||||
for (std::size_t i = 0; i < 2; i++) {
|
||||
in.z[i] = n["z"][i];
|
||||
}
|
||||
}
|
||||
|
||||
void operator>>(const cv::FileNode &n, Extrinsics &ex) {
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
for (std::size_t j = 0; j < 3; j++) {
|
||||
ex.rotation[i][j] = n["rotation"][3 * i + j];
|
||||
}
|
||||
}
|
||||
for (std::size_t i = 0; i < 3; i++) {
|
||||
ex.translation[i] = n["translation"][i];
|
||||
}
|
||||
}
|
||||
|
||||
// old
|
||||
void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t ¶ms) {
|
||||
auto in_left = std::make_shared<IntrinsicsPinhole>();
|
||||
auto in_right = std::make_shared<IntrinsicsPinhole>();
|
||||
params.in_left = in_left;
|
||||
params.in_right = in_right;
|
||||
n["in_left"] >> *in_left;
|
||||
n["in_right"] >> *in_right;
|
||||
n["ex_right_to_left"] >> params.ex_right_to_left;
|
||||
}
|
||||
|
||||
std::shared_ptr<IntrinsicsPinhole> to_intrinsics_pinhole(
|
||||
const cv::FileNode &n, const std::uint8_t &model,
|
||||
const std::uint16_t &width, const std::uint16_t &height) {
|
||||
auto in = std::make_shared<IntrinsicsPinhole>();
|
||||
in->width = width;
|
||||
in->height = height;
|
||||
in->model = model;
|
||||
n["fx"] >> in->fx;
|
||||
n["fy"] >> in->fy;
|
||||
n["cx"] >> in->cx;
|
||||
n["cy"] >> in->cy;
|
||||
for (std::size_t i = 0; i < 5; i++) {
|
||||
in->coeffs[i] = n["coeffs"][i];
|
||||
}
|
||||
return in;
|
||||
}
|
||||
|
||||
std::shared_ptr<IntrinsicsEquidistant> to_intrinsics_equidistant(
|
||||
const cv::FileNode &n, const std::uint8_t &model,
|
||||
const std::uint16_t &width, const std::uint16_t &height) {
|
||||
auto in = std::make_shared<IntrinsicsEquidistant>();
|
||||
in->width = width;
|
||||
in->height = height;
|
||||
for (std::size_t i = 0; i < 8; i++) {
|
||||
in->coeffs[i] = n["coeffs"][i];
|
||||
}
|
||||
MYNTEYE_UNUSED(model)
|
||||
return in;
|
||||
}
|
||||
|
||||
DeviceWriter::img_params_t to_img_params(
|
||||
const cv::FileNode &n, const std::uint8_t &model,
|
||||
const std::uint16_t &width, const std::uint16_t &height) {
|
||||
DeviceWriter::img_params_t params;
|
||||
params.ok = false;
|
||||
|
||||
CalibrationModel calib_model = static_cast<CalibrationModel>(model);
|
||||
switch (calib_model) {
|
||||
case CalibrationModel::PINHOLE: {
|
||||
params.ok = true;
|
||||
params.in_left = to_intrinsics_pinhole(
|
||||
n["in_left"], model, width, height);
|
||||
params.in_right = to_intrinsics_pinhole(
|
||||
n["in_right"], model, width, height);
|
||||
n["ex_right_to_left"] >> params.ex_right_to_left;
|
||||
} break;
|
||||
case CalibrationModel::KANNALA_BRANDT: {
|
||||
params.ok = true;
|
||||
params.in_left = to_intrinsics_equidistant(
|
||||
n["in_left"], model, width, height);
|
||||
params.in_right = to_intrinsics_equidistant(
|
||||
n["in_right"], model, width, height);
|
||||
n["ex_right_to_left"] >> params.ex_right_to_left;
|
||||
} break;
|
||||
default:
|
||||
LOG(FATAL) << "Could not load img params as unknown calib model"
|
||||
", please use latest SDK.";
|
||||
}
|
||||
return params;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DeviceWriter::dev_info_t DeviceWriter::LoadDeviceInfo(
|
||||
const std::string &filepath) {
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::READ);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(FATAL) << "Failed to load file: " << filepath;
|
||||
}
|
||||
DeviceInfo info;
|
||||
info.lens_type = Type(std::string(fs["lens_type"]));
|
||||
info.imu_type = Type(std::string(fs["imu_type"]));
|
||||
fs["nominal_baseline"] >> info.nominal_baseline;
|
||||
fs.release();
|
||||
return info;
|
||||
}
|
||||
|
||||
DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams(
|
||||
const std::string &filepath) {
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::READ);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(FATAL) << "Failed to load file: " << filepath;
|
||||
}
|
||||
|
||||
img_params_map_t img_params_map;
|
||||
if (!fs["version"].isNone()) {
|
||||
std::string version = std::string(fs["version"]);
|
||||
// load params according to verison
|
||||
if (version == "1.0") {
|
||||
fs["img_params_map"][0] >> img_params_map[{752, 480}];
|
||||
} else if (version == "1.1") {
|
||||
fs["img_params_map"][0] >> img_params_map[{1280, 400}];
|
||||
fs["img_params_map"][1] >> img_params_map[{2560, 800}];
|
||||
} else if (version == "1.2") {
|
||||
auto node = fs["img_params"];
|
||||
for (auto it = node.begin(); it < node.end(); it++) {
|
||||
std::uint8_t model;
|
||||
std::uint16_t width, height;
|
||||
(*it)["model"] >> model;
|
||||
(*it)["width"] >> width;
|
||||
(*it)["height"] >> height;
|
||||
auto params = to_img_params(*it, model, width, height);
|
||||
if (params.ok) {
|
||||
params.version = version;
|
||||
img_params_map[{width, height}] = params;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
LOG(ERROR) << "Failed to load img params of version " << version
|
||||
<< ", please use latest SDK.";
|
||||
}
|
||||
} else {
|
||||
// load old params s1030
|
||||
auto in_left = std::make_shared<IntrinsicsPinhole>();
|
||||
auto in_right = std::make_shared<IntrinsicsPinhole>();
|
||||
Extrinsics ex_right_to_left;
|
||||
if (fs["in_left"].isNone()) {
|
||||
std::uint16_t w = 752;
|
||||
std::uint16_t h = 480;
|
||||
std::uint8_t m = 0;
|
||||
if (!fs["width"].isNone())
|
||||
w = static_cast<int>(fs["width"]);
|
||||
if (!fs["height"].isNone())
|
||||
h = static_cast<int>(fs["height"]);
|
||||
if (!fs["model"].isNone())
|
||||
m = static_cast<int>(fs["model"]);
|
||||
|
||||
cv::Mat M1, D1, M2, D2, R, T;
|
||||
fs["M1"] >> M1;
|
||||
fs["D1"] >> D1;
|
||||
fs["M2"] >> M2;
|
||||
fs["D2"] >> D2;
|
||||
fs["R"] >> R;
|
||||
fs["T"] >> T;
|
||||
|
||||
to_intrinsics(w, h, m, M1, D1, in_left.get());
|
||||
to_intrinsics(w, h, m, M2, D2, in_right.get());
|
||||
to_extrinsics(R, T, &ex_right_to_left);
|
||||
} else {
|
||||
fs["in_left"] >> *in_left;
|
||||
fs["in_right"] >> *in_right;
|
||||
fs["ex_right_to_left"] >> ex_right_to_left;
|
||||
}
|
||||
img_params_map[{752, 480}] = {
|
||||
true, "1.0", in_left, in_right, ex_right_to_left
|
||||
};
|
||||
}
|
||||
|
||||
fs.release();
|
||||
return img_params_map;
|
||||
}
|
||||
|
||||
DeviceWriter::imu_params_t DeviceWriter::LoadImuParams(
|
||||
const std::string &filepath) {
|
||||
using FileStorage = cv::FileStorage;
|
||||
FileStorage fs(filepath, FileStorage::READ);
|
||||
if (!fs.isOpened()) {
|
||||
LOG(FATAL) << "Failed to load file: " << filepath;
|
||||
}
|
||||
|
||||
imu_params_t params;
|
||||
if (!fs["version"].isNone()) {
|
||||
std::string version = std::string(fs["version"]);
|
||||
// load params according to verison
|
||||
if (version == "1.2") {
|
||||
params.version = version;
|
||||
fs["in_accel"] >> params.in_accel;
|
||||
fs["in_gyro"] >> params.in_gyro;
|
||||
fs["ex_left_to_imu"] >> params.ex_left_to_imu;
|
||||
} else {
|
||||
LOG(ERROR) << "Failed to load imu params of version " << version
|
||||
<< ", please use latest SDK.";
|
||||
}
|
||||
} else {
|
||||
// load old params
|
||||
fs["in_accel"] >> params.in_accel;
|
||||
fs["in_gyro"] >> params.in_gyro;
|
||||
fs["ex_left_to_imu"] >> params.ex_left_to_imu;
|
||||
}
|
||||
|
||||
fs.release();
|
||||
return params;
|
||||
}
|
||||
|
||||
} // namespace tools
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
// 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.
|
||||
#ifndef MYNTEYE_TOOLS_DEVICE_WRITER_H_ // NOLINT
|
||||
#define MYNTEYE_TOOLS_DEVICE_WRITER_H_
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "mynteye/device/device.h"
|
||||
|
||||
MYNTEYE_BEGIN_NAMESPACE
|
||||
|
||||
class Device;
|
||||
|
||||
namespace tools {
|
||||
|
||||
class DeviceWriter {
|
||||
public:
|
||||
using dev_info_t = DeviceInfo;
|
||||
using imu_params_t = device::imu_params_t;
|
||||
using img_params_t = device::img_params_t;
|
||||
using img_params_map_t = std::map<Resolution, device::img_params_t>;
|
||||
|
||||
explicit DeviceWriter(std::shared_ptr<Device> device);
|
||||
~DeviceWriter();
|
||||
|
||||
bool WriteDeviceInfo(const dev_info_t &info);
|
||||
bool WriteDeviceInfo(const std::string &filepath);
|
||||
|
||||
bool WriteImgParams(const img_params_map_t &img_params_map);
|
||||
bool WriteImgParams(const std::string &filepath);
|
||||
|
||||
bool WriteImuParams(const imu_params_t ¶ms);
|
||||
bool WriteImuParams(const std::string &filepath);
|
||||
|
||||
bool SaveDeviceInfo(const dev_info_t &info, const std::string &filepath);
|
||||
bool SaveImgParams(const img_params_map_t &img_params_map,
|
||||
const std::string &filepath);
|
||||
bool SaveImuParams(const imu_params_t ¶ms, const std::string &filepath);
|
||||
|
||||
/** Save all infos of this device */
|
||||
void SaveAllInfos(const std::string &dir);
|
||||
|
||||
private:
|
||||
dev_info_t LoadDeviceInfo(const std::string &filepath);
|
||||
img_params_map_t LoadImgParams(const std::string &filepath);
|
||||
imu_params_t LoadImuParams(const std::string &filepath);
|
||||
|
||||
std::shared_ptr<Device> device_;
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
|
||||
MYNTEYE_END_NAMESPACE
|
||||
|
||||
#endif // MYNTEYE_TOOLS_DEVICE_WRITER_H_ NOLINT
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
// 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.
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/device/device.h"
|
||||
#include "mynteye/device/utils.h"
|
||||
|
||||
#include "writer/device_writer.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
glog_init _(argc, argv);
|
||||
|
||||
const char *filepath;
|
||||
if (argc >= 2) {
|
||||
filepath = argv[1];
|
||||
} else {
|
||||
LOG(ERROR) << "Usage: ./img_params_writer <filepath>";
|
||||
return 2;
|
||||
}
|
||||
|
||||
auto &&device = device::select();
|
||||
if (!device)
|
||||
return 1;
|
||||
|
||||
tools::DeviceWriter writer(device);
|
||||
writer.WriteImgParams(filepath);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
// 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.
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/device/device.h"
|
||||
#include "mynteye/device/utils.h"
|
||||
|
||||
#include "writer/device_writer.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
glog_init _(argc, argv);
|
||||
|
||||
const char *filepath;
|
||||
if (argc >= 2) {
|
||||
filepath = argv[1];
|
||||
} else {
|
||||
LOG(ERROR) << "Usage: ./imu_params_writer <filepath>";
|
||||
return 2;
|
||||
}
|
||||
|
||||
auto &&device = device::select();
|
||||
if (!device)
|
||||
return 1;
|
||||
|
||||
tools::DeviceWriter writer(device);
|
||||
writer.WriteImuParams(filepath);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
// 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.
|
||||
#include "mynteye/logger.h"
|
||||
#include "mynteye/device/device.h"
|
||||
#include "mynteye/device/utils.h"
|
||||
|
||||
#include "writer/device_writer.h"
|
||||
|
||||
MYNTEYE_USE_NAMESPACE
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
glog_init _(argc, argv);
|
||||
|
||||
std::string dir{"config"};
|
||||
if (argc >= 2) {
|
||||
dir = argv[1];
|
||||
}
|
||||
|
||||
auto &&device = device::select();
|
||||
if (!device)
|
||||
return 1;
|
||||
|
||||
dir.append(MYNTEYE_OS_SEP).append(device->GetInfo()->name);
|
||||
dir.append(MYNTEYE_OS_SEP "SN").append(device->GetInfo()->serial_number);
|
||||
|
||||
tools::DeviceWriter writer(device);
|
||||
writer.SaveAllInfos(dir);
|
||||
|
||||
LOG(INFO) << "Save all infos to \"" << dir << "\"";
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user