diff --git a/src/device/device.cc b/src/device/device.cc index 379c7fc..2fa41e3 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -399,29 +399,42 @@ void Device::StopMotionTracking() { } void Device::ReadDeviceInfo() { - // TODO(JohnZhao): Read device info device_info_ = std::make_shared(); + + CHECK_NOTNULL(channels_); + Channels::img_params_t img_params; + Channels::imu_params_t imu_params; + channels_->GetFiles(device_info_.get(), &img_params, &imu_params); + 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; } void Device::WriteImgIntrinsics(const ImgIntrinsics &intrinsics) { - // TODO(JohnZhao): Write img intrinsics - UNUSED(intrinsics); + CHECK_NOTNULL(channels_); + Channels::img_params_t img_params{intrinsics, img_extrinsics_}; + channels_->SetFiles(nullptr, &img_params, nullptr); } void Device::WriteImgExtrinsics(const ImgExtrinsics &extrinsics) { - // TODO(JohnZhao): Write img extrinsics - UNUSED(extrinsics); + CHECK_NOTNULL(channels_); + Channels::img_params_t img_params{img_intrinsics_, extrinsics}; + channels_->SetFiles(nullptr, &img_params, nullptr); } void Device::WriteImuIntrinsics(const ImuIntrinsics &intrinsics) { - // TODO(JohnZhao): Write imu intrinsics - UNUSED(intrinsics); + CHECK_NOTNULL(channels_); + Channels::imu_params_t imu_params{intrinsics, imu_extrinsics_}; + channels_->SetFiles(nullptr, nullptr, &imu_params); } void Device::WriteImuExtrinsics(const ImuExtrinsics &extrinsics) { - // TODO(JohnZhao): Write imu extrinsics - UNUSED(extrinsics); + CHECK_NOTNULL(channels_); + Channels::imu_params_t imu_params{imu_intrinsics_, extrinsics}; + channels_->SetFiles(nullptr, nullptr, &imu_params); } MYNTEYE_END_NAMESPACE diff --git a/src/internal/channels.cc b/src/internal/channels.cc index c8623f8..e6b1560 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -2,6 +2,7 @@ #include +#include #include #include #include @@ -279,6 +280,488 @@ void Channels::StopImuTracking() { } } +namespace { + +template +T _from_data(const std::uint8_t *data) { + std::size_t size = sizeof(T) / sizeof(std::uint8_t); + T value = 0; + for (std::size_t i = 0; i < size; i++) { + value |= data[i] << (8 * (size - i - 1)); + } + return value; +} + +template <> +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::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) { + std::size_t i = 0; + // name, 16 + info->name = _from_data(data + i); + i += 16; + // serial_number, 16 + info->serial_number = _from_data(data + i); + i += 16; + // firmware_version, 2 + info->firmware_version.set_major(data[i]); + info->firmware_version.set_minor(data[i + 1]); + i += 2; + // hardware_version, 3 + info->hardware_version.set_major(data[i]); + info->hardware_version.set_minor(data[i + 1]); + info->hardware_version.set_flag(std::bitset<8>(data[i + 2])); + i += 3; + // spec_version, 2 + info->spec_version.set_major(data[i]); + info->spec_version.set_minor(data[i + 1]); + i += 2; + // lens_type, 4 + info->lens_type.set_vendor(_from_data(data + i)); + info->lens_type.set_product(_from_data(data + i + 2)); + i += 4; + // imu_type, 4 + info->imu_type.set_vendor(_from_data(data + i)); + info->imu_type.set_product(_from_data(data + i + 2)); + i += 4; + // nominal_baseline, 2 + info->nominal_baseline = _from_data(data + i); + i += 2; + return i; +} + +std::size_t from_data( + Channels::img_params_t *img_params, const std::uint8_t *data) { + std::size_t i = 0; + + auto &&in = img_params->in; + // width, 2 + in.width = _from_data(data + i); + i += 2; + // height, 2 + in.height = _from_data(data + i); + i += 2; + // fx, 8 + in.fx = _from_data(data + i); + i += 8; + // fy, 8 + in.fy = _from_data(data + i); + i += 8; + // cx, 8 + in.cx = _from_data(data + i); + i += 8; + // cy, 8 + in.cy = _from_data(data + i); + i += 8; + // model, 1 + in.model = data[i]; + i += 1; + // coeffs, 40 + for (std::size_t j = 0; j < 5; j++) { + in.coeffs[j] = _from_data(data + i + j * 8); + } + i += 40; + + auto &&ex = img_params->ex.left_to_right; + // 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); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + ex.translation[j] = _from_data(data + i + j * 8); + } + i += 24; + + return i; +} + +std::size_t from_data( + Channels::imu_params_t *imu_params, const std::uint8_t *data) { + 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); + } + } + 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); + } + } + i += 72; + // acc_drift + for (std::size_t j = 0; j < 3; j++) { + in.accel.drift[j] = _from_data(data + i + j * 8); + } + i += 24; + // gyro_drift + for (std::size_t j = 0; j < 3; j++) { + in.gyro.drift[j] = _from_data(data + i + j * 8); + } + i += 24; + // acc_noise + for (std::size_t j = 0; j < 3; j++) { + in.accel.noise[j] = _from_data(data + i + j * 8); + } + i += 24; + // gyro_noise + for (std::size_t j = 0; j < 3; j++) { + in.gyro.noise[j] = _from_data(data + i + j * 8); + } + i += 24; + // acc_bias + for (std::size_t j = 0; j < 3; j++) { + in.accel.bias[j] = _from_data(data + i + j * 8); + } + i += 24; + // gyro_bias + for (std::size_t j = 0; j < 3; j++) { + in.gyro.bias[j] = _from_data(data + i + j * 8); + } + i += 24; + + auto &&ex = imu_params->ex.left_to_imu; + // 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); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + ex.translation[j] = _from_data(data + i + j * 8); + } + i += 24; + + return i; +} + +} // namespace + +bool Channels::GetFiles( + device_info_t *info, img_params_t *img_params, + imu_params_t *imu_params) const { + if (info == nullptr && img_params == nullptr && imu_params == nullptr) { + LOG(WARNING) << "Files are not provided to get"; + return false; + } + + std::uint8_t data[2000]{}; + + std::bitset<8> header; + header[0] = 0; // get + + header[7] = (info != nullptr); + header[6] = (img_params != nullptr); + header[5] = (imu_params != nullptr); + + data[0] = static_cast(header.to_ulong()); + 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]; + + std::uint8_t checksum_cal = 0; + for (std::size_t i = 4, n = 4 + size; i < n; i++) { + checksum_cal = (checksum_cal ^ data[i]); + } + if (checksum_cal != checksum) { + LOG(WARNING) << "Files checksum should be 0x" << std::hex + << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(checksum_cal) << ", but 0x" + << std::setw(2) << std::setfill('0') + << static_cast(checksum) << " now"; + return false; + } + + 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); + i += 3; + switch (file_id) { + case FID_DEVICE_INFO: { + CHECK_EQ(from_data(info, data + i), file_size); + } break; + case FID_IMG_PARAMS: { + CHECK_EQ(from_data(img_params, data + i), file_size); + } break; + case FID_IMU_PARAMS: { + CHECK_EQ(from_data(imu_params, data + i), file_size); + } break; + default: + LOG(FATAL) << "Unsupported file id: " << file_id; + } + i += file_size; + } + + return true; + } else { + LOG(WARNING) << "GetFiles failed"; + return false; + } +} + +namespace { + +template +std::size_t _to_data(T value, std::uint8_t *data) { + std::size_t size = sizeof(T) / sizeof(std::uint8_t); + for (std::size_t i = 0; i < size; i++) { + data[i] = + static_cast((value >> (8 * (size - i - 1))) && 0xFF); + } + return size; +} + +template <> +std::size_t _to_data(double value, std::uint8_t *data) { + std::copy(data, data + 8, reinterpret_cast(&value)); + return 8; +} + +template <> +std::size_t _to_data(std::string value, std::uint8_t *data) { + std::copy(value.begin(), value.end(), data); + data[value.size()] = '\0'; + return value.size() + 1; +} + +std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) { + std::size_t i = 3; + // name, 16 + _to_data(info->name, data + i); + i += 16; + // serial_number, 16 + _to_data(info->serial_number, data + i); + i += 16; + // firmware_version, 2 + data[i] = info->firmware_version.major(); + data[i + 1] = info->firmware_version.minor(); + i += 2; + // hardware_version, 3 + data[i] = info->hardware_version.major(); + data[i + 1] = info->hardware_version.minor(); + data[i + 2] = + static_cast(info->hardware_version.flag().to_ulong()); + i += 3; + // spec_version, 2 + data[i] = info->spec_version.major(); + data[i + 1] = info->spec_version.minor(); + i += 2; + // lens_type, 4 + _to_data(info->lens_type.vendor(), data + i); + _to_data(info->lens_type.product(), data + i + 2); + i += 4; + // imu_type, 4 + _to_data(info->imu_type.vendor(), data + i); + _to_data(info->imu_type.product(), data + i + 2); + i += 4; + // nominal_baseline, 2 + _to_data(info->nominal_baseline, data + i); + i += 2; + // others + std::size_t size = i - 3; + data[0] = Channels::FID_DEVICE_INFO; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +std::size_t to_data( + const Channels::img_params_t *img_params, std::uint8_t *data) { + std::size_t i = 3; + + auto &&in = img_params->in; + // 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; + + auto &&ex = img_params->ex.left_to_right; + // 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); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + _to_data(ex.translation[j], data + i + j * 8); + } + i += 24; + + // others + std::size_t size = i - 3; + data[0] = Channels::FID_IMG_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +std::size_t to_data( + const Channels::imu_params_t *imu_params, std::uint8_t *data) { + std::size_t i = 3; + + 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); + } + } + 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); + } + } + i += 72; + // acc_drift + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.accel.drift[j], data + i + j * 8); + } + i += 24; + // gyro_drift + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.gyro.drift[j], data + i + j * 8); + } + i += 24; + // acc_noise + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.accel.noise[j], data + i + j * 8); + } + i += 24; + // gyro_noise + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.gyro.noise[j], data + i + j * 8); + } + i += 24; + // acc_bias + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.accel.bias[j], data + i + j * 8); + } + i += 24; + // gyro_bias + for (std::size_t j = 0; j < 3; j++) { + _to_data(in.gyro.bias[j], data + i + j * 8); + } + i += 24; + + auto &&ex = imu_params->ex.left_to_imu; + // 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); + } + } + i += 72; + // translation + for (std::size_t j = 0; j < 3; j++) { + _to_data(ex.translation[j], data + i + j * 8); + } + i += 24; + + // others + std::size_t size = i - 3; + data[0] = Channels::FID_IMU_PARAMS; + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + return size + 3; +} + +} // namespace + +bool Channels::SetFiles( + 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; + } + std::uint8_t data[2000]{}; + + std::bitset<8> header; + header[0] = 1; // set + + std::uint16_t size = 0; + if (info != nullptr) { + header[7] = true; + size += to_data(info, data + 3 + size); + } + if (img_params != nullptr) { + header[6] = true; + size += to_data(img_params, data + 3 + size); + } + if (imu_params != nullptr) { + header[5] = true; + size += to_data(imu_params, data + 3 + size); + } + + data[0] = static_cast(header.to_ulong()); + data[1] = static_cast((size >> 8) & 0xFF); + data[2] = static_cast(size & 0xFF); + + if (XuFileQuery(uvc::XU_QUERY_SET, 2000, data)) { + VLOG(2) << "SetFiles success"; + return true; + } else { + LOG(WARNING) << "SetFiles failed"; + return false; + } +} + bool Channels::PuControlRange( Option option, int32_t *min, int32_t *max, int32_t *def) const { CHECK_NOTNULL(device_); @@ -409,6 +892,11 @@ bool Channels::XuImuRead(ImuResPacket *res) const { } } +bool Channels::XuFileQuery( + uvc::xu_query query, uint16_t size, uint8_t *data) const { + return XuControlQuery(CHANNEL_FILE, query, size, data); +} + Channels::control_info_t Channels::PuControlInfo(Option option) const { int32_t min = 0, max = 0, def = 0; if (!PuControlRange(option, &min, &max, &def)) { diff --git a/src/internal/channels.h b/src/internal/channels.h index ee9fff2..764c8fc 100644 --- a/src/internal/channels.h +++ b/src/internal/channels.h @@ -7,6 +7,7 @@ #include #include "mynteye/mynteye.h" +#include "mynteye/types.h" #include "internal/types.h" #include "uvc/uvc.h" @@ -43,8 +44,27 @@ class Channels { XU_CMD_LAST } 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_LAST, + } file_id_t; + using imu_callback_t = std::function; + using device_info_t = DeviceInfo; + + typedef struct ImgParams { + ImgIntrinsics in; + ImgExtrinsics ex; + } img_params_t; + + typedef struct ImuParams { + ImuIntrinsics in; + ImuExtrinsics ex; + } imu_params_t; + explicit Channels(std::shared_ptr device); ~Channels(); @@ -61,6 +81,12 @@ class Channels { void StartImuTracking(imu_callback_t callback = nullptr); void StopImuTracking(); + bool GetFiles( + device_info_t *info, img_params_t *img_params, + imu_params_t *imu_params) const; + bool SetFiles( + device_info_t *info, img_params_t *img_params, imu_params_t *imu_params); + private: bool PuControlRange( Option option, int32_t *min, int32_t *max, int32_t *def) const; @@ -82,6 +108,8 @@ class Channels { bool XuImuWrite(const ImuReqPacket &req) const; bool XuImuRead(ImuResPacket *res) const; + bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const; + control_info_t PuControlInfo(Option option) const; control_info_t XuControlInfo(Option option) const;