diff --git a/src/device/device.cc b/src/device/device.cc index 2fa41e3..f7929cb 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -4,6 +4,7 @@ #include #include +#include #include "device/device_s.h" #include "internal/channels.h" @@ -49,7 +50,7 @@ Device::Device(const Model &model, std::shared_ptr device) channels_(std::make_shared(device)), motions_(std::make_shared(channels_)) { VLOG(2) << __func__; - ReadDeviceInfo(); + ReadAllInfos(); } Device::~Device() { @@ -158,6 +159,14 @@ ImgExtrinsics Device::GetImgExtrinsics() const { return img_extrinsics_; } +void Device::SetImgIntrinsics(const ImgIntrinsics &in) { + img_intrinsics_ = std::move(in); +} + +void Device::SetImgExtrinsics(const ImgExtrinsics &ex) { + img_extrinsics_ = std::move(ex); +} + ImuIntrinsics Device::GetImuIntrinsics() const { return imu_intrinsics_; } @@ -166,6 +175,14 @@ ImuExtrinsics Device::GetImuExtrinsics() const { return imu_extrinsics_; } +void Device::SetImuIntrinsics(const ImuIntrinsics &in) { + imu_intrinsics_ = std::move(in); +} + +void Device::SetImuExtrinsics(const ImuExtrinsics &ex) { + imu_extrinsics_ = std::move(ex); +} + void Device::LogOptionInfos() const { channels_->LogControlInfos(); } @@ -398,13 +415,15 @@ void Device::StopMotionTracking() { motion_tracking_ = false; } -void Device::ReadDeviceInfo() { +void Device::ReadAllInfos() { 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); + if (!channels_->GetFiles(device_info_.get(), &img_params, &imu_params)) { + // LOG(FATAL) << "Read device infos failed :("; + } device_info_->name = uvc::get_name(*device_); img_intrinsics_ = img_params.in; @@ -413,28 +432,24 @@ void Device::ReadDeviceInfo() { imu_extrinsics_ = imu_params.ex; } -void Device::WriteImgIntrinsics(const ImgIntrinsics &intrinsics) { +void Device::WriteImgParams( + const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics) { CHECK_NOTNULL(channels_); - Channels::img_params_t img_params{intrinsics, img_extrinsics_}; - channels_->SetFiles(nullptr, &img_params, nullptr); + Channels::img_params_t img_params{intrinsics, extrinsics}; + if (channels_->SetFiles(nullptr, &img_params, nullptr)) { + img_intrinsics_ = intrinsics; + img_extrinsics_ = extrinsics; + } } -void Device::WriteImgExtrinsics(const ImgExtrinsics &extrinsics) { +void Device::WriteImuParams( + const ImuIntrinsics &intrinsics, const ImuExtrinsics &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) { - CHECK_NOTNULL(channels_); - Channels::imu_params_t imu_params{intrinsics, imu_extrinsics_}; - channels_->SetFiles(nullptr, nullptr, &imu_params); -} - -void Device::WriteImuExtrinsics(const ImuExtrinsics &extrinsics) { - CHECK_NOTNULL(channels_); - Channels::imu_params_t imu_params{imu_intrinsics_, extrinsics}; - channels_->SetFiles(nullptr, nullptr, &imu_params); + Channels::imu_params_t imu_params{intrinsics, extrinsics}; + if (channels_->SetFiles(nullptr, nullptr, &imu_params)) { + imu_intrinsics_ = intrinsics; + imu_extrinsics_ = extrinsics; + } } MYNTEYE_END_NAMESPACE diff --git a/src/device/device.h b/src/device/device.h index bcb5b49..7dd79f1 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -59,9 +59,13 @@ class Device { ImgIntrinsics GetImgIntrinsics() const; ImgExtrinsics GetImgExtrinsics() const; + void SetImgIntrinsics(const ImgIntrinsics &in); + void SetImgExtrinsics(const ImgExtrinsics &ex); ImuIntrinsics GetImuIntrinsics() const; ImuExtrinsics GetImuExtrinsics() const; + void SetImuIntrinsics(const ImuIntrinsics &in); + void SetImuExtrinsics(const ImuExtrinsics &ex); void LogOptionInfos() const; OptionInfo GetOptionInfo(const Option &option) const; @@ -134,13 +138,12 @@ class Device { std::shared_ptr motions_; - void ReadDeviceInfo(); + void ReadAllInfos(); - void WriteImgIntrinsics(const ImgIntrinsics &intrinsics); - void WriteImgExtrinsics(const ImgExtrinsics &extrinsics); - - void WriteImuIntrinsics(const ImuIntrinsics &intrinsics); - void WriteImuExtrinsics(const ImuExtrinsics &extrinsics); + void WriteImgParams( + const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics); + void WriteImuParams( + const ImuIntrinsics &intrinsics, const ImuExtrinsics &extrinsics); // friend DeviceWriter; };