From 6feeb49d625d579e19da8fca1e60b536fa14ad28 Mon Sep 17 00:00:00 2001 From: Kalman Date: Fri, 14 Sep 2018 20:00:28 +0800 Subject: [PATCH] Select the img params by resolution --- include/mynteye/types.h | 6 +- samples/device/CMakeLists.txt | 4 ++ samples/tutorials/CMakeLists.txt | 1 + src/api/api.cc | 35 +++++----- src/api/api.h | 2 + src/device/device.cc | 25 ++++--- src/device/device.h | 8 +++ src/internal/channels.cc | 61 +++++++++++++++-- src/internal/channels.h | 4 +- src/internal/config.cc | 7 +- src/public/types.cc | 5 ++ tools/dataset/CMakeLists.txt | 4 ++ tools/writer/config/img.params | 24 ++++++- tools/writer/device_writer.cc | 110 +++++++++++++++++++++---------- 14 files changed, 221 insertions(+), 75 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 117a2a4..633e11f 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -264,8 +264,8 @@ enum class AddOns : std::uint8_t { * @brief Camera supported resolution. */ enum class Resolution : std::uint8_t { - /** 480x752 */ - RES_480x752, + /** 752x480 */ + RES_752x480, /** 1280x400 */ RES_1280x400, /** 2560x800 */ @@ -373,7 +373,7 @@ struct MYNTEYE_API StreamRequest { StreamRequest(Resolution res, Format format, FrameRate rate) : format(format) { switch (res) { - case Resolution::RES_480x752: + case Resolution::RES_752x480: width = 480, height = 752; break; case Resolution::RES_1280x400: diff --git a/samples/device/CMakeLists.txt b/samples/device/CMakeLists.txt index aa27b4e..cf7ac72 100644 --- a/samples/device/CMakeLists.txt +++ b/samples/device/CMakeLists.txt @@ -14,6 +14,10 @@ get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) +include_directories( + ${PRO_DIR}/src +) + set_outdir( "${OUT_DIR}/lib/${DIR_NAME}" "${OUT_DIR}/lib/${DIR_NAME}" diff --git a/samples/tutorials/CMakeLists.txt b/samples/tutorials/CMakeLists.txt index 24c79dd..9b81b99 100644 --- a/samples/tutorials/CMakeLists.txt +++ b/samples/tutorials/CMakeLists.txt @@ -16,6 +16,7 @@ get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) include_directories( ${CMAKE_CURRENT_SOURCE_DIR} + ${PRO_DIR}/src ) set_outdir( diff --git a/src/api/api.cc b/src/api/api.cc index 18cdd5c..50b0893 100644 --- a/src/api/api.cc +++ b/src/api/api.cc @@ -212,21 +212,7 @@ std::vector get_plugin_paths() { API::API(std::shared_ptr device) : device_(device) { VLOG(2) << __func__; - if (std::dynamic_pointer_cast(device_) != nullptr) { - bool in_l_ok, in_r_ok, ex_l2r_ok; - device_->GetIntrinsics(Stream::LEFT, &in_l_ok); - device_->GetIntrinsics(Stream::RIGHT, &in_r_ok); - device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT, &ex_l2r_ok); - if (!in_l_ok || !in_r_ok || !ex_l2r_ok) { - LOG(FATAL) << "Image params not found, but we need it to process the " - "images. Please `make tools` and use `img_params_writer` " - "to write the image params. If you update the SDK from " - "1.x, the `SN*.conf` is the file contains them. Besides, " - "you could also calibrate them by yourself. Read the guide " - "doc (https://github.com/slightech/MYNT-EYE-SDK-2-Guide) " - "to learn more."; - } - } + std::dynamic_pointer_cast(device_); synthetic_.reset(new Synthetic(this)); } @@ -283,6 +269,7 @@ bool API::Supports(const AddOns &addon) const { void API::SetStreamRequest( const Resolution &res, const Format &format, const FrameRate &rate) { device_->SetStreamRequest(res, format, rate); + CheckImageParams(); } const std::vector &API::GetStreamRequests( @@ -452,4 +439,22 @@ std::shared_ptr API::device() { return device_; } +void API::CheckImageParams() { + if (device_ != nullptr) { + bool in_l_ok, in_r_ok, ex_l2r_ok; + device_->GetIntrinsics(Stream::LEFT, &in_l_ok); + device_->GetIntrinsics(Stream::RIGHT, &in_r_ok); + device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT, &ex_l2r_ok); + if (!in_l_ok || !in_r_ok || !ex_l2r_ok) { + LOG(FATAL) << "Image params not found, but we need it to process the " + "images. Please `make tools` and use `img_params_writer` " + "to write the image params. If you update the SDK from " + "1.x, the `SN*.conf` is the file contains them. Besides, " + "you could also calibrate them by yourself. Read the guide " + "doc (https://github.com/slightech/MYNT-EYE-SDK-2-Guide) " + "to learn more."; + } + } +} + MYNTEYE_END_NAMESPACE diff --git a/src/api/api.h b/src/api/api.h index bd3be97..5f17147 100644 --- a/src/api/api.h +++ b/src/api/api.h @@ -280,6 +280,8 @@ class MYNTEYE_API API { std::shared_ptr device_; std::unique_ptr synthetic_; + + void CheckImageParams(); }; MYNTEYE_END_NAMESPACE diff --git a/src/device/device.cc b/src/device/device.cc index f3a16bc..b92208e 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -435,6 +435,7 @@ std::vector Device::GetMotionDatas() { void Device::SetStreamRequest( const Resolution &res, const Format &format, const FrameRate &rate) { StreamRequest request(res, format, rate); + ConfigIntrinsics(res); request_ = request; } @@ -548,9 +549,8 @@ void Device::ReadAllInfos() { device_info_ = std::make_shared(); CHECK_NOTNULL(channels_); - Channels::img_params_t img_params; Channels::imu_params_t imu_params; - if (!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. Please upgrade your firmware to " "the latest version."; } @@ -566,12 +566,8 @@ void Device::ReadAllInfos() { << ", nominal_baseline: " << device_info_->nominal_baseline << "}"; device_info_->name = uvc::get_name(*device_); - if (img_params.ok) { - SetIntrinsics(Stream::LEFT, img_params.in_left); - SetIntrinsics(Stream::RIGHT, img_params.in_right); - SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params.ex_left_to_right); - VLOG(2) << "Intrinsics left: {" << GetIntrinsics(Stream::LEFT) << "}"; - VLOG(2) << "Intrinsics right: {" << GetIntrinsics(Stream::RIGHT) << "}"; + if (img_params_.ok) { + SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params_.ex_left_to_right); VLOG(2) << "Extrinsics left to right: {" << GetExtrinsics(Stream::LEFT, Stream::RIGHT) << "}"; } else { @@ -588,6 +584,15 @@ void Device::ReadAllInfos() { } } +void Device::ConfigIntrinsics(const Resolution &res) { + if (img_params_.ok) { + SetIntrinsics(Stream::LEFT, img_params_.in_left_map[res]); + SetIntrinsics(Stream::RIGHT, img_params_.in_right_map[res]); + VLOG(2) << "Intrinsics left: {" << GetIntrinsics(Stream::LEFT) << "}"; + VLOG(2) << "Intrinsics right: {" << GetIntrinsics(Stream::RIGHT) << "}"; + } +} + void Device::CallbackPushedStreamData(const Stream &stream) { if (HasStreamCallback(stream)) { auto &&datas = streams_->stream_datas(stream); @@ -611,4 +616,8 @@ void Device::CallbackMotionData(const device::MotionData &data) { } } +Channels::img_params_t Device::GetImgParams() { + return img_params_; +} + MYNTEYE_END_NAMESPACE diff --git a/src/device/device.h b/src/device/device.h index 6f84310..ae9d608 100644 --- a/src/device/device.h +++ b/src/device/device.h @@ -22,6 +22,7 @@ #include #include +#include "internal/channels.h" #include "mynteye/callbacks.h" #include "mynteye/mynteye.h" #include "mynteye/types.h" @@ -256,6 +257,10 @@ class MYNTEYE_API Device { * Get the motion datas. */ std::vector GetMotionDatas(); + /** + * Get the device img params + */ + Channels::img_params_t GetImgParams(); protected: std::shared_ptr device() const { @@ -301,6 +306,7 @@ class MYNTEYE_API Device { std::shared_ptr motion_intrinsics_; std::map motion_from_extrinsics_; + Channels::img_params_t img_params_; stream_callbacks_t stream_callbacks_; motion_callback_t motion_callback_; @@ -319,6 +325,8 @@ class MYNTEYE_API Device { void ReadAllInfos(); + void ConfigIntrinsics(const Resolution &res); + void CallbackPushedStreamData(const Stream &stream); void CallbackMotionData(const device::MotionData &data); diff --git a/src/internal/channels.cc b/src/internal/channels.cc index 3426a6e..06bf1d1 100644 --- a/src/internal/channels.cc +++ b/src/internal/channels.cc @@ -104,7 +104,7 @@ void CheckSpecVersion(const Version *spec_version) { std::vector spec_versions{"1.0"}; for (auto &&spec_ver : spec_versions) { - if (*spec_version == Version(spec_ver)) { + if (*spec_version >= Version(spec_ver)) { return; // supported } } @@ -554,8 +554,33 @@ std::size_t from_data( Channels::img_params_t *img_params, const std::uint8_t *data, const Version *spec_version) { std::size_t i = 0; - i += from_data(&img_params->in_left, data + i, spec_version); - i += from_data(&img_params->in_right, data + i, spec_version); + + if (spec_version->major() == 1) { + if (spec_version->minor() == 0) { + i += from_data( + &img_params->in_left_map[Resolution::RES_752x480], data + i, + spec_version); + i += from_data( + &img_params->in_right_map[Resolution::RES_752x480], data + i, + spec_version); + } + + if (spec_version->minor() == 1) { + i += from_data( + &img_params->in_left_map[Resolution::RES_1280x400], data + i, + spec_version); + i += from_data( + &img_params->in_right_map[Resolution::RES_1280x400], data + i, + spec_version); + i += from_data( + &img_params->in_left_map[Resolution::RES_2560x800], data + i, + spec_version); + i += from_data( + &img_params->in_right_map[Resolution::RES_2560x800], data + i, + spec_version); + } + } + i += from_data(&img_params->ex_left_to_right, data + i, spec_version); return i; } @@ -826,8 +851,34 @@ std::size_t to_data( const Channels::img_params_t *img_params, std::uint8_t *data, const Version *spec_version) { std::size_t i = 3; // skip id, size - i += to_data(&img_params->in_left, data + i, spec_version); - i += to_data(&img_params->in_right, data + i, spec_version); + + if (spec_version->major() == 1) { + if (spec_version->minor() == 0) { + i += to_data( + &img_params->in_left_map.at(Resolution::RES_752x480), data + i, + spec_version); + i += to_data( + &img_params->in_right_map.at(Resolution::RES_752x480), data + i, + spec_version); + } + + if (spec_version->minor() == 1) { + i += to_data( + &img_params->in_left_map.at(Resolution::RES_1280x400), data + i, + spec_version); + i += to_data( + &img_params->in_right_map.at(Resolution::RES_1280x400), data + i, + spec_version); + + i += to_data( + &img_params->in_left_map.at(Resolution::RES_2560x800), data + i, + spec_version); + i += to_data( + &img_params->in_right_map.at(Resolution::RES_2560x800), data + i, + spec_version); + } + } + i += to_data(&img_params->ex_left_to_right, data + i, spec_version); // others std::size_t size = i - 3; diff --git a/src/internal/channels.h b/src/internal/channels.h index c0f9de0..c2f5664 100644 --- a/src/internal/channels.h +++ b/src/internal/channels.h @@ -70,8 +70,8 @@ class MYNTEYE_API Channels { typedef struct ImgParams { bool ok; - Intrinsics in_left; - Intrinsics in_right; + std::map in_left_map; + std::map in_right_map; Extrinsics ex_left_to_right; } img_params_t; diff --git a/src/internal/config.cc b/src/internal/config.cc index 40df322..62def10 100644 --- a/src/internal/config.cc +++ b/src/internal/config.cc @@ -23,10 +23,9 @@ const std::map capabilities_supports_map = { const std::map option_supports_map = { {Model::STANDARD, - {Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST, Option::EXPOSURE_MODE, - Option::MAX_EXPOSURE_TIME, Option::ERASE_CHIP, Option::MIN_EXPOSURE_TIME, - Option::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE, - Option::ACCELEROMETER_LOW_PASS_FILTER, + {Option::BRIGHTNESS, Option::EXPOSURE_MODE, Option::ERASE_CHIP, + Option::MIN_EXPOSURE_TIME, Option::ACCELEROMETER_RANGE, + Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER, Option::GYROSCOPE_LOW_PASS_FILTER}}}; const std::map> diff --git a/src/public/types.cc b/src/public/types.cc index 7516d1b..8af31e4 100644 --- a/src/public/types.cc +++ b/src/public/types.cc @@ -114,6 +114,11 @@ const char *to_string(const Option &value) { CASE(HDR_MODE) CASE(ZERO_DRIFT_CALIBRATION) CASE(ERASE_CHIP) + CASE(MIN_EXPOSURE_TIME) + CASE(ACCELEROMETER_RANGE) + CASE(GYROSCOPE_RANGE) + CASE(ACCELEROMETER_LOW_PASS_FILTER) + CASE(GYROSCOPE_LOW_PASS_FILTER) default: CHECK(is_valid(value)); return "Option::UNKNOWN"; diff --git a/tools/dataset/CMakeLists.txt b/tools/dataset/CMakeLists.txt index 33d8809..22dae35 100644 --- a/tools/dataset/CMakeLists.txt +++ b/tools/dataset/CMakeLists.txt @@ -20,6 +20,10 @@ set_outdir( "${OUT_DIR}/bin/${DIR_NAME}" ) +include_directories( + ${PRO_DIR}/src +) + ## record make_executable(record diff --git a/tools/writer/config/img.params b/tools/writer/config/img.params index 54fbb10..97d6681 100644 --- a/tools/writer/config/img.params +++ b/tools/writer/config/img.params @@ -1,6 +1,6 @@ %YAML:1.0 --- -in_left: +in_left_map: - width: 640 height: 400 @@ -11,7 +11,17 @@ in_left: model: 0 coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02, 2.4914898631983504e-03, -3.7752063658256863e-03, 0. ] -in_right: + - + width: 1280 + height: 800 + fx: 1.9739641213416058e+02 + fy: 1.9772337597617189e+02 + cx: 3.2611983633916327e+02 + cy: 1.9986969132833946e+02 + model: 0 + coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02, + 2.4914898631983504e-03, -3.7752063658256863e-03, 0. ] +in_right_map: - width: 640 height: 400 @@ -22,6 +32,16 @@ in_right: model: 0 coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02, 3.9725942760981507e-03, -3.9689073214945591e-03, 0. ] + - + width: 1280 + height: 800 + fx: 2.0335498653655989e+02 + fy: 2.0453858622699008e+02 + cx: 3.1589962248180814e+02 + cy: 2.1871688038954812e+02 + model: 0 + coeffs: [ 2.2904330559241560e-02, -2.9561990079971841e-02, + 3.9725942760981507e-03, -3.9689073214945591e-03, 0. ] ex_left_to_right: rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03, -4.3917309443490191e-03, 1.8166060642710027e-03, diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index 7c57456..6e51eb1 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -17,10 +17,14 @@ #include +#include #include #include "mynteye/device.h" #include "mynteye/files.h" +#include "mynteye/types.h" + +#include "internal/types.h" MYNTEYE_BEGIN_NAMESPACE @@ -70,8 +74,8 @@ bool DeviceWriter::WriteImgParams(const img_params_t ¶ms) { nullptr, const_cast(¶ms), nullptr, &dev_info->spec_version)) { LOG(INFO) << "Write img params success"; - LOG(INFO) << "Intrinsics left: {" << params.in_left << "}"; - LOG(INFO) << "Intrinsics right: {" << params.in_right << "}"; + // LOG(INFO) << "Intrinsics left: {" << params.in_left << "}"; + // LOG(INFO) << "Intrinsics right: {" << params.in_right << "}"; LOG(INFO) << "Extrinsics left to right: {" << params.ex_left_to_right << "}"; return true; @@ -184,11 +188,33 @@ bool DeviceWriter::SaveImgParams( LOG(ERROR) << "Failed to save file: " << filepath; return false; } - fs << "in_left" << std::vector{params.in_left} << "in_right" - << std::vector{params.in_right} << "ex_left_to_right" - << params.ex_left_to_right; - fs.release(); - return true; + + std::vector v_left; + std::vector v_right; + std::map::const_iterator it; + + for (it = params.in_left_map.begin(); it != params.in_left_map.end(); it++) + v_left.push_back((*it).second); + + for (it = params.in_right_map.begin(); it != params.in_right_map.end(); it++) + v_right.push_back((*it).second); + + if (v_left.size() == v_right.size() && v_left.size() > 0) { + fs << "in_left_map"; + for (unsigned int i = 0; i < v_left.size(); i++) + fs << v_left[i]; + + fs << "in_right_map"; + for (unsigned int i = 0; i < v_right.size(); i++) + fs << v_right[i]; + + fs << "ex_left_to_right" << params.ex_left_to_right; + fs.release(); + return true; + } else { + fs.release(); + return false; + } } bool DeviceWriter::SaveImuParams( @@ -210,11 +236,7 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) { LOG(FATAL) << "Create directory failed: " << dir; } SaveDeviceInfo(*device_->GetInfo(), dir + OS_SEP "device.info"); - SaveImgParams( - {false, device_->GetIntrinsics(Stream::LEFT), - device_->GetIntrinsics(Stream::RIGHT), - device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)}, - dir + OS_SEP "img.params"); + SaveImgParams(device_->GetImgParams(), dir + OS_SEP "img.params"); auto &&m_in = device_->GetMotionIntrinsics(); SaveImuParams( { @@ -327,34 +349,50 @@ DeviceWriter::img_params_t DeviceWriter::LoadImgParams( } img_params_t params; - if (fs["in_left"].isNone()) { - std::uint16_t w = 640; - std::uint16_t h = 400; - std::uint8_t m = 0; - if (!fs["width"].isNone()) - w = static_cast(fs["width"]); - if (!fs["height"].isNone()) - h = static_cast(fs["height"]); - if (!fs["model"].isNone()) - m = static_cast(fs["model"]); + if (fs["version"].isNone()) { + if (fs["in_left"].isNone()) { + std::uint16_t w = 752; + std::uint16_t h = 480; + std::uint8_t m = 0; + if (!fs["width"].isNone()) + w = static_cast(fs["width"]); + if (!fs["height"].isNone()) + h = static_cast(fs["height"]); + if (!fs["model"].isNone()) + m = static_cast(fs["model"]); - cv::Mat M1, D1, M2, D2, R, T; - fs["M1"] >> M1; - fs["D1"] >> D1; - fs["M2"] >> M2; - fs["D2"] >> D2; - fs["R"] >> R; - fs["T"] >> T; + cv::Mat M1, D1, M2, D2, R, T; + fs["M1"] >> M1; + fs["D1"] >> D1; + fs["M2"] >> M2; + fs["D2"] >> D2; + fs["R"] >> R; + fs["T"] >> T; - to_intrinsics(w, h, m, M1, D1, ¶ms.in_left); - to_intrinsics(w, h, m, M2, D2, ¶ms.in_right); - to_extrinsics(R, T, ¶ms.ex_left_to_right); + to_intrinsics( + w, h, m, M1, D1, ¶ms.in_left_map[Resolution::RES_752x480]); + to_intrinsics( + w, h, m, M2, D2, ¶ms.in_right_map[Resolution::RES_752x480]); + to_extrinsics(R, T, ¶ms.ex_left_to_right); + } else { + fs["in_left"][0] >> params.in_left_map[Resolution::RES_752x480]; + fs["in_right"][0] >> params.in_right_map[Resolution::RES_752x480]; + fs["ex_left_to_right"] >> params.ex_left_to_right; + } } else { - fs["in_left"][0] >> params.in_left; - fs["in_right"][0] >> params.in_right; - fs["ex_left_to_right"] >> params.ex_left_to_right; + if (static_cast(fs["version"]) == 1.0) { + fs["in_left_map"][0] >> params.in_left_map[Resolution::RES_752x480]; + fs["in_right_map"][0] >> params.in_right_map[Resolution::RES_752x480]; + fs["ex_left_to_right"] >> params.ex_left_to_right; + } + if (static_cast(fs["version"]) == 1.1) { + fs["in_left_map"][0] >> params.in_left_map[Resolution::RES_1280x400]; + fs["in_left_map"][1] >> params.in_left_map[Resolution::RES_2560x800]; + fs["in_right_map"][0] >> params.in_right_map[Resolution::RES_1280x400]; + fs["in_right_map"][1] >> params.in_left_map[Resolution::RES_2560x800]; + fs["ex_left_to_right"] >> params.ex_left_to_right; + } } - fs.release(); return params; }