From e04a05f6f981f8514cc2102d6aec3f4faf753fac Mon Sep 17 00:00:00 2001 From: John Zhao Date: Thu, 19 Apr 2018 17:23:02 +0800 Subject: [PATCH] Complete read/write files --- doc/zh-Hans/spec_control_channel.md | 6 +- include/mynteye/types.h | 12 +++ src/device/device.cc | 52 +++++++-- src/device/device.h | 1 + src/internal/channels.cc | 157 +++++++++++++++++++--------- src/internal/channels.h | 15 +-- src/public/types.cc | 67 ++++++++++++ 7 files changed, 244 insertions(+), 66 deletions(-) diff --git a/doc/zh-Hans/spec_control_channel.md b/doc/zh-Hans/spec_control_channel.md index bf1a218..9d3ed48 100644 --- a/doc/zh-Hans/spec_control_channel.md +++ b/doc/zh-Hans/spec_control_channel.md @@ -48,6 +48,6 @@ | File | ID | Max Size | | :--- | :- | :------- | -| 硬件信息 | 0 | 250 | -| 图像参数 | 1 | 250 | -| IMU 参数 | 2 | 500 | +| 硬件信息 | 1 | 250 | +| 图像参数 | 2 | 250 | +| IMU 参数 | 4 | 500 | diff --git a/include/mynteye/types.h b/include/mynteye/types.h index f34cf11..3272e4d 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -269,6 +269,8 @@ struct MYNTEYE_API ImgIntrinsics { double coeffs[5]; }; +std::ostream &operator<<(std::ostream &os, const ImgIntrinsics &in); + /** * @ingroup calibration * IMU sensor intrinsics: scale, drift and variances. @@ -289,6 +291,8 @@ struct MYNTEYE_API ImuSensorIntrinsics { double bias[3]; }; +std::ostream &operator<<(std::ostream &os, const ImuSensorIntrinsics &in); + /** * @ingroup calibration * IMU intrinsics, including accelerometer and gyroscope. @@ -298,6 +302,8 @@ struct MYNTEYE_API ImuIntrinsics { ImuSensorIntrinsics gyro; }; +std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in); + /** * @ingroup calibration * Extrinsics, represent how the different datas are connected. @@ -307,6 +313,8 @@ struct MYNTEYE_API Extrinsics { double translation[3]; /**< translation vector */ }; +std::ostream &operator<<(std::ostream &os, const Extrinsics &ex); + /** * @ingroup calibration * Image extrinsics. @@ -315,6 +323,8 @@ struct MYNTEYE_API ImgExtrinsics { Extrinsics left_to_right; }; +std::ostream &operator<<(std::ostream &os, const ImgExtrinsics &ex); + /** * @ingroup calibration * IMU extrinsics. @@ -323,6 +333,8 @@ struct MYNTEYE_API ImuExtrinsics { Extrinsics left_to_imu; }; +std::ostream &operator<<(std::ostream &os, const ImuExtrinsics &ex); + /** * @defgroup datatypes Datatypes * @brief Public data types. diff --git a/src/device/device.cc b/src/device/device.cc index 930d09a..7e0c249 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -424,21 +424,54 @@ void Device::ReadAllInfos() { Channels::img_params_t img_params; Channels::imu_params_t imu_params; if (!channels_->GetFiles(device_info_.get(), &img_params, &imu_params)) { - // LOG(FATAL) << "Read device infos failed :("; + LOG(FATAL) << "Read device infos failed :("; } + VLOG(2) << "Device name: " << device_info_->name + << ", serial_number: " << device_info_->serial_number + << ", firmware_version: " + << device_info_->firmware_version.to_string() + << ", hardware_version: " + << device_info_->hardware_version.to_string() + << ", spec_version: " << device_info_->spec_version.to_string() + << ", lens_type: " << device_info_->lens_type.to_string() + << ", imu_type: " << device_info_->imu_type.to_string() + << ", nominal_baseline: " << device_info_->nominal_baseline; device_info_->name = uvc::get_name(*device_); - img_intrinsics_ = img_params.in; - img_extrinsics_ = img_params.ex; - imu_intrinsics_ = imu_params.in; - imu_extrinsics_ = imu_params.ex; + if (img_params.ok) { + img_intrinsics_ = img_params.in; + img_extrinsics_ = img_params.ex; + VLOG(2) << "ImgIntrinsics " << img_intrinsics_; + VLOG(2) << "ImgExtrinsics " << img_extrinsics_; + } else { + LOG(WARNING) << "Img intrinsics & extrinsics not exist"; + } + if (imu_params.ok) { + imu_intrinsics_ = imu_params.in; + imu_extrinsics_ = imu_params.ex; + VLOG(2) << "ImuIntrinsics " << imu_intrinsics_; + VLOG(2) << "ImuExtrinsics " << imu_extrinsics_; + } else { + LOG(WARNING) << "Imu intrinsics & extrinsics not exist"; + } +} + +void Device::WriteDeviceInfo(const DeviceInfo &device_info) { + CHECK_NOTNULL(channels_); + DeviceInfo info = device_info; + if (channels_->SetFiles(&info, nullptr, nullptr)) { + device_info_->lens_type = info.lens_type; + device_info_->imu_type = info.imu_type; + device_info_->nominal_baseline = info.nominal_baseline; + } } void Device::WriteImgParams( const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics) { CHECK_NOTNULL(channels_); - Channels::img_params_t img_params{intrinsics, extrinsics}; - if (channels_->SetFiles(nullptr, &img_params, nullptr)) { + Channels::img_params_t img_params{false, intrinsics, extrinsics}; + if (channels_->SetFiles( + nullptr, &img_params, nullptr, &device_info_->spec_version)) { img_intrinsics_ = intrinsics; img_extrinsics_ = extrinsics; } @@ -447,8 +480,9 @@ void Device::WriteImgParams( void Device::WriteImuParams( const ImuIntrinsics &intrinsics, const ImuExtrinsics &extrinsics) { CHECK_NOTNULL(channels_); - Channels::imu_params_t imu_params{intrinsics, extrinsics}; - if (channels_->SetFiles(nullptr, nullptr, &imu_params)) { + Channels::imu_params_t imu_params{false, intrinsics, extrinsics}; + if (channels_->SetFiles( + nullptr, nullptr, &imu_params, &device_info_->spec_version)) { imu_intrinsics_ = intrinsics; imu_extrinsics_ = extrinsics; } diff --git a/src/device/device.h b/src/device/device.h index 7dd79f1..e124640 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -140,6 +140,7 @@ class Device { void ReadAllInfos(); + void WriteDeviceInfo(const DeviceInfo &device_info); void WriteImgParams( const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics); void WriteImuParams( diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 1cda925..59852e6 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -9,6 +9,8 @@ #include #include +#include "internal/strings.h" + MYNTEYE_BEGIN_NAMESPACE namespace { @@ -57,6 +59,26 @@ 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"}; + 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 be {" << ss.str() << "}"; +} + } // namespace Channels::Channels(std::shared_ptr device) @@ -316,27 +338,24 @@ T _from_data(const std::uint8_t *data) { return value; } -template <> -std::string _from_data(const std::uint8_t *data) { - return std::string(reinterpret_cast(data)); -} - template <> double _from_data(const std::uint8_t *data) { return *(reinterpret_cast(data)); } -// std::string _from_data(const std::uint8_t *data, std::size_t n) { -// return std::string(reinterpret_cast(data), n); -// } +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 = 0; + std::size_t i = 4; // skip vid, pid // name, 16 - info->name = _from_data(data + i); + info->name = _from_data(data + i, 16); i += 16; // serial_number, 16 - info->serial_number = _from_data(data + i); + info->serial_number = _from_data(data + i, 16); i += 16; // firmware_version, 2 info->firmware_version.set_major(data[i]); @@ -362,11 +381,13 @@ std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) { // nominal_baseline, 2 info->nominal_baseline = _from_data(data + i); i += 2; + return i; } std::size_t from_data( - Channels::img_params_t *img_params, const std::uint8_t *data) { + Channels::img_params_t *img_params, const std::uint8_t *data, + const Version *spec_version) { std::size_t i = 0; auto &&in = img_params->in; @@ -401,7 +422,7 @@ std::size_t from_data( // 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); + ex.rotation[j][k] = _from_data(data + i + (j * 3 + k) * 8); } } i += 72; @@ -411,25 +432,27 @@ std::size_t from_data( } i += 24; + UNUSED(spec_version) return i; } std::size_t from_data( - Channels::imu_params_t *imu_params, const std::uint8_t *data) { + Channels::imu_params_t *imu_params, const std::uint8_t *data, + const Version *spec_version) { std::size_t i = 0; auto &&in = imu_params->in; // acc_scale for (std::size_t j = 0; j < 3; j++) { for (std::size_t k = 0; k < 3; k++) { - in.accel.scale[j][k] = _from_data(data + i + j * 3 + k); + in.accel.scale[j][k] = _from_data(data + i + (j * 3 + k) * 8); } } i += 72; // gyro_scale for (std::size_t j = 0; j < 3; j++) { for (std::size_t k = 0; k < 3; k++) { - in.gyro.scale[j][k] = _from_data(data + i + j * 3 + k); + in.gyro.scale[j][k] = _from_data(data + i + (j * 3 + k) * 8); } } i += 72; @@ -468,7 +491,7 @@ std::size_t from_data( // 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); + ex.rotation[j][k] = _from_data(data + i + (j * 3 + k) * 8); } } i += 72; @@ -478,14 +501,15 @@ std::size_t from_data( } i += 24; + UNUSED(spec_version) return i; } } // namespace bool Channels::GetFiles( - device_info_t *info, img_params_t *img_params, - imu_params_t *imu_params) const { + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, + Version *spec_version) const { if (info == nullptr && img_params == nullptr && imu_params == nullptr) { LOG(WARNING) << "Files are not provided to get"; return false; @@ -501,21 +525,22 @@ bool Channels::GetFiles( header[2] = (imu_params != nullptr); data[0] = static_cast(header.to_ulong()); - // VLOG(2) << "GetFiles header: 0x" << std::hex << std::uppercase - // << std::setw(2) << std::setfill('0') << static_cast(data[0]); + VLOG(2) << "GetFiles header: 0x" << std::hex << std::uppercase << std::setw(2) + << std::setfill('0') << static_cast(data[0]); if (!XuFileQuery(uvc::XU_QUERY_SET, 2000, data)) { LOG(WARNING) << "GetFiles failed"; return false; } if (XuFileQuery(uvc::XU_QUERY_GET, 2000, data)) { - VLOG(2) << "GetFiles success"; // header = std::bitset<8>(data[0]); std::uint16_t size = _from_data(data + 1); std::uint8_t checksum = data[3 + size]; + VLOG(2) << "GetFiles data size: " << size << ", checksum: 0x" << std::hex + << std::setw(2) << std::setfill('0') << static_cast(checksum); std::uint8_t checksum_now = 0; - for (std::size_t i = 4, n = 4 + size; i < n; i++) { + for (std::size_t i = 3, n = 3 + size; i < n; i++) { checksum_now = (checksum_now ^ data[i]); } if (checksum != checksum_now) { @@ -527,21 +552,34 @@ bool Channels::GetFiles( return false; } + Version *spec_ver = spec_version; std::size_t i = 3; std::size_t end = 3 + size; while (i < end) { std::uint8_t file_id = *(data + i); std::uint16_t file_size = _from_data(data + i + 1); + VLOG(2) << "GetFiles id: " << static_cast(file_id) + << ", size: " << file_size; i += 3; switch (file_id) { case FID_DEVICE_INFO: { CHECK_EQ(from_data(info, data + i), file_size); + spec_ver = &info->spec_version; + CheckSpecVersion(spec_ver); } break; case FID_IMG_PARAMS: { - CHECK_EQ(from_data(img_params, data + i), file_size); + img_params->ok = file_size > 0; + if (img_params->ok) { + CheckSpecVersion(spec_ver); + CHECK_EQ(from_data(img_params, data + i, spec_ver), file_size); + } } break; case FID_IMU_PARAMS: { - CHECK_EQ(from_data(imu_params, data + i), file_size); + imu_params->ok = file_size > 0; + if (imu_params->ok) { + CheckSpecVersion(spec_ver); + CHECK_EQ(from_data(imu_params, data + i, spec_ver), file_size); + } } break; default: LOG(FATAL) << "Unsupported file id: " << file_id; @@ -549,6 +587,7 @@ bool Channels::GetFiles( i += file_size; } + VLOG(2) << "GetFiles success"; return true; } else { LOG(WARNING) << "GetFiles failed"; @@ -562,32 +601,36 @@ 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); + data[i] = static_cast((value >> (8 * (size - i - 1))) & 0xFF); } return size; } template <> std::size_t _to_data(double value, std::uint8_t *data) { - std::copy(data, data + 8, reinterpret_cast(&value)); + std::uint8_t *val = reinterpret_cast(&value); + std::copy(val, val + 8, data); return 8; } -template <> -std::size_t _to_data(std::string value, std::uint8_t *data) { +std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) { std::copy(value.begin(), value.end(), data); - data[value.size()] = '\0'; - return value.size() + 1; + 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) { - std::size_t i = 3; +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); + _to_data(info->name, data + i, 16); i += 16; // serial_number, 16 - _to_data(info->serial_number, data + i); + _to_data(info->serial_number, data + i, 16); i += 16; // firmware_version, 2 data[i] = info->firmware_version.major(); @@ -614,6 +657,9 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) { // nominal_baseline, 2 _to_data(info->nominal_baseline, data + i); i += 2; + + UNUSED(spec_version) + // others std::size_t size = i - 3; data[0] = Channels::FID_DEVICE_INFO; @@ -623,8 +669,9 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) { } std::size_t to_data( - const Channels::img_params_t *img_params, std::uint8_t *data) { - std::size_t i = 3; + const Channels::img_params_t *img_params, std::uint8_t *data, + const Version *spec_version) { + std::size_t i = 3; // skip id, size auto &&in = img_params->in; // width, 2 @@ -658,7 +705,7 @@ std::size_t to_data( // 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); + _to_data(ex.rotation[j][k], data + i + (j * 3 + k) * 8); } } i += 72; @@ -668,6 +715,8 @@ std::size_t to_data( } i += 24; + UNUSED(spec_version) + // others std::size_t size = i - 3; data[0] = Channels::FID_IMG_PARAMS; @@ -677,21 +726,22 @@ std::size_t to_data( } std::size_t to_data( - const Channels::imu_params_t *imu_params, std::uint8_t *data) { - std::size_t i = 3; + const Channels::imu_params_t *imu_params, std::uint8_t *data, + const Version *spec_version) { + std::size_t i = 3; // skip id, size auto &&in = imu_params->in; // acc_scale for (std::size_t j = 0; j < 3; j++) { for (std::size_t k = 0; k < 3; k++) { - _to_data(in.accel.scale[j][k], data + i + j * 3 + k); + _to_data(in.accel.scale[j][k], data + i + (j * 3 + k) * 8); } } i += 72; // gyro_scale for (std::size_t j = 0; j < 3; j++) { for (std::size_t k = 0; k < 3; k++) { - _to_data(in.gyro.scale[j][k], data + i + j * 3 + k); + _to_data(in.gyro.scale[j][k], data + i + (j * 3 + k) * 8); } } i += 72; @@ -730,7 +780,7 @@ std::size_t to_data( // 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); + _to_data(ex.rotation[j][k], data + i + (j * 3 + k) * 8); } } i += 72; @@ -740,6 +790,8 @@ std::size_t to_data( } i += 24; + UNUSED(spec_version) + // others std::size_t size = i - 3; data[0] = Channels::FID_IMU_PARAMS; @@ -751,11 +803,18 @@ std::size_t to_data( } // namespace bool Channels::SetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) { + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, + Version *spec_version) { 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]{}; std::bitset<8> header; @@ -764,21 +823,23 @@ bool Channels::SetFiles( std::uint16_t size = 0; if (info != nullptr) { header[0] = true; - size += to_data(info, data + 3 + size); + size += to_data(info, data + 3 + size, spec_ver); } if (img_params != nullptr) { header[1] = true; - size += to_data(img_params, data + 3 + size); + size += to_data(img_params, data + 3 + size, spec_ver); } if (imu_params != nullptr) { header[2] = true; - size += to_data(imu_params, data + 3 + size); + size += to_data(imu_params, data + 3 + size, spec_ver); } data[0] = static_cast(header.to_ulong()); data[1] = static_cast((size >> 8) & 0xFF); data[2] = static_cast(size & 0xFF); + VLOG(2) << "SetFiles header: 0x" << std::hex << std::uppercase << std::setw(2) + << std::setfill('0') << static_cast(data[0]); if (XuFileQuery(uvc::XU_QUERY_SET, 2000, data)) { VLOG(2) << "SetFiles success"; return true; diff --git a/src/internal/channels.h b/src/internal/channels.h index 764c8fc..d881310 100644 --- a/src/internal/channels.h +++ b/src/internal/channels.h @@ -45,9 +45,9 @@ class Channels { } xu_cmd_t; typedef enum FileId { - FID_DEVICE_INFO = 0, // device info - FID_IMG_PARAMS = 1, // image intrinsics & extrinsics - FID_IMU_PARAMS = 2, // imu intrinsics & extrinsics + 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; @@ -56,11 +56,13 @@ class Channels { using device_info_t = DeviceInfo; typedef struct ImgParams { + bool ok; ImgIntrinsics in; ImgExtrinsics ex; } img_params_t; typedef struct ImuParams { + bool ok; ImuIntrinsics in; ImuExtrinsics ex; } imu_params_t; @@ -82,10 +84,11 @@ class Channels { void StopImuTracking(); bool GetFiles( - device_info_t *info, img_params_t *img_params, - imu_params_t *imu_params) const; + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, + Version *spec_version = nullptr) const; bool SetFiles( - device_info_t *info, img_params_t *img_params, imu_params_t *imu_params); + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params, + Version *spec_version = nullptr); private: bool PuControlRange( diff --git a/src/public/types.cc b/src/public/types.cc index 82cef84..21ebf5d 100644 --- a/src/public/types.cc +++ b/src/public/types.cc @@ -145,4 +145,71 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) { << ", format: " << request.format << ", fps: " << request.fps; } +std::ostream &operator<<(std::ostream &os, const ImgIntrinsics &in) { + os << "width: " << in.width << ", height: " << in.height << ", fx: " << in.fx + << ", fy: " << in.fy << ", cx: " << in.cx << ", cy: " << in.cy + << ", model: " << static_cast(in.model) << ", coeffs: ["; + for (int i = 0; i <= 3; i++) + os << in.coeffs[i] << ", "; + return os << in.coeffs[4] << "]"; +} + +std::ostream &operator<<(std::ostream &os, const ImuSensorIntrinsics &in) { + os << "scale: ["; + for (int i = 0; i <= 2; i++) + os << in.scale[0][i] << ", "; + for (int i = 0; i <= 2; i++) + os << in.scale[1][i] << ", "; + for (int i = 0; i <= 1; i++) + os << in.scale[2][i] << ", "; + os << in.scale[2][2] << "]"; + + os << ", drift: ["; + for (int i = 0; i <= 1; i++) + os << in.drift[i] << ", "; + os << in.drift[2] << "]"; + + os << ", noise: ["; + for (int i = 0; i <= 1; i++) + os << in.noise[i] << ", "; + os << in.noise[2] << "]"; + + os << ", bias: ["; + for (int i = 0; i <= 1; i++) + os << in.bias[i] << ", "; + os << in.bias[2] << "]"; + + return os; +} + +std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) { + return os << "accel: {" << in.accel << "}, gyro: {" << in.gyro << "}"; +} + +std::ostream &operator<<(std::ostream &os, const Extrinsics &ex) { + os << "rotation: ["; + for (int i = 0; i <= 2; i++) + os << ex.rotation[0][i] << ", "; + for (int i = 0; i <= 2; i++) + os << ex.rotation[1][i] << ", "; + for (int i = 0; i <= 1; i++) + os << ex.rotation[2][i] << ", "; + os << ex.rotation[2][2] << "]"; + + os << ", translation: ["; + for (int i = 0; i <= 1; i++) + os << ex.translation[i] << ", "; + os << ex.translation[2] << "]"; + + return os; +} + +std::ostream &operator<<(std::ostream &os, const ImgExtrinsics &ex) { + return os << "left_to_right: {" << ex.left_to_right << "}"; +} + +std::ostream &operator<<(std::ostream &os, const ImuExtrinsics &ex) { + return os << "left_to_imu: {" << ex.left_to_imu << "}"; +} + MYNTEYE_END_NAMESPACE