From c580f6e8dca7b8c50444ae331b8aa14a43887050 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 5 Jan 2019 22:04:59 +0800 Subject: [PATCH] feat(channels): add img params new parser --- include/mynteye/types.h | 37 ++-- src/mynteye/api/api.cc | 2 +- .../api/processor/rectify_processor.cc | 4 +- src/mynteye/device/channel/bytes.cc | 105 ++++++++-- src/mynteye/device/channel/bytes.h | 14 +- src/mynteye/device/channel/channels.cc | 69 +++---- src/mynteye/device/channel/channels.h | 6 +- src/mynteye/device/channel/file_channel.cc | 179 ++++++++++++++---- src/mynteye/device/channel/file_channel.h | 33 ++-- src/mynteye/types.cc | 13 ++ tools/writer/device_writer.cc | 6 +- 11 files changed, 329 insertions(+), 139 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 2517447..ddb63b7 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -400,22 +400,33 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request); * Camera calibration model. */ enum class CalibrationModel : std::uint8_t { - /** Unknow */ - CALIB_MODEL_UNKNOW = 0, /** Pinhole */ - CALIB_MODEL_PINHOLE = 1, + PINHOLE = 0, /** Equidistant: KANNALA_BRANDT */ - CALIB_MODEL_KANNALA_BRANDT = 2, - // CALIB_MODEL_SCARAMUZZA, - // CALIB_MODEL_MEI, + KANNALA_BRANDT = 1, + /** Unknow */ + UNKNOW }; +MYNTEYE_API const char *to_string(const CalibrationModel &model); + +inline std::ostream &operator<<(std::ostream &os, + const CalibrationModel &model) { + return os << to_string(model); +} + struct MYNTEYE_API IntrinsicsBase { IntrinsicsBase() { - calib_model = CalibrationModel::CALIB_MODEL_UNKNOW; + calib_model = CalibrationModel::UNKNOW; } virtual ~IntrinsicsBase() {} + + /** The calibration model */ CalibrationModel calib_model; + /** The width of the image in pixels */ + std::uint16_t width; + /** The height of the image in pixels */ + std::uint16_t height; }; /** @@ -424,12 +435,8 @@ struct MYNTEYE_API IntrinsicsBase { */ struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase { IntrinsicsPinhole() { - calib_model = CalibrationModel::CALIB_MODEL_PINHOLE; + calib_model = CalibrationModel::PINHOLE; } - /** The width of the image in pixels */ - std::uint16_t width; - /** The height of the image in pixels */ - std::uint16_t height; /** The focal length of the image plane, as a multiple of pixel width */ double fx; /** The focal length of the image plane, as a multiple of pixel height */ @@ -458,12 +465,8 @@ using Intrinsics = IntrinsicsPinhole; */ struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase { IntrinsicsEquidistant() { - calib_model = CalibrationModel::CALIB_MODEL_KANNALA_BRANDT; + calib_model = CalibrationModel::KANNALA_BRANDT; } - /** The width of the image in pixels */ - std::uint16_t width; - /** The height of the image in pixels */ - std::uint16_t height; /** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */ double coeffs[8]; }; diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index f030e45..e6986f6 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -296,7 +296,7 @@ std::string API::GetInfo(const Info &info) const { IntrinsicsPinhole API::GetIntrinsics(const Stream &stream) const { auto in = GetIntrinsicsBase(stream); - if (in->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) { + if (in->calib_model == CalibrationModel::PINHOLE) { return *std::dynamic_pointer_cast(in); } throw std::runtime_error("Intrinsics is not pinhole model" diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index aa36559..334575a 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -43,13 +43,13 @@ std::string RectifyProcessor::Name() { void RectifyProcessor::NotifyImageParamsChanged() { auto in_left = device_->GetIntrinsics(Stream::LEFT); auto in_right = device_->GetIntrinsics(Stream::RIGHT); - if (in_left->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) { + if (in_left->calib_model == CalibrationModel::PINHOLE) { InitParams( *std::dynamic_pointer_cast(in_left), *std::dynamic_pointer_cast(in_right), device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); } else if (in_left->calib_model == - CalibrationModel::CALIB_MODEL_KANNALA_BRANDT) { + CalibrationModel::KANNALA_BRANDT) { InitParams( *std::dynamic_pointer_cast(in_left), *std::dynamic_pointer_cast(in_right), diff --git a/src/mynteye/device/channel/bytes.cc b/src/mynteye/device/channel/bytes.cc index d08164a..b84c66f 100644 --- a/src/mynteye/device/channel/bytes.cc +++ b/src/mynteye/device/channel/bytes.cc @@ -13,6 +13,7 @@ // limitations under the License. #include "mynteye/device/channel/bytes.h" +#include "mynteye/logger.h" #include "mynteye/util/strings.h" MYNTEYE_BEGIN_NAMESPACE @@ -29,15 +30,32 @@ 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(IntrinsicsBase *in, const std::uint8_t *data, + bool get_size) { + switch (in->calib_model) { + case CalibrationModel::PINHOLE: + return from_data(dynamic_cast(in), data, + get_size); + case CalibrationModel::KANNALA_BRANDT: + return from_data(dynamic_cast(in), data, + get_size); + default: + LOG(FATAL) << "Unknown calib model: " << in->calib_model; + } +} + +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, + bool get_size) { 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; + if (get_size) { + // 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; @@ -62,6 +80,27 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) { return i; } +std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data, + bool get_size) { + std::size_t i = 0; + + if (get_size) { + // width, 2 + in->width = _from_data(data + i); + i += 2; + // height, 2 + in->height = _from_data(data + i); + i += 2; + } + // coeffs, 64 + for (std::size_t j = 0; j < 8; j++) { + in->coeffs[j] = _from_data(data + i + j * 8); + } + i += 64; + + return i; +} + std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) { std::size_t i = 0; @@ -122,15 +161,32 @@ 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 IntrinsicsBase *in, std::uint8_t *data, + bool set_size) { + switch (in->calib_model) { + case CalibrationModel::PINHOLE: + return to_data(dynamic_cast(in), data, + set_size); + case CalibrationModel::KANNALA_BRANDT: + return to_data(dynamic_cast(in), data, + set_size); + default: + LOG(FATAL) << "Unknown calib model: " << in->calib_model; + } +} + +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, + bool set_size) { 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; + if (set_size) { + // 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; @@ -155,6 +211,27 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) { return i; } +std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data, + bool set_size) { + std::size_t i = 0; + + if (set_size) { + // width, 2 + _to_data(in->width, data + i); + i += 2; + // height, 2 + _to_data(in->height, data + i); + i += 2; + } + // coeffs, 64 + for (std::size_t j = 0; j < 8; j++) { + _to_data(in->coeffs[j], data + i + j * 8); + } + i += 64; + + return i; +} + std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) { std::size_t i = 0; diff --git a/src/mynteye/device/channel/bytes.h b/src/mynteye/device/channel/bytes.h index 5014e05..01bafdc 100644 --- a/src/mynteye/device/channel/bytes.h +++ b/src/mynteye/device/channel/bytes.h @@ -48,7 +48,12 @@ 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(IntrinsicsBase *in, const std::uint8_t *data, + bool get_size); +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, + bool get_size); +std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data, + bool get_size); std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data); @@ -76,7 +81,12 @@ 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 IntrinsicsBase *in, std::uint8_t *data, + bool set_size); +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, + bool set_size); +std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data, + bool set_size); std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data); diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index f29ccf0..7b70e9d 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -97,26 +97,6 @@ int XuHalfDuplexId(Option option) { } } -void CheckSpecVersion(const Version *spec_version) { - if (spec_version == nullptr) { - LOG(FATAL) << "Spec version must be specified"; - } - - std::vector spec_versions{"1.0", "1.1"}; - for (auto &&spec_ver : spec_versions) { - if (*spec_version == Version(spec_ver)) { - return; // supported - } - } - - std::ostringstream ss; - std::copy( - spec_versions.begin(), spec_versions.end(), - std::ostream_iterator(ss, ",")); - LOG(FATAL) << "Spec version " << spec_version->to_string() - << " not supported, must in [" << ss.str() << "]"; -} - } // namespace Channels::Channels(const std::shared_ptr &device, @@ -431,8 +411,7 @@ void Channels::StopImuTracking() { } bool Channels::GetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version) { + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) { if (info == nullptr && img_params == nullptr && imu_params == nullptr) { LOG(WARNING) << "Files are not provided to get"; return false; @@ -475,7 +454,6 @@ bool Channels::GetFiles( return false; } - Version *spec_ver = spec_version; std::size_t i = 3; std::size_t end = 3 + size; while (i < end) { @@ -486,25 +464,24 @@ bool Channels::GetFiles( i += 3; switch (file_id) { case FID_DEVICE_INFO: { - auto &&n = file_channel_.GetDeviceInfoFromData(data + i, info); + auto &&n = file_channel_.GetDeviceInfoFromData( + data + i, file_size, info); CHECK_EQ(n, file_size) << "The firmware not support getting device info, you could " "upgrade to latest"; - spec_ver = &info->spec_version; - CheckSpecVersion(spec_ver); } break; case FID_IMG_PARAMS: { if (file_size > 0) { - CheckSpecVersion(spec_ver); - auto &&n = file_channel_.GetImgParamsFromData(data + i, img_params); + auto &&n = file_channel_.GetImgParamsFromData( + data + i, file_size, img_params); CHECK_EQ(n, file_size); } } break; case FID_IMU_PARAMS: { imu_params->ok = file_size > 0; if (imu_params->ok) { - CheckSpecVersion(spec_ver); - auto &&n = file_channel_.GetImuParamsFromData(data + i, imu_params); + auto &&n = file_channel_.GetImuParamsFromData( + data + i, file_size, imu_params); CHECK_EQ(n, file_size); } } break; @@ -523,17 +500,11 @@ bool Channels::GetFiles( } bool Channels::SetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version) { + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) { if (info == nullptr && img_params == nullptr && imu_params == nullptr) { LOG(WARNING) << "Files are not provided to set"; return false; } - Version *spec_ver = spec_version; - if (spec_ver == nullptr && info != nullptr) { - spec_ver = &info->spec_version; - } - CheckSpecVersion(spec_ver); std::uint8_t data[2000]{}; @@ -542,16 +513,28 @@ bool Channels::SetFiles( std::uint16_t size = 0; if (info != nullptr) { - header[0] = true; - size += file_channel_.SetDeviceInfoToData(info, data + 3 + size); + auto n = file_channel_.SetDeviceInfoToData(info, data + 3 + size); + if (n > 0) { + header[0] = true; + size += n; + } } if (img_params != nullptr) { - header[1] = true; - size += file_channel_.SetImgParamsToData(img_params, data + 3 + size); + auto n = file_channel_.SetImgParamsToData(img_params, data + 3 + size); + if (n > 0) { + header[1] = true; + size += n; + } } if (imu_params != nullptr) { - header[2] = true; - size += file_channel_.SetImuParamsToData(imu_params, data + 3 + size); + auto n = file_channel_.SetImuParamsToData(imu_params, data + 3 + size); + if (n > 0) { + header[2] = true; + size += n; + } + } + if (size + 3 > 2000) { + LOG(FATAL) << "SetFiles failed, data is too large: " << (size + 3); } data[0] = static_cast(header.to_ulong()); diff --git a/src/mynteye/device/channel/channels.h b/src/mynteye/device/channel/channels.h index 1ab453e..8b0f0de 100644 --- a/src/mynteye/device/channel/channels.h +++ b/src/mynteye/device/channel/channels.h @@ -82,11 +82,9 @@ class MYNTEYE_API Channels { void StopImuTracking(); bool GetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version = nullptr); + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params); bool SetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, - Version *spec_version = nullptr); + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params); private: bool PuControlRange( diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc index d3b4da1..f24df2c 100644 --- a/src/mynteye/device/channel/file_channel.cc +++ b/src/mynteye/device/channel/file_channel.cc @@ -29,8 +29,9 @@ FileChannel::~FileChannel() { } std::size_t FileChannel::GetDeviceInfoFromData( - const std::uint8_t *data, device_info_t *info) { - auto n = dev_info_parser_->GetFromData(data, info); + const std::uint8_t *data, const std::uint16_t &data_size, + device_info_t *info) { + auto n = dev_info_parser_->GetFromData(data, data_size, info); auto spec_version = info->spec_version; img_params_parser_->SetSpecVersion(spec_version); imu_params_parser_->SetSpecVersion(spec_version); @@ -46,9 +47,10 @@ std::size_t FileChannel::SetDeviceInfoToData( } std::size_t FileChannel::GetImgParamsFromData( - const std::uint8_t *data, img_params_t *img_params) { + const std::uint8_t *data, const std::uint16_t &data_size, + img_params_t *img_params) { CHECK_NOTNULL(img_params_parser_); - return img_params_parser_->GetFromData(data, img_params); + return img_params_parser_->GetFromData(data, data_size, img_params); } std::size_t FileChannel::SetImgParamsToData( @@ -58,8 +60,9 @@ std::size_t FileChannel::SetImgParamsToData( } std::size_t FileChannel::GetImuParamsFromData( - const std::uint8_t *data, imu_params_t *imu_params) { - return imu_params_parser_->GetFromData(data, imu_params); + const std::uint8_t *data, const std::uint16_t &data_size, + imu_params_t *imu_params) { + return imu_params_parser_->GetFromData(data, data_size, imu_params); } std::size_t FileChannel::SetImuParamsToData( @@ -76,7 +79,8 @@ DeviceInfoParser::~DeviceInfoParser() { } std::size_t DeviceInfoParser::GetFromData( - const std::uint8_t *data, device_info_t *info) const { + const std::uint8_t *data, const std::uint16_t &data_size, + device_info_t *info) const { std::size_t i = 4; // skip vid, pid // name, 16 info->name = bytes::_from_data(data + i, 16); @@ -111,6 +115,7 @@ std::size_t DeviceInfoParser::GetFromData( // get other infos according to spec_version + MYNTEYE_UNUSED(data_size) return i; } @@ -169,17 +174,18 @@ ImgParamsParser::~ImgParamsParser() { } std::size_t ImgParamsParser::GetFromData( - const std::uint8_t *data, img_params_t *img_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + return GetFromData_v1_0(data, data_size, img_params); } else { - return GetFromData_v1_1(data, img_params); + return GetFromData_v1_1(data, data_size, img_params); } } else { // get img params with version header - return GetFromData_new(data, img_params); + return GetFromData_new(data, data_size, img_params); } } @@ -199,19 +205,21 @@ std::size_t ImgParamsParser::SetToData( } std::size_t ImgParamsParser::GetFromData_v1_0( - const std::uint8_t *data, img_params_t *img_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + 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(in_left.get(), data + i, true); + i += bytes::from_data(in_right.get(), data + i, true); 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}; + MYNTEYE_UNUSED(data_size) return i; } @@ -220,10 +228,8 @@ std::size_t ImgParamsParser::SetToData_v1_0( 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(params.in_left.get(), data + i, true); + i += bytes::to_data(params.in_right.get(), data + i, true); i += bytes::to_data(¶ms.ex_right_to_left, data + i); // others @@ -235,23 +241,24 @@ std::size_t ImgParamsParser::SetToData_v1_0( } std::size_t ImgParamsParser::GetFromData_v1_1( - const std::uint8_t *data, img_params_t *img_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + i += bytes::from_data(in_left.get(), data + i, true); + i += bytes::from_data(in_right.get(), data + i, true); (*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); + i += bytes::from_data(in_left.get(), data + i, true); + i += bytes::from_data(in_right.get(), data + i, true); (*img_params)[{2560, 800}] = {true, spec_version_.to_string(), in_left, in_right, ex_right_to_left}; } @@ -261,6 +268,7 @@ std::size_t ImgParamsParser::GetFromData_v1_1( (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; } + MYNTEYE_UNUSED(data_size) return i; } @@ -270,19 +278,13 @@ std::size_t ImgParamsParser::SetToData_v1_1( { 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); + i += bytes::to_data(params.in_left.get(), data + i, true); + i += bytes::to_data(params.in_right.get(), data + i, true); } { 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(params.in_left.get(), data + i, true); + i += bytes::to_data(params.in_right.get(), data + i, true); i += bytes::to_data(¶ms.ex_right_to_left, data + i); } @@ -295,12 +297,102 @@ std::size_t ImgParamsParser::SetToData_v1_1( } std::size_t ImgParamsParser::GetFromData_new( - const std::uint8_t *data, img_params_t *img_params) const { - return 0; + const std::uint8_t *data, const std::uint16_t &data_size, + img_params_t *img_params) const { + std::size_t i = 0; + // version, 2 + Version version(data[i], data[i + 1]); + i += 2; + // get img params according to version + if (version == Version(1, 2)) { // v1.2 + for (; i < data_size;) { + // calib_model, 1 + auto calib_model = static_cast(data[i]); + i += 1; + // width, 2 + auto width = bytes::_from_data(data + i); + i += 2; + // height, 2 + auto height = bytes::_from_data(data + i); + i += 2; + // calib_params + std::shared_ptr in_left, in_right; + Extrinsics ex_right_to_left; + switch (calib_model) { + case CalibrationModel::PINHOLE: + in_left = std::make_shared(); + in_right = std::make_shared(); + break; + case CalibrationModel::KANNALA_BRANDT: + in_left = std::make_shared(); + in_right = std::make_shared(); + break; + default: + LOG(FATAL) << "Could not get img params as unknown calib model" + ", please use latest SDK."; + } + i += bytes::from_data(in_left.get(), data + i, false); + i += bytes::from_data(in_right.get(), data + i, false); + i += bytes::from_data(&ex_right_to_left, data + i); + in_left->calib_model = calib_model; + in_left->width = width; + in_left->height = height; + in_right->calib_model = calib_model; + in_right->width = width; + in_right->height = height; + (*img_params)[{width, height}] = {true, version.to_string(), + in_left, in_right, ex_right_to_left}; + } + } else { + LOG(FATAL) << "Could not get img params of version " + << version.to_string() << ", please use latest SDK."; + } + + MYNTEYE_UNUSED(data_size) + return i; } + std::size_t ImgParamsParser::SetToData_new( const img_params_t *img_params, std::uint8_t *data) const { - return 0; + if (img_params->empty()) { + return 0; + } + + std::size_t i = 3; // skip id, size + // version, 2 + Version version(img_params->begin()->second.version); + data[i] = version.major(); + data[i + 1] = version.minor(); + i += 2; + // set img params according to version + if (version == Version(1, 2)) { // v1.2 + for (auto &&entry : *img_params) { + auto &¶ms = entry.second; + // calib_model, 1 + data[i] = static_cast(params.in_left->calib_model); + i += 1; + // width, 2 + bytes::_to_data(params.in_left->width, data + i); + i += 2; + // height, 2 + bytes::_to_data(params.in_left->height, data + i); + i += 2; + // calib_params + i += bytes::to_data(params.in_left.get(), data + i, false); + i += bytes::to_data(params.in_right.get(), data + i, false); + i += bytes::to_data(¶ms.ex_right_to_left, data + i); + } + } else { + LOG(FATAL) << "Could not set img params of version " + << version.to_string() << ", please use latest SDK."; + } + + // 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; } // ImuParamsParser @@ -312,13 +404,14 @@ ImuParamsParser::~ImuParamsParser() { } std::size_t ImuParamsParser::GetFromData( - const std::uint8_t *data, imu_params_t *imu_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + return GetFromData_old(data, data_size, imu_params); } else { // get imu params with version header - return GetFromData_new(data, imu_params); + return GetFromData_new(data, data_size, imu_params); } } @@ -334,11 +427,13 @@ std::size_t ImuParamsParser::SetToData( } std::size_t ImuParamsParser::GetFromData_old( - const std::uint8_t *data, imu_params_t *imu_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + MYNTEYE_UNUSED(data_size) return i; } @@ -357,7 +452,8 @@ std::size_t ImuParamsParser::SetToData_old( } std::size_t ImuParamsParser::GetFromData_new( - const std::uint8_t *data, imu_params_t *imu_params) const { + const std::uint8_t *data, const std::uint16_t &data_size, + imu_params_t *imu_params) const { std::size_t i = 0; // version, 2 Version version(data[i], data[i + 1]); @@ -372,6 +468,7 @@ std::size_t ImuParamsParser::GetFromData_new( LOG(FATAL) << "Could not get imu params of version " << version.to_string() << ", please use latest SDK."; } + MYNTEYE_UNUSED(data_size) return i; } diff --git a/src/mynteye/device/channel/file_channel.h b/src/mynteye/device/channel/file_channel.h index 2058124..3f2c334 100644 --- a/src/mynteye/device/channel/file_channel.h +++ b/src/mynteye/device/channel/file_channel.h @@ -39,17 +39,20 @@ class FileChannel { ~FileChannel(); std::size_t GetDeviceInfoFromData( - const std::uint8_t *data, device_info_t *info); + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + const std::uint8_t *data, const std::uint16_t &data_size, + 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); + const std::uint8_t *data, const std::uint16_t &data_size, + imu_params_t *imu_params); std::size_t SetImuParamsToData( const imu_params_t *imu_params, std::uint8_t *data); @@ -67,7 +70,8 @@ class DeviceInfoParser { ~DeviceInfoParser(); std::size_t GetFromData( - const std::uint8_t *data, device_info_t *info) const; + const std::uint8_t *data, const std::uint16_t &data_size, + device_info_t *info) const; std::size_t SetToData( const device_info_t *info, std::uint8_t *data) const; }; @@ -84,22 +88,26 @@ class ImgParamsParser { } std::size_t GetFromData( - const std::uint8_t *data, img_params_t *img_params) const; + const std::uint8_t *data, const std::uint16_t &data_size, + 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; + const std::uint8_t *data, const std::uint16_t &data_size, + 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; + const std::uint8_t *data, const std::uint16_t &data_size, + 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; + const std::uint8_t *data, const std::uint16_t &data_size, + img_params_t *img_params) const; std::size_t SetToData_new( const img_params_t *img_params, std::uint8_t *data) const; @@ -119,17 +127,20 @@ class ImuParamsParser { } std::size_t GetFromData( - const std::uint8_t *data, imu_params_t *imu_params) const; + const std::uint8_t *data, const std::uint16_t &data_size, + 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; + const std::uint8_t *data, const std::uint16_t &data_size, + 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; + const std::uint8_t *data, const std::uint16_t &data_size, + imu_params_t *imu_params) const; std::size_t SetToData_new( const imu_params_t *imu_params, std::uint8_t *data) const; diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 1cab68e..915a2f8 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -188,6 +188,19 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) { << ", format: " << request.format << ", fps: " << request.fps; } +const char *to_string(const CalibrationModel &model) { +#define CASE(X) \ + case CalibrationModel::X: \ + return "CalibrationModel::" #X; + switch (model) { + CASE(PINHOLE) + CASE(KANNALA_BRANDT) + default: + return "CalibrationModel::UNKNOWN"; + } +#undef CASE +} + std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) { os << "pinhole, " << FULL_PRECISION << "width: " << in.width << ", height: " << in.height diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index a609222..f458df3 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -66,8 +66,7 @@ bool DeviceWriter::WriteImgParams(const img_params_map_t &img_params_map) { auto &&channels = device_->channels(); auto &&dev_info = device_->GetInfo(); if (channels->SetFiles( - nullptr, const_cast(&img_params_map), nullptr, - &dev_info->spec_version)) { + nullptr, const_cast(&img_params_map), nullptr)) { LOG(INFO) << "Write img params success"; std::map::const_iterator it; for (it = img_params_map.begin(); it != img_params_map.end(); it++) { @@ -93,8 +92,7 @@ bool DeviceWriter::WriteImuParams(const imu_params_t ¶ms) { auto &&channels = device_->channels(); auto &&dev_info = device_->GetInfo(); if (channels->SetFiles( - nullptr, nullptr, const_cast(¶ms), - &dev_info->spec_version)) { + nullptr, nullptr, const_cast(¶ms))) { LOG(INFO) << "Write imu params success"; LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}"; LOG(INFO) << "Imu intrinsics gyro: {" << params.in_gyro << "}";