From ed45497d6c3c4c6f45ef84a59304bc90cb738534 Mon Sep 17 00:00:00 2001 From: kalman Date: Wed, 26 Dec 2018 19:58:37 +0800 Subject: [PATCH] fix(tools): fix complile bug --- tools/writer/config/S1030/device.info | 10 ++ tools/writer/config/S1030/img.params | 33 ++++++ .../writer/config/{ => S1030}/img.params.old | 0 tools/writer/config/S1030/imu.params | 24 ++++ tools/writer/config/{ => S210A}/device.info | 6 +- tools/writer/config/S210A/img.params | 62 ++++++++++ tools/writer/config/S210A/imu.params | 15 +++ tools/writer/config/img.params | 53 --------- tools/writer/config/imu.params | 15 --- tools/writer/device_writer.cc | 109 +++++++++--------- tools/writer/device_writer.h | 10 +- 11 files changed, 206 insertions(+), 131 deletions(-) create mode 100644 tools/writer/config/S1030/device.info create mode 100644 tools/writer/config/S1030/img.params rename tools/writer/config/{ => S1030}/img.params.old (100%) create mode 100644 tools/writer/config/S1030/imu.params rename tools/writer/config/{ => S210A}/device.info (60%) create mode 100644 tools/writer/config/S210A/img.params create mode 100644 tools/writer/config/S210A/imu.params delete mode 100644 tools/writer/config/img.params delete mode 100644 tools/writer/config/imu.params diff --git a/tools/writer/config/S1030/device.info b/tools/writer/config/S1030/device.info new file mode 100644 index 0000000..0c9e26b --- /dev/null +++ b/tools/writer/config/S1030/device.info @@ -0,0 +1,10 @@ +%YAML:1.0 +--- +device_name: MYNT-EYE-S1030 +serial_number: "02811F3B00090807" +firmware_version: "2.2" +hardware_version: "2.0" +spec_version: "1.0" +lens_type: "0000" +imu_type: "0000" +nominal_baseline: 120 diff --git a/tools/writer/config/S1030/img.params b/tools/writer/config/S1030/img.params new file mode 100644 index 0000000..28d2f6a --- /dev/null +++ b/tools/writer/config/S1030/img.params @@ -0,0 +1,33 @@ +%YAML:1.0 +--- +version: "1.0" +img_params_map: + - + in_left: + width: 752 + height: 480 + fx: 3.6220059643202876e+02 + fy: 3.6350065250745848e+02 + cx: 4.0658699068023441e+02 + cy: 2.3435161110061483e+02 + model: 0 + coeffs: [ -2.5034765682756088e-01, 5.0579399202897619e-02, + -7.0536676161976066e-04, -8.5255451307033846e-03, 0. ] + in_right: + width: 752 + height: 480 + fx: 3.6514014888558478e+02 + fy: 3.6513385298966961e+02 + cx: 3.8932395100630907e+02 + cy: 2.3495160212312547e+02 + model: 0 + coeffs: [ -3.0377346762098512e-01, 7.9929693673999838e-02, + 5.1547517530716883e-05, -6.7345903740579250e-04, 0. ] + ex_right_to_left: + rotation: [ 9.9867908939669447e-01, -6.3445566137485428e-03, + 5.0988459509619687e-02, 5.9890316389333252e-03, + 9.9995670037792639e-01, 7.1224201868366971e-03, + -5.1031440326695092e-02, -6.8076406092671274e-03, + 9.9867384471984544e-01 ] + translation: [ -1.2002489764113250e+02, -1.1782637409050747e+00, + -5.2058205159996538e+00 ] diff --git a/tools/writer/config/img.params.old b/tools/writer/config/S1030/img.params.old similarity index 100% rename from tools/writer/config/img.params.old rename to tools/writer/config/S1030/img.params.old diff --git a/tools/writer/config/S1030/imu.params b/tools/writer/config/S1030/imu.params new file mode 100644 index 0000000..4500d4e --- /dev/null +++ b/tools/writer/config/S1030/imu.params @@ -0,0 +1,24 @@ +%YAML:1.0 +--- +in_accel: + scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] + drift: [ 0., 0., 0. ] + noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02, + 1.7452487504590969e-02 ] + bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04, + 5.4490537096493644e-04 ] +in_gyro: + scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] + drift: [ 0., 0., 0. ] + noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03, + 1.1003229919806443e-03 ] + bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05, + 1.4970418056326829e-05 ] +ex_left_to_imu: + rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01, + -7.6356499999999999e-03, 9.9997908999999996e-01, + -6.4656599999999998e-03, -9.5580000000000003e-05, + 4.6199999999999998e-05, -7.6361099999999998e-03, + 9.9997084000000003e-01 ] + translation: [ 5.3364600000000003e-03, -4.3029220000000000e-02, + 2.3031240000000001e-02 ] diff --git a/tools/writer/config/device.info b/tools/writer/config/S210A/device.info similarity index 60% rename from tools/writer/config/device.info rename to tools/writer/config/S210A/device.info index 59bd67a..6bce65d 100644 --- a/tools/writer/config/device.info +++ b/tools/writer/config/S210A/device.info @@ -1,10 +1,10 @@ %YAML:1.0 --- device_name: MYNT-EYE-S210A -serial_number: "0386322C0009070E" -firmware_version: "1.0" +serial_number: "07C40D1C0009071F" +firmware_version: "0.1" hardware_version: "1.0" spec_version: "1.1" lens_type: "0000" imu_type: "0000" -nominal_baseline: 120 +nominal_baseline: 0 diff --git a/tools/writer/config/S210A/img.params b/tools/writer/config/S210A/img.params new file mode 100644 index 0000000..a368dbb --- /dev/null +++ b/tools/writer/config/S210A/img.params @@ -0,0 +1,62 @@ +%YAML:1.0 +--- +version: "1.1" +img_params_map: + - + in_left: + width: 640 + height: 400 + 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: + width: 640 + height: 400 + 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_right_to_left: + rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03, + -4.3917309443490191e-03, 1.8166060642710027e-03, + 9.9968925981619028e-01, -2.4861290203142431e-02, + 4.4382582477776426e-03, 2.4853026274046636e-02, + 9.9968126367795229e-01 ] + translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00, + 2.2588034344482368e+00 ] + - + in_left: + 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: + 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_right_to_left: + rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03, + -4.3917309443490191e-03, 1.8166060642710027e-03, + 9.9968925981619028e-01, -2.4861290203142431e-02, + 4.4382582477776426e-03, 2.4853026274046636e-02, + 9.9968126367795229e-01 ] + translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00, + 2.2588034344482368e+00 ] diff --git a/tools/writer/config/S210A/imu.params b/tools/writer/config/S210A/imu.params new file mode 100644 index 0000000..070f13a --- /dev/null +++ b/tools/writer/config/S210A/imu.params @@ -0,0 +1,15 @@ +%YAML:1.0 +--- +in_accel: + scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] + drift: [ 0., 0., 0. ] + noise: [ 0., 0., 0. ] + bias: [ 0., 0., 0. ] +in_gyro: + scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] + drift: [ 0., 0., 0. ] + noise: [ 0., 0., 0. ] + bias: [ 0., 0., 0. ] +ex_left_to_imu: + rotation: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] + translation: [ 0., 0., 0. ] diff --git a/tools/writer/config/img.params b/tools/writer/config/img.params deleted file mode 100644 index f1af6d7..0000000 --- a/tools/writer/config/img.params +++ /dev/null @@ -1,53 +0,0 @@ -%YAML:1.0 ---- -version: "1.1" -in_left_map: - - - width: 640 - height: 400 - 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. ] - - - 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 - 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. ] - - - 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_right_to_left: - rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03, - -4.3917309443490191e-03, 1.8166060642710027e-03, - 9.9968925981619028e-01, -2.4861290203142431e-02, - 4.4382582477776426e-03, 2.4853026274046636e-02, - 9.9968126367795229e-01 ] - translation: [ -8.2270200890555529e+01, -1.9535144360069059e+00, - 2.2588034344482368e+00 ] diff --git a/tools/writer/config/imu.params b/tools/writer/config/imu.params deleted file mode 100644 index b80f2c4..0000000 --- a/tools/writer/config/imu.params +++ /dev/null @@ -1,15 +0,0 @@ -%YAML:1.0 ---- -in_accel: - scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] - drift: [ 0.0, 0.0, 0.0 ] - noise: [ 0.016925432397973516, 0.016735310195561025, 0.017452487504590969 ] - bias: [ 0.00019031356589714596, 0.00016996777864587261, 0.00054490537096493644 ] -in_gyro: - scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] - drift: [ 0., 0., 0. ] - noise: [ 0.0010848026158819934, 0.0012466367883501759, 0.0011003229919806443 ] - bias: [ 0.000023404834136742844, 0.000023596771567764949, 0.000014970418056326829 ] -ex_left_to_imu: - rotation: [ -0.0064662, -0.99994994, -0.00763565, 0.99997909, -0.00646566, -0.00009558, 0.0000462, -0.00763611, 0.99997084 ] - translation: [ 0.00533646, -0.04302922, 0.02303124 ] diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index 0d834ef..fb38c3e 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -13,7 +13,6 @@ // limitations under the License. #include "writer/device_writer.h" -#include #include #include @@ -63,17 +62,22 @@ bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) { return WriteDeviceInfo(LoadDeviceInfo(filepath)); } -bool DeviceWriter::WriteImgParams(const img_params_t ¶ms) { +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(¶ms), nullptr, + nullptr, const_cast(&img_params_map), 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) << "Extrinsics left to right: {" << params.ex_right_to_left - << "}"; + std::map::const_iterator it; + for (it = img_params_map.begin(); it != img_params_map.end(); it++) { + LOG(INFO) << "Image params for resolution " + << (*it).first.width << "x" << (*it).first.height << " :"; + LOG(INFO) << "Intrinsics left: {" << (*it).second.in_left << "}"; + LOG(INFO) << "Intrinsics right: {" << (*it).second.in_right << "}"; + LOG(INFO) << "Extrinsics left to right: {" + << (*it).second.ex_right_to_left << "}"; + } return true; } else { LOG(ERROR) << "Write img params failed"; @@ -117,25 +121,6 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Intrinsics &in) { return fs; } -cv::FileStorage &operator<<( - cv::FileStorage &fs, const std::vector &vec) { - fs << "["; - for (auto &&in : vec) - fs << in; - fs << "]"; - return fs; -} - -cv::FileStorage &operator<<( - cv::FileStorage &fs, const std::map &mapIn) { - fs << "["; - std::map::const_iterator it; - for (it = mapIn.begin(); it != mapIn.end(); it++) - fs << (*it).second; - fs << "]"; - return fs; -} - cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) { std::vector scales; for (std::size_t i = 0; i < 3; i++) { @@ -164,6 +149,25 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) { return fs; } +cv::FileStorage &operator<<( + cv::FileStorage &fs, const device::img_params_t ¶ms) { + fs << "{" + << "in_left" << params.in_left + << "in_right" << params.in_right + << "ex_right_to_left" << params.ex_right_to_left << "}"; + return fs; +} + +cv::FileStorage &operator<<( + cv::FileStorage &fs, const DeviceWriter::img_params_map_t &img_params_map) { + fs << "["; + std::map::const_iterator it; + for (it = img_params_map.begin(); it != img_params_map.end(); it++) + fs << (*it).second; + fs << "]"; + return fs; +} + } // namespace bool DeviceWriter::SaveDeviceInfo( @@ -187,7 +191,7 @@ bool DeviceWriter::SaveDeviceInfo( } bool DeviceWriter::SaveImgParams( - const dev_info_t &info, const img_params_t ¶ms, + const dev_info_t &info, const img_params_map_t &img_params_map, const std::string &filepath) { using FileStorage = cv::FileStorage; FileStorage fs(filepath, FileStorage::WRITE); @@ -195,17 +199,10 @@ bool DeviceWriter::SaveImgParams( LOG(ERROR) << "Failed to save file: " << filepath; return false; } - - if (params.in_left_map.size() == params.in_right_map.size()) { - fs << "version" << info.spec_version.to_string() << "in_left_map" - << params.in_left_map << "in_right_map" << params.in_right_map - << "ex_right_to_left" << params.ex_right_to_left; - fs.release(); - return true; - } else { - fs.release(); - return false; - } + fs << "version" << info.spec_version.to_string(); + fs << "img_params_map" << img_params_map; + fs.release(); + return true; } bool DeviceWriter::SaveImuParams( @@ -316,6 +313,12 @@ void operator>>(const cv::FileNode &n, Extrinsics &ex) { } } +void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t ¶mas) { + n["in_left"] >> paramas.in_left; + n["in_right"] >> paramas.in_right; + n["ex_right_to_left"] >> paramas.ex_right_to_left; +} + } // namespace DeviceWriter::dev_info_t DeviceWriter::LoadDeviceInfo( @@ -333,7 +336,7 @@ DeviceWriter::dev_info_t DeviceWriter::LoadDeviceInfo( return info; } -DeviceWriter::img_params_t DeviceWriter::LoadImgParams( +DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams( const std::string &filepath) { using FileStorage = cv::FileStorage; FileStorage fs(filepath, FileStorage::READ); @@ -341,7 +344,7 @@ DeviceWriter::img_params_t DeviceWriter::LoadImgParams( LOG(FATAL) << "Failed to load file: " << filepath; } - img_params_t params; + img_params_map_t img_params_map; if (fs["version"].isNone()) { if (fs["in_left"].isNone()) { std::uint16_t w = 752; @@ -363,32 +366,26 @@ DeviceWriter::img_params_t DeviceWriter::LoadImgParams( fs["T"] >> T; to_intrinsics( - w, h, m, M1, D1, ¶ms.in_left_map[Resolution::RES_752x480]); + w, h, m, M1, D1, &img_params_map[{752, 480}].in_left); to_intrinsics( - w, h, m, M2, D2, ¶ms.in_right_map[Resolution::RES_752x480]); - to_extrinsics(R, T, ¶ms.ex_right_to_left); + w, h, m, M2, D2, &img_params_map[{752, 480}].in_right); + to_extrinsics(R, T, &img_params_map[{752, 480}].ex_right_to_left); } 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_right_to_left"] >> params.ex_right_to_left; + fs["in_left"][0] >> img_params_map[{752, 480}].in_left; + fs["in_right"][0] >> img_params_map[{752, 480}].in_right; + fs["ex_right_to_left"] >> img_params_map[{752, 480}].ex_right_to_left; } } else { - // TODO(Kalman): Is there a more reasonable way? 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_right_to_left"] >> params.ex_right_to_left; + fs["img_params_map"][0] >> img_params_map[{752, 480}]; } 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_right_map[Resolution::RES_2560x800]; - fs["ex_right_to_left"] >> params.ex_right_to_left; + fs["img_params_map"][0] >> img_params_map[{1280, 400}]; + fs["img_params_map"][1] >> img_params_map[{2560, 800}]; } } fs.release(); - return params; + return img_params_map; } DeviceWriter::imu_params_t DeviceWriter::LoadImuParams( diff --git a/tools/writer/device_writer.h b/tools/writer/device_writer.h index 5e443e0..80059fe 100644 --- a/tools/writer/device_writer.h +++ b/tools/writer/device_writer.h @@ -15,6 +15,7 @@ #define MYNTEYE_TOOLS_DEVICE_WRITER_H_ #pragma once +#include #include #include @@ -31,8 +32,9 @@ namespace tools { class DeviceWriter { public: using dev_info_t = DeviceInfo; - using img_params_t = device::img_params_t; using imu_params_t = device::imu_params_t; + using img_params_t = device::img_params_t; + using img_params_map_t = std::map; explicit DeviceWriter(std::shared_ptr device); ~DeviceWriter(); @@ -40,7 +42,7 @@ class DeviceWriter { bool WriteDeviceInfo(const dev_info_t &info); bool WriteDeviceInfo(const std::string &filepath); - bool WriteImgParams(const img_params_t ¶ms); + bool WriteImgParams(const img_params_map_t &img_params_map); bool WriteImgParams(const std::string &filepath); bool WriteImuParams(const imu_params_t ¶ms); @@ -48,7 +50,7 @@ class DeviceWriter { bool SaveDeviceInfo(const dev_info_t &info, const std::string &filepath); bool SaveImgParams( - const dev_info_t &info, const img_params_t ¶ms, + const dev_info_t &info, const img_params_map_t &img_params_map, const std::string &filepath); bool SaveImuParams(const imu_params_t ¶ms, const std::string &filepath); @@ -57,7 +59,7 @@ class DeviceWriter { private: dev_info_t LoadDeviceInfo(const std::string &filepath); - img_params_t LoadImgParams(const std::string &filepath); + img_params_map_t LoadImgParams(const std::string &filepath); imu_params_t LoadImuParams(const std::string &filepath); std::shared_ptr device_;