diff --git a/include/mynteye/types.h b/include/mynteye/types.h index a8bb6d4..ced9afb 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -434,6 +434,9 @@ struct MYNTEYE_API IntrinsicsBase { CalibrationModel calib_model_; }; +MYNTEYE_API +std::ostream &operator<<(std::ostream &os, const IntrinsicsBase &in); + /** * @ingroup calibration * Stream intrinsics (Pinhole) diff --git a/src/mynteye/device/types.cc b/src/mynteye/device/types.cc index f29ce8e..36855d4 100644 --- a/src/mynteye/device/types.cc +++ b/src/mynteye/device/types.cc @@ -43,6 +43,7 @@ std::vector Version::split(const std::string &s) { } Version::value_t Version::parse_part(const std::string &name, size_t part) { + if (name.empty()) return 0; return std::stoi(split(name)[part]); } diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 915a2f8..e723031 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -201,6 +201,17 @@ const char *to_string(const CalibrationModel &model) { #undef CASE } +std::ostream &operator<<(std::ostream &os, const IntrinsicsBase &in) { + switch (in.calib_model()) { + case CalibrationModel::PINHOLE: + return os << dynamic_cast(in); + case CalibrationModel::KANNALA_BRANDT: + return os << dynamic_cast(in); + default: + return os << "unknown calib model"; + } +} + std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) { os << "pinhole, " << FULL_PRECISION << "width: " << in.width << ", height: " << in.height diff --git a/tools/writer/config/S1030/img.params.old b/tools/writer/config/S1030/deprecated/img.params.old similarity index 100% rename from tools/writer/config/S1030/img.params.old rename to tools/writer/config/S1030/deprecated/img.params.old diff --git a/tools/writer/config/S1030/deprecated/img.params.old2 b/tools/writer/config/S1030/deprecated/img.params.old2 new file mode 100644 index 0000000..350a171 --- /dev/null +++ b/tools/writer/config/S1030/deprecated/img.params.old2 @@ -0,0 +1,30 @@ +%YAML:1.0 +--- +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/S1030/img.params b/tools/writer/config/S1030/deprecated/img.params.old3 similarity index 100% rename from tools/writer/config/S1030/img.params rename to tools/writer/config/S1030/deprecated/img.params.old3 diff --git a/tools/writer/config/S1030/deprecated/imu.params.old b/tools/writer/config/S1030/deprecated/imu.params.old new file mode 100644 index 0000000..4500d4e --- /dev/null +++ b/tools/writer/config/S1030/deprecated/imu.params.old @@ -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/S1030/img.params.equidistant b/tools/writer/config/S1030/img.params.equidistant new file mode 100644 index 0000000..06600b5 --- /dev/null +++ b/tools/writer/config/S1030/img.params.equidistant @@ -0,0 +1,28 @@ +%YAML:1.0 +--- +version: "1.2" +img_params: + - + model: 1 + width: 752 + height: 480 + in_left: + # coeffs: k2,k3,k4,k5,mu,mv,u0,v0 + coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02, + 5.3814579203036512e-02, -3.6663721418756977e-02, + 3.6797709792391299e+02, 3.6808712539453859e+02, + 3.7414963027144353e+02, 2.3125000326472903e+02 ] + in_right: + # coeffs: k2,k3,k4,k5,mu,mv,u0,v0 + coeffs: [ -1.8964880945048872e-02, -2.6172447729615463e-02, + 4.0863326732708288e-02, -2.1522718578606163e-02, + 3.6807315003638877e+02, 3.6806204885853174e+02, + 3.8499216569965887e+02, 2.3183027347834383e+02 ] + 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/S1030/img.params.pinhole b/tools/writer/config/S1030/img.params.pinhole new file mode 100644 index 0000000..a375acc --- /dev/null +++ b/tools/writer/config/S1030/img.params.pinhole @@ -0,0 +1,38 @@ +%YAML:1.0 +--- +version: "1.2" +img_params: + - + model: 0 + width: 752 + height: 480 + in_left: + # [fx 0 cx] + # [ 0 fy cy] + # [ 0 0 1] + fx: 3.6220059643202876e+02 + fy: 3.6350065250745848e+02 + cx: 4.0658699068023441e+02 + cy: 2.3435161110061483e+02 + # coeffs: k1,k2,p1,p2,k3 + coeffs: [ -2.5034765682756088e-01, 5.0579399202897619e-02, + -7.0536676161976066e-04, -8.5255451307033846e-03, 0. ] + in_right: + # [fx 0 cx] + # [ 0 fy cy] + # [ 0 0 1] + fx: 3.6514014888558478e+02 + fy: 3.6513385298966961e+02 + cx: 3.8932395100630907e+02 + cy: 2.3495160212312547e+02 + # coeffs: k1,k2,p1,p2,k3 + 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/S1030/imu.params b/tools/writer/config/S1030/imu.params index 4500d4e..00993e2 100644 --- a/tools/writer/config/S1030/imu.params +++ b/tools/writer/config/S1030/imu.params @@ -1,5 +1,6 @@ %YAML:1.0 --- +version: "1.2" in_accel: scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] drift: [ 0., 0., 0. ] diff --git a/tools/writer/config/S210A/img.params b/tools/writer/config/S210A/deprecated/img.params.old similarity index 100% rename from tools/writer/config/S210A/img.params rename to tools/writer/config/S210A/deprecated/img.params.old diff --git a/tools/writer/config/S210A/deprecated/imu.params.old b/tools/writer/config/S210A/deprecated/imu.params.old new file mode 100644 index 0000000..070f13a --- /dev/null +++ b/tools/writer/config/S210A/deprecated/imu.params.old @@ -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/S210A/img.params.equidistant b/tools/writer/config/S210A/img.params.equidistant new file mode 100644 index 0000000..8949748 --- /dev/null +++ b/tools/writer/config/S210A/img.params.equidistant @@ -0,0 +1,51 @@ +%YAML:1.0 +--- +version: "1.2" +img_params: + # There are two params of resolution [640x400], [1280x800]. + # If you only set one of them, write will keep another params if it exists. + - + model: 1 + width: 640 + height: 400 + in_left: + # coeffs: k2,k3,k4,k5,mu,mv,u0,v0 + coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02, + 5.3814579203036512e-02, -3.6663721418756977e-02, + 3.6797709792391299e+02, 3.6808712539453859e+02, + 3.7414963027144353e+02, 2.3125000326472903e+02 ] + in_right: + coeffs: [ -1.8964880945048872e-02, -2.6172447729615463e-02, + 4.0863326732708288e-02, -2.1522718578606163e-02, + 3.6807315003638877e+02, 3.6806204885853174e+02, + 3.8499216569965887e+02, 2.3183027347834383e+02 ] + 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 ] + - + model: 1 + width: 1280 + height: 800 + in_left: + coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02, + 5.3814579203036512e-02, -3.6663721418756977e-02, + 3.6797709792391299e+02, 3.6808712539453859e+02, + 3.7414963027144353e+02, 2.3125000326472903e+02 ] + in_right: + coeffs: [ -1.3549950401924169e-02, -3.2306077263853893e-02, + 5.3814579203036512e-02, -3.6663721418756977e-02, + 3.6797709792391299e+02, 3.6808712539453859e+02, + 3.7414963027144353e+02, 2.3125000326472903e+02 ] + 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/img.params.pinhole b/tools/writer/config/S210A/img.params.pinhole new file mode 100644 index 0000000..eb1b2a7 --- /dev/null +++ b/tools/writer/config/S210A/img.params.pinhole @@ -0,0 +1,62 @@ +%YAML:1.0 +--- +version: "1.2" +img_params: + # There are two params of resolution [640x400], [1280x800]. + # If you only set one of them, write will keep another params if it exists. + - + model: 0 + width: 640 + height: 400 + in_left: + # [fx 0 cx] + # [ 0 fy cy] + # [ 0 0 1] + fx: 1.9739641213416058e+02 + fy: 1.9772337597617189e+02 + cx: 3.2611983633916327e+02 + cy: 1.9986969132833946e+02 + # coeffs: k1,k2,p1,p2,k3 + coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02, + 2.4914898631983504e-03, -3.7752063658256863e-03, 0. ] + in_right: + fx: 2.0335498653655989e+02 + fy: 2.0453858622699008e+02 + cx: 3.1589962248180814e+02 + cy: 2.1871688038954812e+02 + 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 ] + - + model: 0 + width: 1280 + height: 800 + in_left: + fx: 1.9739641213416058e+02 + fy: 1.9772337597617189e+02 + cx: 3.2611983633916327e+02 + cy: 1.9986969132833946e+02 + coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02, + 2.4914898631983504e-03, -3.7752063658256863e-03, 0. ] + in_right: + fx: 2.0335498653655989e+02 + fy: 2.0453858622699008e+02 + cx: 3.1589962248180814e+02 + cy: 2.1871688038954812e+02 + 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 index 070f13a..00993e2 100644 --- a/tools/writer/config/S210A/imu.params +++ b/tools/writer/config/S210A/imu.params @@ -1,15 +1,25 @@ %YAML:1.0 --- +version: "1.2" in_accel: - scale: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] + scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] drift: [ 0., 0., 0. ] - noise: [ 0., 0., 0. ] - bias: [ 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: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] + scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ] drift: [ 0., 0., 0. ] - noise: [ 0., 0., 0. ] - bias: [ 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: [ 0., 0., 0., 0., 0., 0., 0., 0., 0. ] - translation: [ 0., 0., 0. ] + 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/device_writer.cc b/tools/writer/device_writer.cc index 5fa37dd..a164dc0 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -37,10 +37,13 @@ DeviceWriter::~DeviceWriter() { bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) { auto &&channels = device_->channels(); + + // Update device info auto &&dev_info = device_->GetInfo(); dev_info->lens_type = Type(info.lens_type); dev_info->imu_type = Type(info.imu_type); dev_info->nominal_baseline = info.nominal_baseline; + if (channels->SetFiles(dev_info.get(), nullptr, nullptr)) { LOG(INFO) << "Write device info success"; LOG(INFO) << "Device info: {name: " << dev_info->name @@ -66,17 +69,26 @@ bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) { 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(&img_params_map), nullptr)) { + + img_params_map_t *img_params_new = + const_cast(&img_params_map); + // Update image params with raw + auto &&img_params_raw = device_->GetImgParams(); + for (auto entry_raw : img_params_raw) { + // Add raw params if not load this resolution + if (img_params_new->find(entry_raw.first) == img_params_new->end()) { + (*img_params_new)[entry_raw.first] = entry_raw.second; + } + } + + if (channels->SetFiles(nullptr, img_params_new, nullptr)) { LOG(INFO) << "Write img params success"; - 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: {" + for (auto it = img_params_new->begin(); it != img_params_new->end(); it++) { + LOG(INFO) << "Resolution: {width: " << (*it).first.width + << ", height: " << (*it).first.height << "}"; + LOG(INFO) << "Intrinsics left: {" << *(*it).second.in_left << "}"; + LOG(INFO) << "Intrinsics right: {" << *(*it).second.in_right << "}"; + LOG(INFO) << "Extrinsics right to left: {" << (*it).second.ex_right_to_left << "}"; } return true; @@ -92,7 +104,6 @@ bool DeviceWriter::WriteImgParams(const std::string &filepath) { bool DeviceWriter::WriteImuParams(const imu_params_t ¶ms) { auto &&channels = device_->channels(); - auto &&dev_info = device_->GetInfo(); if (channels->SetFiles( nullptr, nullptr, const_cast(¶ms))) { LOG(INFO) << "Write imu params success"; @@ -277,6 +288,7 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) { namespace { +// old void to_intrinsics( const std::uint16_t &width, const std::uint16_t &height, const std::uint8_t &model, const cv::Mat &M, const cv::Mat &D, @@ -300,6 +312,7 @@ void to_intrinsics( } } +// old void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) { for (std::size_t i = 0; i < 3; i++) { for (std::size_t j = 0; j < 3; j++) { @@ -310,7 +323,7 @@ void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) { ex->translation[i] = T.at(i); } } - +// old void operator>>(const cv::FileNode &n, IntrinsicsPinhole &in) { n["width"] >> in.width; n["height"] >> in.height; @@ -352,14 +365,76 @@ void operator>>(const cv::FileNode &n, Extrinsics &ex) { } } -void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t ¶mas) { +// old +void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t ¶ms) { auto in_left = std::make_shared(); auto in_right = std::make_shared(); - paramas.in_left = in_left; - paramas.in_right = in_right; + params.in_left = in_left; + params.in_right = in_right; n["in_left"] >> *in_left; n["in_right"] >> *in_right; - n["ex_right_to_left"] >> paramas.ex_right_to_left; + n["ex_right_to_left"] >> params.ex_right_to_left; +} + +std::shared_ptr to_intrinsics_pinhole( + const cv::FileNode &n, const std::uint8_t &model, + const std::uint16_t &width, const std::uint16_t &height) { + auto in = std::make_shared(); + in->width = width; + in->height = height; + in->model = model; + n["fx"] >> in->fx; + n["fy"] >> in->fy; + n["cx"] >> in->cx; + n["cy"] >> in->cy; + for (std::size_t i = 0; i < 5; i++) { + in->coeffs[i] = n["coeffs"][i]; + } + return in; +} + +std::shared_ptr to_intrinsics_equidistant( + const cv::FileNode &n, const std::uint8_t &model, + const std::uint16_t &width, const std::uint16_t &height) { + auto in = std::make_shared(); + in->width = width; + in->height = height; + for (std::size_t i = 0; i < 8; i++) { + in->coeffs[i] = n["coeffs"][i]; + } + MYNTEYE_UNUSED(model) + return in; +} + +DeviceWriter::img_params_t to_img_params( + const cv::FileNode &n, const std::uint8_t &model, + const std::uint16_t &width, const std::uint16_t &height) { + DeviceWriter::img_params_t params; + params.ok = false; + + CalibrationModel calib_model = static_cast(model); + switch (calib_model) { + case CalibrationModel::PINHOLE: { + params.ok = true; + params.in_left = to_intrinsics_pinhole( + n["in_left"], model, width, height); + params.in_right = to_intrinsics_pinhole( + n["in_right"], model, width, height); + n["ex_right_to_left"] >> params.ex_right_to_left; + } break; + case CalibrationModel::KANNALA_BRANDT: { + params.ok = true; + params.in_left = to_intrinsics_equidistant( + n["in_left"], model, width, height); + params.in_right = to_intrinsics_equidistant( + n["in_right"], model, width, height); + n["ex_right_to_left"] >> params.ex_right_to_left; + } break; + default: + LOG(FATAL) << "Could not load img params as unknown calib model" + ", please use latest SDK."; + } + return params; } } // namespace @@ -388,11 +463,37 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams( } img_params_map_t img_params_map; - if (fs["version"].isNone()) { + if (!fs["version"].isNone()) { + std::string version = std::string(fs["version"]); + // load params according to verison + if (version == "1.0") { + fs["img_params_map"][0] >> img_params_map[{752, 480}]; + } else if (version == "1.1") { + fs["img_params_map"][0] >> img_params_map[{1280, 400}]; + fs["img_params_map"][1] >> img_params_map[{2560, 800}]; + } else if (version == "1.2") { + auto node = fs["img_params"]; + for (auto it = node.begin(); it < node.end(); it++) { + std::uint8_t model; + std::uint16_t width, height; + (*it)["model"] >> model; + (*it)["width"] >> width; + (*it)["height"] >> height; + auto params = to_img_params(*it, model, width, height); + if (params.ok) { + params.version = version; + img_params_map[{width, height}] = params; + } + } + } else { + LOG(ERROR) << "Failed to load img params of version " << version + << ", please use latest SDK."; + } + } else { + // load old params s1030 auto in_left = std::make_shared(); auto in_right = std::make_shared(); - img_params_map[{752, 480}].in_left = in_left; - img_params_map[{752, 480}].in_right = in_right; + Extrinsics ex_right_to_left; if (fs["in_left"].isNone()) { std::uint16_t w = 752; std::uint16_t h = 480; @@ -412,25 +513,19 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams( fs["R"] >> R; fs["T"] >> T; - to_intrinsics( - w, h, m, M1, D1, in_left.get()); - to_intrinsics( - w, h, m, M2, D2, in_right.get()); - to_extrinsics(R, T, &img_params_map[{752, 480}].ex_right_to_left); + to_intrinsics(w, h, m, M1, D1, in_left.get()); + to_intrinsics(w, h, m, M2, D2, in_right.get()); + to_extrinsics(R, T, &ex_right_to_left); } else { - fs["in_left"][0] >> *in_left; - fs["in_right"][0] >> *in_right; - fs["ex_right_to_left"] >> img_params_map[{752, 480}].ex_right_to_left; - } - } else { - if (static_cast(fs["version"]) == "1.0") { - fs["img_params_map"][0] >> img_params_map[{752, 480}]; - } - if (static_cast(fs["version"]) == "1.1") { - fs["img_params_map"][0] >> img_params_map[{1280, 400}]; - fs["img_params_map"][1] >> img_params_map[{2560, 800}]; + fs["in_left"] >> *in_left; + fs["in_right"] >> *in_right; + fs["ex_right_to_left"] >> ex_right_to_left; } + img_params_map[{752, 480}] = { + true, "1.0", in_left, in_right, ex_right_to_left + }; } + fs.release(); return img_params_map; } @@ -442,10 +537,27 @@ DeviceWriter::imu_params_t DeviceWriter::LoadImuParams( if (!fs.isOpened()) { LOG(FATAL) << "Failed to load file: " << filepath; } + imu_params_t params; - fs["in_accel"] >> params.in_accel; - fs["in_gyro"] >> params.in_gyro; - fs["ex_left_to_imu"] >> params.ex_left_to_imu; + if (!fs["version"].isNone()) { + std::string version = std::string(fs["version"]); + // load params according to verison + if (version == "1.2") { + params.version = version; + fs["in_accel"] >> params.in_accel; + fs["in_gyro"] >> params.in_gyro; + fs["ex_left_to_imu"] >> params.ex_left_to_imu; + } else { + LOG(ERROR) << "Failed to load imu params of version " << version + << ", please use latest SDK."; + } + } else { + // load old params + fs["in_accel"] >> params.in_accel; + fs["in_gyro"] >> params.in_gyro; + fs["ex_left_to_imu"] >> params.ex_left_to_imu; + } + fs.release(); return params; }