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

refactor(channels): sepreate bytes and file channel

Browse Source
This commit is contained in:
John Zhao 2019-01-05 20:02:08 +08:00
parent ca51d2cf94
commit e539cb9fe0
21 changed files with 940 additions and 624 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

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

14
include/mynteye/device/callbacks.h
View File

@ -30,20 +30,6 @@ MYNTEYE_BEGIN_NAMESPACE
namespace device {
typedef struct ImgParams {
bool ok;
std::shared_ptr<IntrinsicsBase> in_left;
std::shared_ptr<IntrinsicsBase> 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.

20
include/mynteye/device/device.h
View File

@ -51,6 +51,26 @@ class StreamsAdapter;
template <class Data>
class AsyncCallback;
namespace device {
typedef struct ImgParams {
bool ok;
std::string version;
std::shared_ptr<IntrinsicsBase> in_left;
std::shared_ptr<IntrinsicsBase> 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.
*/

208
src/mynteye/device/channel/bytes.cc Normal file
View File

@ -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<const char *>(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<std::uint16_t>(data + i);
i += 2;
// height, 2
in->height = _from_data<std::uint16_t>(data + i);
i += 2;
// fx, 8
in->fx = _from_data<double>(data + i);
i += 8;
// fy, 8
in->fy = _from_data<double>(data + i);
i += 8;
// cx, 8
in->cx = _from_data<double>(data + i);
i += 8;
// cy, 8
in->cy = _from_data<double>(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<double>(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<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
// drift
for (std::size_t j = 0; j < 3; j++) {
in->drift[j] = _from_data<double>(data + i + j * 8);
}
i += 24;
// noise
for (std::size_t j = 0; j < 3; j++) {
in->noise[j] = _from_data<double>(data + i + j * 8);
}
i += 24;
// bias
for (std::size_t j = 0; j < 3; j++) {
in->bias[j] = _from_data<double>(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<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
// translation
for (std::size_t j = 0; j < 3; j++) {
ex->translation[j] = _from_data<double>(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

89
src/mynteye/device/channel/bytes.h Normal file
View File

@ -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 <algorithm>
#include <string>
#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 <typename T>
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<const double *>(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 <typename T>
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<std::uint8_t>((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<std::uint8_t *>(&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_

326
src/mynteye/device/channels/channels.cc → src/mynteye/device/channel/channels.cc
View File

@ -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 <bitset>
#include <chrono>
@ -19,10 +19,10 @@
#include <iterator>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#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<std::uint8_t>(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<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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<const char *>(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<std::uint16_t>(data + i));
info->lens_type.set_product(_from_data<std::uint16_t>(data + i + 2));
i += 4;
// imu_type, 4
info->imu_type.set_vendor(_from_data<std::uint16_t>(data + i));
info->imu_type.set_product(_from_data<std::uint16_t>(data + i + 2));
i += 4;
// nominal_baseline, 2
info->nominal_baseline = _from_data<std::uint16_t>(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<std::uint16_t>(data + i);
i += 2;
// height, 2
in->height = _from_data<std::uint16_t>(data + i);
i += 2;
// fx, 8
in->fx = _from_data<double>(data + i);
i += 8;
// fy, 8
in->fy = _from_data<double>(data + i);
i += 8;
// cx, 8
in->cx = _from_data<double>(data + i);
i += 8;
// cy, 8
in->cy = _from_data<double>(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<double>(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<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
// drift
for (std::size_t j = 0; j < 3; j++) {
in->drift[j] = _from_data<double>(data + i + j * 8);
}
i += 24;
// noise
for (std::size_t j = 0; j < 3; j++) {
in->noise[j] = _from_data<double>(data + i + j * 8);
}
i += 24;
// bias
for (std::size_t j = 0; j < 3; j++) {
in->bias[j] = _from_data<double>(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<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
// translation
for (std::size_t j = 0; j < 3; j++) {
ex->translation[j] = _from_data<double>(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<std::uint8_t>(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<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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

118
src/mynteye/device/channels/channels.h → src/mynteye/device/channel/channels.h
View File

@ -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 <algorithm>
#include <map>
#include <memory>
#include <set>
#include <string>
#include <thread>
#include <vector>
#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<void(const ImuPacket &packet)>;
using device_info_t = DeviceInfo;
using img_params_t = std::map<Resolution, device::img_params_t>;
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<uvc::device> &device,
const std::shared_ptr<ChannelsAdapter> &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<uvc::device> device_;
std::shared_ptr<ChannelsAdapter> adapter_;
FileChannel file_channel_;
std::map<Option, control_info_t> control_infos_;
bool is_imu_tracking_;
@ -165,88 +149,8 @@ class ChannelsAdapter {
virtual std::vector<std::int32_t> 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 <typename T>
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<const double *>(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 <typename T>
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<std::uint8_t>((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<std::uint8_t *>(&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_

40
src/mynteye/device/channel/def.h Normal file
View File

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

403
src/mynteye/device/channel/file_channel.cc Normal file
View File

@ -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<DeviceInfoParser>();
img_params_parser_ = std::make_shared<ImgParamsParser>();
imu_params_parser_ = std::make_shared<ImuParamsParser>();
}
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<std::uint16_t>(data + i));
info->lens_type.set_product(bytes::_from_data<std::uint16_t>(data + i + 2));
i += 4;
// imu_type, 4
info->imu_type.set_vendor(bytes::_from_data<std::uint16_t>(data + i));
info->imu_type.set_product(bytes::_from_data<std::uint16_t>(data + i + 2));
i += 4;
// nominal_baseline, 2
info->nominal_baseline = bytes::_from_data<std::uint16_t>(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<std::uint8_t>(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<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(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(&params.ex_right_to_left, data + i);
// others
std::size_t size = i - 3;
data[0] = FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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(&params.ex_right_to_left, data + i);
}
// others
std::size_t size = i - 3;
data[0] = FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(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<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

142
src/mynteye/device/channel/file_channel.h Normal file
View File

@ -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 <map>
#include <memory>
#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<Resolution, device::img_params_t>;
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<DeviceInfoParser> dev_info_parser_;
std::shared_ptr<ImgParamsParser> img_params_parser_;
std::shared_ptr<ImuParamsParser> 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_

2
src/mynteye/device/device.cc
View File

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

2
src/mynteye/device/motions.cc
View File

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

37
src/mynteye/device/standard/channels_adapter_s.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(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(&params.ex_right_to_left, data + i, version);
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

9
src/mynteye/device/standard/channels_adapter_s.h
View File

@ -19,7 +19,7 @@
#include <set>
#include <vector>
#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<std::int32_t> 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

60
src/mynteye/device/standard2/channels_adapter_s2.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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(&params.ex_right_to_left, data + i, version);
}
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

9
src/mynteye/device/standard2/channels_adapter_s2.h
View File

@ -19,7 +19,7 @@
#include <set>
#include <vector>
#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<std::int32_t> 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

60
src/mynteye/device/standard2/channels_adapter_s210a.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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(&params.ex_right_to_left, data + i, version);
}
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
MYNTEYE_END_NAMESPACE

9
src/mynteye/device/standard2/channels_adapter_s210a.h
View File

@ -19,7 +19,7 @@
#include <set>
#include <vector>
#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<std::int32_t> 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

4
src/mynteye/device/types.h
View File

@ -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_;
}

4
tools/writer/device_writer.cc
View File

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

2
tools/writer/device_writer.h
View File

@ -20,7 +20,7 @@
#include <string>
#include "mynteye/mynteye.h"
#include "mynteye/device/channels/channels.h"
#include "mynteye/device/channel/channels.h"
#include "mynteye/device/types.h"
MYNTEYE_BEGIN_NAMESPACE