From e539cb9fe05ac321b3b3e8cbd79a0fa1eb26abea Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 5 Jan 2019 20:02:08 +0800 Subject: [PATCH] refactor(channels): sepreate bytes and file channel --- CMakeLists.txt | 4 +- include/mynteye/device/callbacks.h | 14 - include/mynteye/device/device.h | 22 +- src/mynteye/device/channel/bytes.cc | 208 +++++++++ src/mynteye/device/channel/bytes.h | 89 ++++ .../device/{channels => channel}/channels.cc | 326 +------------- .../device/{channels => channel}/channels.h | 118 +---- src/mynteye/device/channel/def.h | 40 ++ src/mynteye/device/channel/file_channel.cc | 403 ++++++++++++++++++ src/mynteye/device/channel/file_channel.h | 142 ++++++ src/mynteye/device/device.cc | 2 +- src/mynteye/device/motions.cc | 2 +- .../device/standard/channels_adapter_s.cc | 37 -- .../device/standard/channels_adapter_s.h | 9 +- .../device/standard2/channels_adapter_s2.cc | 60 --- .../device/standard2/channels_adapter_s2.h | 9 +- .../standard2/channels_adapter_s210a.cc | 60 --- .../device/standard2/channels_adapter_s210a.h | 9 +- src/mynteye/device/types.h | 4 + tools/writer/device_writer.cc | 4 +- tools/writer/device_writer.h | 2 +- 21 files changed, 940 insertions(+), 624 deletions(-) create mode 100644 src/mynteye/device/channel/bytes.cc create mode 100644 src/mynteye/device/channel/bytes.h rename src/mynteye/device/{channels => channel}/channels.cc (72%) rename src/mynteye/device/{channels => channel}/channels.h (56%) create mode 100644 src/mynteye/device/channel/def.h create mode 100644 src/mynteye/device/channel/file_channel.cc create mode 100644 src/mynteye/device/channel/file_channel.h diff --git a/CMakeLists.txt b/CMakeLists.txt index c32423e..e0ba082 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -202,7 +202,9 @@ set(MYNTEYE_SRCS src/mynteye/types.cc src/mynteye/util/files.cc src/mynteye/util/strings.cc - src/mynteye/device/channels/channels.cc + src/mynteye/device/channel/bytes.cc + src/mynteye/device/channel/channels.cc + src/mynteye/device/channel/file_channel.cc src/mynteye/device/config.cc src/mynteye/device/context.cc src/mynteye/device/device.cc diff --git a/include/mynteye/device/callbacks.h b/include/mynteye/device/callbacks.h index 91cf934..b1601e3 100644 --- a/include/mynteye/device/callbacks.h +++ b/include/mynteye/device/callbacks.h @@ -30,20 +30,6 @@ MYNTEYE_BEGIN_NAMESPACE namespace device { -typedef struct ImgParams { - bool ok; - std::shared_ptr in_left; - std::shared_ptr in_right; - Extrinsics ex_right_to_left; -} img_params_t; - -typedef struct ImuParams { - bool ok; - ImuIntrinsics in_accel; - ImuIntrinsics in_gyro; - Extrinsics ex_left_to_imu; -} imu_params_t; - /** * @ingroup datatypes * Frame with raw data. diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index 50e1d50..6dded7a 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -51,6 +51,26 @@ class StreamsAdapter; template class AsyncCallback; +namespace device { + +typedef struct ImgParams { + bool ok; + std::string version; + std::shared_ptr in_left; + std::shared_ptr in_right; + Extrinsics ex_right_to_left; +} img_params_t; + +typedef struct ImuParams { + bool ok; + std::string version; + ImuIntrinsics in_accel; + ImuIntrinsics in_gyro; + Extrinsics ex_left_to_imu; +} imu_params_t; + +} // namespace device + /** * The Device class to communicate with MYNT® EYE device. */ @@ -266,7 +286,7 @@ class MYNTEYE_API Device { */ device::StreamData GetStreamData(const Stream &stream); - /** + /** * @deprecated Replaced by GetStreamData(const Stream &stream) */ device::StreamData GetLatestStreamData(const Stream &stream); diff --git a/src/mynteye/device/channel/bytes.cc b/src/mynteye/device/channel/bytes.cc new file mode 100644 index 0000000..d08164a --- /dev/null +++ b/src/mynteye/device/channel/bytes.cc @@ -0,0 +1,208 @@ +// 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/device/channel/bytes.h" + +#include "mynteye/util/strings.h" + +MYNTEYE_BEGIN_NAMESPACE + +namespace bytes { + +// from + +std::string _from_data(const std::uint8_t *data, std::size_t count) { + std::string s(reinterpret_cast(data), count); + strings::trim(s); + return s; +} + +// from types + +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) { + std::size_t i = 0; + + // width, 2 + in->width = _from_data(data + i); + i += 2; + // height, 2 + in->height = _from_data(data + i); + i += 2; + // fx, 8 + in->fx = _from_data(data + i); + i += 8; + // fy, 8 + in->fy = _from_data(data + i); + i += 8; + // cx, 8 + in->cx = _from_data(data + i); + i += 8; + // cy, 8 + in->cy = _from_data(data + i); + i += 8; + // model, 1 + in->model = data[i]; + i += 1; + // coeffs, 40 + for (std::size_t j = 0; j < 5; j++) { + in->coeffs[j] = _from_data(data + i + j * 8); + } + i += 40; + + return i; +} + +std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) { + std::size_t i = 0; + + // scale + for (std::size_t j = 0; j < 3; j++) { + for (std::size_t k = 0; k < 3; k++) { + in->scale[j][k] = _from_data(data + i + (j * 3 + k) * 8); + } + } + i += 72; + // drift + for (std::size_t j = 0; j < 3; j++) { + in->drift[j] = _from_data(data + i + j * 8); + } + i += 24; + // noise + for (std::size_t j = 0; j < 3; j++) { + in->noise[j] = _from_data(data + i + j * 8); + } + i += 24; + // bias + for (std::size_t j = 0; j < 3; j++) { + in->bias[j] = _from_data(data + i + j * 8); + } + i += 24; + + return i; +} + +std::size_t from_data(Extrinsics *ex, const std::uint8_t *data) { + std::size_t i = 0; + + // rotation + for (std::size_t j = 0; j < 3; j++) { + for (std::size_t k = 0; k < 3; k++) { + ex->rotation[j][k] = _from_data(data + i + (j * 3 + k) * 8); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + ex->translation[j] = _from_data(data + i + j * 8); + } + i += 24; + + return i; +} + +// to + +std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) { + std::copy(value.begin(), value.end(), data); + for (std::size_t i = value.size(); i < count; i++) { + data[i] = ' '; + } + return count; +} + +// to types + +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) { + std::size_t i = 0; + + // width, 2 + _to_data(in->width, data + i); + i += 2; + // height, 2 + _to_data(in->height, data + i); + i += 2; + // fx, 8 + _to_data(in->fx, data + i); + i += 8; + // fy, 8 + _to_data(in->fy, data + i); + i += 8; + // cx, 8 + _to_data(in->cx, data + i); + i += 8; + // cy, 8 + _to_data(in->cy, data + i); + i += 8; + // model, 1 + data[i] = in->model; + i += 1; + // coeffs, 40 + for (std::size_t j = 0; j < 5; j++) { + _to_data(in->coeffs[j], data + i + j * 8); + } + i += 40; + + return i; +} + +std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) { + std::size_t i = 0; + + // scale + for (std::size_t j = 0; j < 3; j++) { + for (std::size_t k = 0; k < 3; k++) { + _to_data(in->scale[j][k], data + i + (j * 3 + k) * 8); + } + } + i += 72; + // drift + for (std::size_t j = 0; j < 3; j++) { + _to_data(in->drift[j], data + i + j * 8); + } + i += 24; + // noise + for (std::size_t j = 0; j < 3; j++) { + _to_data(in->noise[j], data + i + j * 8); + } + i += 24; + // bias + for (std::size_t j = 0; j < 3; j++) { + _to_data(in->bias[j], data + i + j * 8); + } + i += 24; + + return i; +} + +std::size_t to_data(const Extrinsics *ex, std::uint8_t *data) { + std::size_t i = 0; + + // rotation + for (std::size_t j = 0; j < 3; j++) { + for (std::size_t k = 0; k < 3; k++) { + _to_data(ex->rotation[j][k], data + i + (j * 3 + k) * 8); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + _to_data(ex->translation[j], data + i + j * 8); + } + i += 24; + + return i; +} + +} // namespace bytes + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/channel/bytes.h b/src/mynteye/device/channel/bytes.h new file mode 100644 index 0000000..5014e05 --- /dev/null +++ b/src/mynteye/device/channel/bytes.h @@ -0,0 +1,89 @@ +// 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_DEVICE_CHANNEL_BYTES_H_ +#define MYNTEYE_DEVICE_CHANNEL_BYTES_H_ +#pragma once + +#include +#include + +#include "mynteye/mynteye.h" +#include "mynteye/types.h" +#include "mynteye/device/channel/def.h" +#include "mynteye/device/types.h" + +MYNTEYE_BEGIN_NAMESPACE + +namespace bytes { + +// from + +template +T _from_data(const std::uint8_t *data) { + std::size_t size = sizeof(T) / sizeof(std::uint8_t); + T value = 0; + for (std::size_t i = 0; i < size; i++) { + value |= data[i] << (8 * (size - i - 1)); + } + return value; +} + +template <> +inline double _from_data(const std::uint8_t *data) { + return *(reinterpret_cast(data)); +} + +std::string _from_data(const std::uint8_t *data, std::size_t count); + +// from types + +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data); + +std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data); + +std::size_t from_data(Extrinsics *ex, const std::uint8_t *data); + +// to + +template +std::size_t _to_data(T value, std::uint8_t *data) { + std::size_t size = sizeof(T) / sizeof(std::uint8_t); + for (std::size_t i = 0; i < size; i++) { + data[i] = static_cast((value >> (8 * (size - i - 1))) & 0xFF); + } + return size; +} + +template <> +inline std::size_t _to_data(double value, std::uint8_t *data) { + std::uint8_t *val = reinterpret_cast(&value); + std::copy(val, val + 8, data); + return 8; +} + +std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count); + +// to types + +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data); + +std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data); + +std::size_t to_data(const Extrinsics *ex, std::uint8_t *data); + +} // namespace bytes + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_DEVICE_CHANNEL_BYTES_H_ diff --git a/src/mynteye/device/channels/channels.cc b/src/mynteye/device/channel/channels.cc similarity index 72% rename from src/mynteye/device/channels/channels.cc rename to src/mynteye/device/channel/channels.cc index 6a73fd0..f29ccf0 100644 --- a/src/mynteye/device/channels/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -11,7 +11,7 @@ // 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/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" #include #include @@ -19,10 +19,10 @@ #include #include #include +#include #include #include "mynteye/logger.h" -#include "mynteye/util/strings.h" #include "mynteye/util/times.h" #define IMU_TRACK_PERIOD 25 // ms @@ -432,7 +432,7 @@ void Channels::StopImuTracking() { bool Channels::GetFiles( device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version) const { + Version *spec_version) { if (info == nullptr && img_params == nullptr && imu_params == nullptr) { LOG(WARNING) << "Files are not provided to get"; return false; @@ -486,7 +486,8 @@ bool Channels::GetFiles( i += 3; switch (file_id) { case FID_DEVICE_INFO: { - CHECK_EQ(bytes::from_data(info, data + i), file_size) + auto &&n = file_channel_.GetDeviceInfoFromData(data + i, info); + CHECK_EQ(n, file_size) << "The firmware not support getting device info, you could " "upgrade to latest"; spec_ver = &info->spec_version; @@ -495,8 +496,7 @@ bool Channels::GetFiles( case FID_IMG_PARAMS: { if (file_size > 0) { CheckSpecVersion(spec_ver); - auto &&n = adapter_->GetImgParamsFromData( - data + i, spec_ver, img_params); + auto &&n = file_channel_.GetImgParamsFromData(data + i, img_params); CHECK_EQ(n, file_size); } } break; @@ -504,8 +504,7 @@ bool Channels::GetFiles( imu_params->ok = file_size > 0; if (imu_params->ok) { CheckSpecVersion(spec_ver); - auto &&n = adapter_->GetImuParamsFromData( - data + i, spec_ver, imu_params); + auto &&n = file_channel_.GetImuParamsFromData(data + i, imu_params); CHECK_EQ(n, file_size); } } break; @@ -544,15 +543,15 @@ bool Channels::SetFiles( std::uint16_t size = 0; if (info != nullptr) { header[0] = true; - size += bytes::to_data(info, data + 3 + size, spec_ver); + size += file_channel_.SetDeviceInfoToData(info, data + 3 + size); } if (img_params != nullptr) { header[1] = true; - size += adapter_->SetImgParamsToData(img_params, spec_ver, data + 3 + size); + size += file_channel_.SetImgParamsToData(img_params, data + 3 + size); } if (imu_params != nullptr) { header[2] = true; - size += adapter_->SetImuParamsToData(imu_params, spec_ver, data + 3 + size); + size += file_channel_.SetImuParamsToData(imu_params, data + 3 + size); } data[0] = static_cast(header.to_ulong()); @@ -737,309 +736,4 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const { return {min, max, def}; } - -std::size_t ChannelsAdapter::GetImuParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::imu_params_t *imu_params) { - std::size_t i = 0; - i += bytes::from_data(&imu_params->in_accel, data + i, version); - i += bytes::from_data(&imu_params->in_gyro, data + i, version); - i += bytes::from_data(&imu_params->ex_left_to_imu, data + i, version); - return i; -} - -std::size_t ChannelsAdapter::SetImuParamsToData( - const Channels::imu_params_t *imu_params, const Version *version, - std::uint8_t *data) { - std::size_t i = 3; // skip id, size - i += bytes::to_data(&imu_params->in_accel, data + i, version); - i += bytes::to_data(&imu_params->in_gyro, data + i, version); - i += bytes::to_data(&imu_params->ex_left_to_imu, data + i, version); - // others - std::size_t size = i - 3; - data[0] = Channels::FID_IMU_PARAMS; - data[1] = static_cast((size >> 8) & 0xFF); - data[2] = static_cast(size & 0xFF); - return size + 3; -} - - -namespace bytes { - -// from - -std::string _from_data(const std::uint8_t *data, std::size_t count) { - std::string s(reinterpret_cast(data), count); - strings::trim(s); - return s; -} - -std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) { - std::size_t i = 4; // skip vid, pid - // name, 16 - info->name = _from_data(data + i, 16); - i += 16; - // serial_number, 16 - info->serial_number = _from_data(data + i, 16); - i += 16; - // firmware_version, 2 - info->firmware_version.set_major(data[i]); - info->firmware_version.set_minor(data[i + 1]); - i += 2; - // hardware_version, 3 - info->hardware_version.set_major(data[i]); - info->hardware_version.set_minor(data[i + 1]); - info->hardware_version.set_flag(std::bitset<8>(data[i + 2])); - i += 3; - // spec_version, 2 - info->spec_version.set_major(data[i]); - info->spec_version.set_minor(data[i + 1]); - i += 2; - // lens_type, 4 - info->lens_type.set_vendor(_from_data(data + i)); - info->lens_type.set_product(_from_data(data + i + 2)); - i += 4; - // imu_type, 4 - info->imu_type.set_vendor(_from_data(data + i)); - info->imu_type.set_product(_from_data(data + i + 2)); - i += 4; - // nominal_baseline, 2 - info->nominal_baseline = _from_data(data + i); - i += 2; - - return i; -} - -std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // width, 2 - in->width = _from_data(data + i); - i += 2; - // height, 2 - in->height = _from_data(data + i); - i += 2; - // fx, 8 - in->fx = _from_data(data + i); - i += 8; - // fy, 8 - in->fy = _from_data(data + i); - i += 8; - // cx, 8 - in->cx = _from_data(data + i); - i += 8; - // cy, 8 - in->cy = _from_data(data + i); - i += 8; - // model, 1 - in->model = data[i]; - i += 1; - // coeffs, 40 - for (std::size_t j = 0; j < 5; j++) { - in->coeffs[j] = _from_data(data + i + j * 8); - } - i += 40; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // scale - for (std::size_t j = 0; j < 3; j++) { - for (std::size_t k = 0; k < 3; k++) { - in->scale[j][k] = _from_data(data + i + (j * 3 + k) * 8); - } - } - i += 72; - // drift - for (std::size_t j = 0; j < 3; j++) { - in->drift[j] = _from_data(data + i + j * 8); - } - i += 24; - // noise - for (std::size_t j = 0; j < 3; j++) { - in->noise[j] = _from_data(data + i + j * 8); - } - i += 24; - // bias - for (std::size_t j = 0; j < 3; j++) { - in->bias[j] = _from_data(data + i + j * 8); - } - i += 24; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -std::size_t from_data(Extrinsics *ex, const std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // rotation - for (std::size_t j = 0; j < 3; j++) { - for (std::size_t k = 0; k < 3; k++) { - ex->rotation[j][k] = _from_data(data + i + (j * 3 + k) * 8); - } - } - i += 72; - // translation - for (std::size_t j = 0; j < 3; j++) { - ex->translation[j] = _from_data(data + i + j * 8); - } - i += 24; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -// to - -std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) { - std::copy(value.begin(), value.end(), data); - for (std::size_t i = value.size(); i < count; i++) { - data[i] = ' '; - } - return count; -} - -std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 3; // skip id, size - i += 4; // skip vid, pid - // name, 16 - _to_data(info->name, data + i, 16); - i += 16; - // serial_number, 16 - _to_data(info->serial_number, data + i, 16); - i += 16; - // firmware_version, 2 - data[i] = info->firmware_version.major(); - data[i + 1] = info->firmware_version.minor(); - i += 2; - // hardware_version, 3 - data[i] = info->hardware_version.major(); - data[i + 1] = info->hardware_version.minor(); - data[i + 2] = - static_cast(info->hardware_version.flag().to_ulong()); - i += 3; - // spec_version, 2 - data[i] = info->spec_version.major(); - data[i + 1] = info->spec_version.minor(); - i += 2; - // lens_type, 4 - _to_data(info->lens_type.vendor(), data + i); - _to_data(info->lens_type.product(), data + i + 2); - i += 4; - // imu_type, 4 - _to_data(info->imu_type.vendor(), data + i); - _to_data(info->imu_type.product(), data + i + 2); - i += 4; - // nominal_baseline, 2 - _to_data(info->nominal_baseline, data + i); - i += 2; - - MYNTEYE_UNUSED(spec_version) - - // others - std::size_t size = i - 3; - data[0] = Channels::FID_DEVICE_INFO; - data[1] = static_cast((size >> 8) & 0xFF); - data[2] = static_cast(size & 0xFF); - return size + 3; -} - -std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // width, 2 - _to_data(in->width, data + i); - i += 2; - // height, 2 - _to_data(in->height, data + i); - i += 2; - // fx, 8 - _to_data(in->fx, data + i); - i += 8; - // fy, 8 - _to_data(in->fy, data + i); - i += 8; - // cx, 8 - _to_data(in->cx, data + i); - i += 8; - // cy, 8 - _to_data(in->cy, data + i); - i += 8; - // model, 1 - data[i] = in->model; - i += 1; - // coeffs, 40 - for (std::size_t j = 0; j < 5; j++) { - _to_data(in->coeffs[j], data + i + j * 8); - } - i += 40; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // scale - for (std::size_t j = 0; j < 3; j++) { - for (std::size_t k = 0; k < 3; k++) { - _to_data(in->scale[j][k], data + i + (j * 3 + k) * 8); - } - } - i += 72; - // drift - for (std::size_t j = 0; j < 3; j++) { - _to_data(in->drift[j], data + i + j * 8); - } - i += 24; - // noise - for (std::size_t j = 0; j < 3; j++) { - _to_data(in->noise[j], data + i + j * 8); - } - i += 24; - // bias - for (std::size_t j = 0; j < 3; j++) { - _to_data(in->bias[j], data + i + j * 8); - } - i += 24; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -std::size_t to_data(const Extrinsics *ex, std::uint8_t *data, - const Version *spec_version) { - std::size_t i = 0; - - // rotation - for (std::size_t j = 0; j < 3; j++) { - for (std::size_t k = 0; k < 3; k++) { - _to_data(ex->rotation[j][k], data + i + (j * 3 + k) * 8); - } - } - i += 72; - // translation - for (std::size_t j = 0; j < 3; j++) { - _to_data(ex->translation[j], data + i + j * 8); - } - i += 24; - - MYNTEYE_UNUSED(spec_version) - return i; -} - -} // namespace bytes - MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/channels/channels.h b/src/mynteye/device/channel/channels.h similarity index 56% rename from src/mynteye/device/channels/channels.h rename to src/mynteye/device/channel/channels.h index 371f3af..1ab453e 100644 --- a/src/mynteye/device/channels/channels.h +++ b/src/mynteye/device/channel/channels.h @@ -11,20 +11,19 @@ // 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_DEVICE_CHANNELS_CHANNELS_H_ -#define MYNTEYE_DEVICE_CHANNELS_CHANNELS_H_ +#ifndef MYNTEYE_DEVICE_CHANNEL_CHANNELS_H_ +#define MYNTEYE_DEVICE_CHANNEL_CHANNELS_H_ #pragma once -#include #include #include #include -#include #include #include #include "mynteye/mynteye.h" -#include "mynteye/types.h" +#include "mynteye/device/channel/def.h" +#include "mynteye/device/channel/file_channel.h" #include "mynteye/device/device.h" #include "mynteye/device/types.h" #include "mynteye/uvc/uvc.h" @@ -42,15 +41,6 @@ class ChannelsAdapter; class MYNTEYE_API Channels { public: - typedef enum Channel { - CHANNEL_CAM_CTRL = 1, - CHANNEL_HALF_DUPLEX = 2, - CHANNEL_IMU_WRITE = 3, - CHANNEL_IMU_READ = 4, - CHANNEL_FILE = 5, - CHANNEL_LAST - } channel_t; - typedef struct ControlInfo { std::int32_t min; std::int32_t max; @@ -63,19 +53,11 @@ class MYNTEYE_API Channels { XU_CMD_LAST } xu_cmd_t; - typedef enum FileId { - FID_DEVICE_INFO = 1, // device info - FID_IMG_PARAMS = 2, // image intrinsics & extrinsics - FID_IMU_PARAMS = 4, // imu intrinsics & extrinsics - FID_LAST, - } file_id_t; - using imu_callback_t = std::function; - using device_info_t = DeviceInfo; - - using img_params_t = std::map; - using imu_params_t = device::imu_params_t; + using device_info_t = FileChannel::device_info_t; + using img_params_t = FileChannel::img_params_t; + using imu_params_t = FileChannel::imu_params_t; Channels(const std::shared_ptr &device, const std::shared_ptr &adapter); @@ -101,7 +83,7 @@ class MYNTEYE_API Channels { bool GetFiles( device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version = nullptr) const; + Version *spec_version = nullptr); bool SetFiles( device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, Version *spec_version = nullptr); @@ -142,6 +124,8 @@ class MYNTEYE_API Channels { std::shared_ptr device_; std::shared_ptr adapter_; + FileChannel file_channel_; + std::map control_infos_; bool is_imu_tracking_; @@ -165,88 +149,8 @@ class ChannelsAdapter { virtual std::vector GetGyroRangeValues() = 0; virtual void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) = 0; - - virtual std::size_t GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) = 0; - virtual std::size_t SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) = 0; - - virtual std::size_t GetImuParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::imu_params_t *imu_params); - virtual std::size_t SetImuParamsToData( - const Channels::imu_params_t *imu_params, const Version *version, - std::uint8_t *data); }; -namespace bytes { - -// from - -template -T _from_data(const std::uint8_t *data) { - std::size_t size = sizeof(T) / sizeof(std::uint8_t); - T value = 0; - for (std::size_t i = 0; i < size; i++) { - value |= data[i] << (8 * (size - i - 1)); - } - return value; -} - -template <> -inline double _from_data(const std::uint8_t *data) { - return *(reinterpret_cast(data)); -} - -std::string _from_data(const std::uint8_t *data, std::size_t count); - -std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data); - -std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, - const Version *spec_version); - -std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data, - const Version *spec_version); - -std::size_t from_data(Extrinsics *ex, const std::uint8_t *data, - const Version *spec_version); - -// to - -template -std::size_t _to_data(T value, std::uint8_t *data) { - std::size_t size = sizeof(T) / sizeof(std::uint8_t); - for (std::size_t i = 0; i < size; i++) { - data[i] = static_cast((value >> (8 * (size - i - 1))) & 0xFF); - } - return size; -} - -template <> -inline std::size_t _to_data(double value, std::uint8_t *data) { - std::uint8_t *val = reinterpret_cast(&value); - std::copy(val, val + 8, data); - return 8; -} - -std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count); - -std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data, - const Version *spec_version); - -std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, - const Version *spec_version); - -std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data, - const Version *spec_version); - -std::size_t to_data(const Extrinsics *ex, std::uint8_t *data, - const Version *spec_version); - -} // namespace bytes - MYNTEYE_END_NAMESPACE -#endif // MYNTEYE_DEVICE_CHANNELS_CHANNELS_H_ +#endif // MYNTEYE_DEVICE_CHANNEL_CHANNELS_H_ diff --git a/src/mynteye/device/channel/def.h b/src/mynteye/device/channel/def.h new file mode 100644 index 0000000..7e5d39e --- /dev/null +++ b/src/mynteye/device/channel/def.h @@ -0,0 +1,40 @@ +// 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_DEVICE_CHANNEL_DEF_H_ +#define MYNTEYE_DEVICE_CHANNEL_DEF_H_ +#pragma once + +#include "mynteye/mynteye.h" + +MYNTEYE_BEGIN_NAMESPACE + +typedef enum Channel { + CHANNEL_CAM_CTRL = 1, + CHANNEL_HALF_DUPLEX = 2, + CHANNEL_IMU_WRITE = 3, + CHANNEL_IMU_READ = 4, + CHANNEL_FILE = 5, + CHANNEL_LAST +} channel_t; + +typedef enum FileId { + FID_DEVICE_INFO = 1, // device info + FID_IMG_PARAMS = 2, // image intrinsics & extrinsics + FID_IMU_PARAMS = 4, // imu intrinsics & extrinsics + FID_LAST, +} file_id_t; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_DEVICE_CHANNEL_DEF_H_ diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc new file mode 100644 index 0000000..d3b4da1 --- /dev/null +++ b/src/mynteye/device/channel/file_channel.cc @@ -0,0 +1,403 @@ +// 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/device/channel/file_channel.h" + +#include "mynteye/logger.h" + +MYNTEYE_BEGIN_NAMESPACE + +// FileChannel + +FileChannel::FileChannel() { + dev_info_parser_ = std::make_shared(); + img_params_parser_ = std::make_shared(); + imu_params_parser_ = std::make_shared(); +} + +FileChannel::~FileChannel() { +} + +std::size_t FileChannel::GetDeviceInfoFromData( + const std::uint8_t *data, device_info_t *info) { + auto n = dev_info_parser_->GetFromData(data, info); + auto spec_version = info->spec_version; + img_params_parser_->SetSpecVersion(spec_version); + imu_params_parser_->SetSpecVersion(spec_version); + return n; +} + +std::size_t FileChannel::SetDeviceInfoToData( + const device_info_t *info, std::uint8_t *data) { + auto spec_version = info->spec_version; + img_params_parser_->SetSpecVersion(spec_version); + imu_params_parser_->SetSpecVersion(spec_version); + return dev_info_parser_->SetToData(info, data); +} + +std::size_t FileChannel::GetImgParamsFromData( + const std::uint8_t *data, img_params_t *img_params) { + CHECK_NOTNULL(img_params_parser_); + return img_params_parser_->GetFromData(data, img_params); +} + +std::size_t FileChannel::SetImgParamsToData( + const img_params_t *img_params, std::uint8_t *data) { + CHECK_NOTNULL(img_params_parser_); + return img_params_parser_->SetToData(img_params, data); +} + +std::size_t FileChannel::GetImuParamsFromData( + const std::uint8_t *data, imu_params_t *imu_params) { + return imu_params_parser_->GetFromData(data, imu_params); +} + +std::size_t FileChannel::SetImuParamsToData( + const imu_params_t *imu_params, std::uint8_t *data) { + return imu_params_parser_->SetToData(imu_params, data); +} + +// DeviceInfoParser + +DeviceInfoParser::DeviceInfoParser() { +} + +DeviceInfoParser::~DeviceInfoParser() { +} + +std::size_t DeviceInfoParser::GetFromData( + const std::uint8_t *data, device_info_t *info) const { + std::size_t i = 4; // skip vid, pid + // name, 16 + info->name = bytes::_from_data(data + i, 16); + i += 16; + // serial_number, 16 + info->serial_number = bytes::_from_data(data + i, 16); + i += 16; + // firmware_version, 2 + info->firmware_version.set_major(data[i]); + info->firmware_version.set_minor(data[i + 1]); + i += 2; + // hardware_version, 3 + info->hardware_version.set_major(data[i]); + info->hardware_version.set_minor(data[i + 1]); + info->hardware_version.set_flag(std::bitset<8>(data[i + 2])); + i += 3; + // spec_version, 2 + info->spec_version.set_major(data[i]); + info->spec_version.set_minor(data[i + 1]); + i += 2; + // lens_type, 4 + info->lens_type.set_vendor(bytes::_from_data(data + i)); + info->lens_type.set_product(bytes::_from_data(data + i + 2)); + i += 4; + // imu_type, 4 + info->imu_type.set_vendor(bytes::_from_data(data + i)); + info->imu_type.set_product(bytes::_from_data(data + i + 2)); + i += 4; + // nominal_baseline, 2 + info->nominal_baseline = bytes::_from_data(data + i); + i += 2; + + // get other infos according to spec_version + + return i; +} + +std::size_t DeviceInfoParser::SetToData( + const device_info_t *info, std::uint8_t *data) const { + std::size_t i = 3; // skip id, size + i += 4; // skip vid, pid + // name, 16 + bytes::_to_data(info->name, data + i, 16); + i += 16; + // serial_number, 16 + bytes::_to_data(info->serial_number, data + i, 16); + i += 16; + // firmware_version, 2 + data[i] = info->firmware_version.major(); + data[i + 1] = info->firmware_version.minor(); + i += 2; + // hardware_version, 3 + data[i] = info->hardware_version.major(); + data[i + 1] = info->hardware_version.minor(); + data[i + 2] = + static_cast(info->hardware_version.flag().to_ulong()); + i += 3; + // spec_version, 2 + data[i] = info->spec_version.major(); + data[i + 1] = info->spec_version.minor(); + i += 2; + // lens_type, 4 + bytes::_to_data(info->lens_type.vendor(), data + i); + bytes::_to_data(info->lens_type.product(), data + i + 2); + i += 4; + // imu_type, 4 + bytes::_to_data(info->imu_type.vendor(), data + i); + bytes::_to_data(info->imu_type.product(), data + i + 2); + i += 4; + // nominal_baseline, 2 + bytes::_to_data(info->nominal_baseline, data + i); + i += 2; + + // set other infos according to spec_version + + // others + std::size_t size = i - 3; + data[0] = FID_DEVICE_INFO; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +// ImgParamsParser + +ImgParamsParser::ImgParamsParser() { +} + +ImgParamsParser::~ImgParamsParser() { +} + +std::size_t ImgParamsParser::GetFromData( + const std::uint8_t *data, img_params_t *img_params) const { + if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) { + // get img params without version header + if (spec_version_ == Version(1, 0)) { + return GetFromData_v1_0(data, img_params); + } else { + return GetFromData_v1_1(data, img_params); + } + } else { + // get img params with version header + return GetFromData_new(data, img_params); + } +} + +std::size_t ImgParamsParser::SetToData( + const img_params_t *img_params, std::uint8_t *data) const { + if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) { + // set img params without version header + if (spec_version_ == Version(1, 0)) { + return SetToData_v1_0(img_params, data); + } else { + return SetToData_v1_1(img_params, data); + } + } else { + // set img params with version header + return SetToData_new(img_params, data); + } +} + +std::size_t ImgParamsParser::GetFromData_v1_0( + const std::uint8_t *data, img_params_t *img_params) const { + std::size_t i = 0; + + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + Extrinsics ex_right_to_left; + + i += bytes::from_data(in_left.get(), data + i); + i += bytes::from_data(in_right.get(), data + i); + i += bytes::from_data(&ex_right_to_left, data + i); + (*img_params)[{752, 480}] = {true, spec_version_.to_string(), + in_left, in_right, ex_right_to_left}; + + return i; +} + +std::size_t ImgParamsParser::SetToData_v1_0( + const img_params_t *img_params, std::uint8_t *data) const { + std::size_t i = 3; // skip id, size + + auto params = (*img_params).at({752, 480}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast(params.in_right); + i += bytes::to_data(in_left.get(), data + i); + i += bytes::to_data(in_right.get(), data + i); + i += bytes::to_data(¶ms.ex_right_to_left, data + i); + + // others + std::size_t size = i - 3; + data[0] = FID_IMG_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +std::size_t ImgParamsParser::GetFromData_v1_1( + const std::uint8_t *data, img_params_t *img_params) const { + std::size_t i = 0; + + Extrinsics ex_right_to_left; + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i); + i += bytes::from_data(in_right.get(), data + i); + (*img_params)[{1280, 400}] = {true, spec_version_.to_string(), + in_left, in_right, ex_right_to_left}; + } + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i); + i += bytes::from_data(in_right.get(), data + i); + (*img_params)[{2560, 800}] = {true, spec_version_.to_string(), + in_left, in_right, ex_right_to_left}; + } + { + i += bytes::from_data(&ex_right_to_left, data + i); + (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; + (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; + } + + return i; +} + +std::size_t ImgParamsParser::SetToData_v1_1( + const img_params_t *img_params, std::uint8_t *data) const { + std::size_t i = 3; // skip id, size + + { + auto params = (*img_params).at({1280, 400}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i); + i += bytes::to_data(in_right.get(), data + i); + } + { + auto params = (*img_params).at({2560, 800}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i); + i += bytes::to_data(in_right.get(), data + i); + i += bytes::to_data(¶ms.ex_right_to_left, data + i); + } + + // others + std::size_t size = i - 3; + data[0] = FID_IMG_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +std::size_t ImgParamsParser::GetFromData_new( + const std::uint8_t *data, img_params_t *img_params) const { + return 0; +} +std::size_t ImgParamsParser::SetToData_new( + const img_params_t *img_params, std::uint8_t *data) const { + return 0; +} + +// ImuParamsParser + +ImuParamsParser::ImuParamsParser() { +} + +ImuParamsParser::~ImuParamsParser() { +} + +std::size_t ImuParamsParser::GetFromData( + const std::uint8_t *data, imu_params_t *imu_params) const { + if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) { + // get imu params without version header + return GetFromData_old(data, imu_params); + } else { + // get imu params with version header + return GetFromData_new(data, imu_params); + } +} + +std::size_t ImuParamsParser::SetToData( + const imu_params_t *imu_params, std::uint8_t *data) const { + if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) { + // set imu params without version header + return SetToData_old(imu_params, data); + } else { + // set imu params with version header + return SetToData_new(imu_params, data); + } +} + +std::size_t ImuParamsParser::GetFromData_old( + const std::uint8_t *data, imu_params_t *imu_params) const { + std::size_t i = 0; + i += bytes::from_data(&imu_params->in_accel, data + i); + i += bytes::from_data(&imu_params->in_gyro, data + i); + i += bytes::from_data(&imu_params->ex_left_to_imu, data + i); + return i; +} + +std::size_t ImuParamsParser::SetToData_old( + const imu_params_t *imu_params, std::uint8_t *data) const { + std::size_t i = 3; // skip id, size + i += bytes::to_data(&imu_params->in_accel, data + i); + i += bytes::to_data(&imu_params->in_gyro, data + i); + i += bytes::to_data(&imu_params->ex_left_to_imu, data + i); + // others + std::size_t size = i - 3; + data[0] = FID_IMU_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +std::size_t ImuParamsParser::GetFromData_new( + const std::uint8_t *data, imu_params_t *imu_params) const { + std::size_t i = 0; + // version, 2 + Version version(data[i], data[i + 1]); + imu_params->version = version.to_string(); + i += 2; + // get imu params according to version + if (version == Version(1, 2)) { // v1.2 + i += bytes::from_data(&imu_params->in_accel, data + i); + i += bytes::from_data(&imu_params->in_gyro, data + i); + i += bytes::from_data(&imu_params->ex_left_to_imu, data + i); + } else { + LOG(FATAL) << "Could not get imu params of version " + << version.to_string() << ", please use latest SDK."; + } + return i; +} + +std::size_t ImuParamsParser::SetToData_new( + const imu_params_t *imu_params, std::uint8_t *data) const { + std::size_t i = 3; // skip id, size + // version, 2 + Version version(imu_params->version); + data[i] = version.major(); + data[i + 1] = version.minor(); + i += 2; + // set imu params according to version + if (version == Version(1, 2)) { // v1.2 + i += bytes::to_data(&imu_params->in_accel, data + i); + i += bytes::to_data(&imu_params->in_gyro, data + i); + i += bytes::to_data(&imu_params->ex_left_to_imu, data + i); + } else { + LOG(FATAL) << "Could not set imu params of version " + << version.to_string() << ", please use latest SDK."; + } + // others + std::size_t size = i - 3; + data[0] = FID_IMU_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/channel/file_channel.h b/src/mynteye/device/channel/file_channel.h new file mode 100644 index 0000000..2058124 --- /dev/null +++ b/src/mynteye/device/channel/file_channel.h @@ -0,0 +1,142 @@ +// 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_DEVICE_CHANNEL_FILE_CHANNEL_H_ +#define MYNTEYE_DEVICE_CHANNEL_FILE_CHANNEL_H_ +#pragma once + +#include +#include + +#include "mynteye/mynteye.h" +#include "mynteye/device/device.h" +#include "mynteye/device/channel/bytes.h" + +MYNTEYE_BEGIN_NAMESPACE + +class DeviceInfoParser; +class ImgParamsParser; +class ImuParamsParser; + +class FileChannel { + public: + using device_info_t = DeviceInfo; + + using img_params_t = std::map; + using imu_params_t = device::imu_params_t; + + FileChannel(); + ~FileChannel(); + + std::size_t GetDeviceInfoFromData( + const std::uint8_t *data, device_info_t *info); + std::size_t SetDeviceInfoToData( + const device_info_t *info, std::uint8_t *data); + + std::size_t GetImgParamsFromData( + const std::uint8_t *data, img_params_t *img_params); + std::size_t SetImgParamsToData( + const img_params_t *img_params, std::uint8_t *data); + + std::size_t GetImuParamsFromData( + const std::uint8_t *data, imu_params_t *imu_params); + std::size_t SetImuParamsToData( + const imu_params_t *imu_params, std::uint8_t *data); + + private: + std::shared_ptr dev_info_parser_; + std::shared_ptr img_params_parser_; + std::shared_ptr imu_params_parser_; +}; + +class DeviceInfoParser { + public: + using device_info_t = FileChannel::device_info_t; + + DeviceInfoParser(); + ~DeviceInfoParser(); + + std::size_t GetFromData( + const std::uint8_t *data, device_info_t *info) const; + std::size_t SetToData( + const device_info_t *info, std::uint8_t *data) const; +}; + +class ImgParamsParser { + public: + using img_params_t = FileChannel::img_params_t; + + ImgParamsParser(); + ~ImgParamsParser(); + + void SetSpecVersion(const Version& spec_version) { + spec_version_ = spec_version; + } + + std::size_t GetFromData( + const std::uint8_t *data, img_params_t *img_params) const; + std::size_t SetToData( + const img_params_t *img_params, std::uint8_t *data) const; + + std::size_t GetFromData_v1_0( + const std::uint8_t *data, img_params_t *img_params) const; + std::size_t SetToData_v1_0( + const img_params_t *img_params, std::uint8_t *data) const; + + std::size_t GetFromData_v1_1( + const std::uint8_t *data, img_params_t *img_params) const; + std::size_t SetToData_v1_1( + const img_params_t *img_params, std::uint8_t *data) const; + + std::size_t GetFromData_new( + const std::uint8_t *data, img_params_t *img_params) const; + std::size_t SetToData_new( + const img_params_t *img_params, std::uint8_t *data) const; + + private: + Version spec_version_; +}; + +class ImuParamsParser { + public: + using imu_params_t = FileChannel::imu_params_t; + + ImuParamsParser(); + ~ImuParamsParser(); + + void SetSpecVersion(const Version& spec_version) { + spec_version_ = spec_version; + } + + std::size_t GetFromData( + const std::uint8_t *data, imu_params_t *imu_params) const; + std::size_t SetToData( + const imu_params_t *imu_params, std::uint8_t *data) const; + + std::size_t GetFromData_old( + const std::uint8_t *data, imu_params_t *imu_params) const; + std::size_t SetToData_old( + const imu_params_t *imu_params, std::uint8_t *data) const; + + std::size_t GetFromData_new( + const std::uint8_t *data, imu_params_t *imu_params) const; + std::size_t SetToData_new( + const imu_params_t *imu_params, std::uint8_t *data) const; + + private: + Version spec_version_; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_DEVICE_CHANNEL_FILE_CHANNEL_H_ diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index beb58a2..b1a6295 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -21,7 +21,7 @@ #include "mynteye/logger.h" #include "mynteye/device/async_callback.h" -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" #include "mynteye/device/config.h" #include "mynteye/device/motions.h" #include "mynteye/device/standard/device_s.h" diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index d5a0719..71c40ab 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -14,7 +14,7 @@ #include "mynteye/device/motions.h" #include "mynteye/logger.h" -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" MYNTEYE_BEGIN_NAMESPACE diff --git a/src/mynteye/device/standard/channels_adapter_s.cc b/src/mynteye/device/standard/channels_adapter_s.cc index ea013c9..a2fc0ee 100644 --- a/src/mynteye/device/standard/channels_adapter_s.cc +++ b/src/mynteye/device/standard/channels_adapter_s.cc @@ -126,41 +126,4 @@ void StandardChannelsAdapter::GetImuResPacket( unpack_imu_res_packet(data, res); } -std::size_t StandardChannelsAdapter::GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) { - std::size_t i = 0; - - auto in_left = std::make_shared(); - auto in_right = std::make_shared(); - Extrinsics ex_right_to_left; - - i += bytes::from_data(in_left.get(), data + i, version); - i += bytes::from_data(in_right.get(), data + i, version); - i += bytes::from_data(&ex_right_to_left, data + i, version); - (*img_params)[{752, 480}] = {true, in_left, in_right, ex_right_to_left}; - - return i; -} - -std::size_t StandardChannelsAdapter::SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) { - std::size_t i = 3; // skip id, size - - auto params = (*img_params).at({752, 480}); - auto in_left = std::dynamic_pointer_cast(params.in_left); - auto in_right = std::dynamic_pointer_cast(params.in_right); - i += bytes::to_data(in_left.get(), data + i, version); - i += bytes::to_data(in_right.get(), data + i, version); - i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); - - // others - std::size_t size = i - 3; - data[0] = Channels::FID_IMG_PARAMS; - data[1] = static_cast((size >> 8) & 0xFF); - data[2] = static_cast(size & 0xFF); - return size + 3; -} - MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard/channels_adapter_s.h b/src/mynteye/device/standard/channels_adapter_s.h index b768c22..54205a3 100644 --- a/src/mynteye/device/standard/channels_adapter_s.h +++ b/src/mynteye/device/standard/channels_adapter_s.h @@ -19,7 +19,7 @@ #include #include -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" MYNTEYE_BEGIN_NAMESPACE @@ -37,13 +37,6 @@ class StandardChannelsAdapter : public ChannelsAdapter { std::vector GetGyroRangeValues() override; void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override; - - std::size_t GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) override; - std::size_t SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) override; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc index dc91636..3257039 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.cc +++ b/src/mynteye/device/standard2/channels_adapter_s2.cc @@ -122,64 +122,4 @@ void Standard2ChannelsAdapter::GetImuResPacket( unpack_imu_res_packet(data, res); } -std::size_t Standard2ChannelsAdapter::GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) { - std::size_t i = 0; - - Extrinsics ex_right_to_left; - { - auto in_left = std::make_shared(); - auto in_right = std::make_shared(); - i += bytes::from_data(in_left.get(), data + i, version); - i += bytes::from_data(in_right.get(), data + i, version); - (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; - } - { - auto in_left = std::make_shared(); - auto in_right = std::make_shared(); - i += bytes::from_data(in_left.get(), data + i, version); - i += bytes::from_data(in_right.get(), data + i, version); - (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; - } - { - i += bytes::from_data(&ex_right_to_left, data + i, version); - (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; - (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; - } - - return i; -} - -std::size_t Standard2ChannelsAdapter::SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) { - std::size_t i = 3; // skip id, size - - { - auto params = (*img_params).at({1280, 400}); - auto in_left = std::dynamic_pointer_cast(params.in_left); - auto in_right = std::dynamic_pointer_cast( - params.in_right); - i += bytes::to_data(in_left.get(), data + i, version); - i += bytes::to_data(in_right.get(), data + i, version); - } - { - auto params = (*img_params).at({2560, 800}); - auto in_left = std::dynamic_pointer_cast(params.in_left); - auto in_right = std::dynamic_pointer_cast( - params.in_right); - i += bytes::to_data(in_left.get(), data + i, version); - i += bytes::to_data(in_right.get(), data + i, version); - i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); - } - - // others - std::size_t size = i - 3; - data[0] = Channels::FID_IMG_PARAMS; - data[1] = static_cast((size >> 8) & 0xFF); - data[2] = static_cast(size & 0xFF); - return size + 3; -} - MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s2.h b/src/mynteye/device/standard2/channels_adapter_s2.h index d9a5e1a..375ba3e 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.h +++ b/src/mynteye/device/standard2/channels_adapter_s2.h @@ -19,7 +19,7 @@ #include #include -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" MYNTEYE_BEGIN_NAMESPACE @@ -37,13 +37,6 @@ class Standard2ChannelsAdapter : public ChannelsAdapter { std::vector GetGyroRangeValues() override; void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override; - - std::size_t GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) override; - std::size_t SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) override; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s210a.cc b/src/mynteye/device/standard2/channels_adapter_s210a.cc index 6283c8a..e2ff9af 100644 --- a/src/mynteye/device/standard2/channels_adapter_s210a.cc +++ b/src/mynteye/device/standard2/channels_adapter_s210a.cc @@ -122,64 +122,4 @@ void Standard210aChannelsAdapter::GetImuResPacket( unpack_imu_res_packet(data, res); } -std::size_t Standard210aChannelsAdapter::GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) { - std::size_t i = 0; - - Extrinsics ex_right_to_left; - { - auto in_left = std::make_shared(); - auto in_right = std::make_shared(); - i += bytes::from_data(in_left.get(), data + i, version); - i += bytes::from_data(in_right.get(), data + i, version); - (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; - } - { - auto in_left = std::make_shared(); - auto in_right = std::make_shared(); - i += bytes::from_data(in_left.get(), data + i, version); - i += bytes::from_data(in_right.get(), data + i, version); - (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; - } - { - i += bytes::from_data(&ex_right_to_left, data + i, version); - (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; - (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; - } - - return i; -} - -std::size_t Standard210aChannelsAdapter::SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) { - std::size_t i = 3; // skip id, size - - { - auto params = (*img_params).at({1280, 400}); - auto in_left = std::dynamic_pointer_cast(params.in_left); - auto in_right = std::dynamic_pointer_cast( - params.in_right); - i += bytes::to_data(in_left.get(), data + i, version); - i += bytes::to_data(in_right.get(), data + i, version); - } - { - auto params = (*img_params).at({2560, 800}); - auto in_left = std::dynamic_pointer_cast(params.in_left); - auto in_right = std::dynamic_pointer_cast( - params.in_right); - i += bytes::to_data(in_left.get(), data + i, version); - i += bytes::to_data(in_right.get(), data + i, version); - i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); - } - - // others - std::size_t size = i - 3; - data[0] = Channels::FID_IMG_PARAMS; - data[1] = static_cast((size >> 8) & 0xFF); - data[2] = static_cast(size & 0xFF); - return size + 3; -} - MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/standard2/channels_adapter_s210a.h b/src/mynteye/device/standard2/channels_adapter_s210a.h index 0de7d28..b63e4dc 100644 --- a/src/mynteye/device/standard2/channels_adapter_s210a.h +++ b/src/mynteye/device/standard2/channels_adapter_s210a.h @@ -19,7 +19,7 @@ #include #include -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" MYNTEYE_BEGIN_NAMESPACE @@ -37,13 +37,6 @@ class Standard210aChannelsAdapter : public ChannelsAdapter { std::vector GetGyroRangeValues() override; void GetImuResPacket(const std::uint8_t *data, ImuResPacket *res) override; - - std::size_t GetImgParamsFromData( - const std::uint8_t *data, const Version *version, - Channels::img_params_t *img_params) override; - std::size_t SetImgParamsToData( - const Channels::img_params_t *img_params, const Version *version, - std::uint8_t *data) override; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/types.h b/src/mynteye/device/types.h index 44efaf5..41b372a 100644 --- a/src/mynteye/device/types.h +++ b/src/mynteye/device/types.h @@ -51,6 +51,10 @@ class MYNTEYE_API Version { : major_(parse_part(name, 0)), minor_(parse_part(name, 1)) {} virtual ~Version() {} + bool empty() const { + return major_ == 0 && minor_ == 0; + } + bool operator==(const Version &other) const { return major_ == other.major_ && minor_ == other.minor_; } diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index 9352c3c..a609222 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -232,7 +232,9 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) { auto &&m_in = device_->GetMotionIntrinsics(); SaveImuParams( { - false, m_in.accel, m_in.gyro, + false, + device_->GetInfo()->spec_version.to_string(), + m_in.accel, m_in.gyro, device_->GetMotionExtrinsics(Stream::LEFT), }, dir + MYNTEYE_OS_SEP "imu.params"); diff --git a/tools/writer/device_writer.h b/tools/writer/device_writer.h index ac4b2a5..de942fc 100644 --- a/tools/writer/device_writer.h +++ b/tools/writer/device_writer.h @@ -20,7 +20,7 @@ #include #include "mynteye/mynteye.h" -#include "mynteye/device/channels/channels.h" +#include "mynteye/device/channel/channels.h" #include "mynteye/device/types.h" MYNTEYE_BEGIN_NAMESPACE