fix(tools): fix complile bug

2018-12-26 19:58:37 +08:00 · 2018-12-26 19:58:37 +08:00 · ed45497d6c
commit ed45497d6c
parent 56c4d50c93
11 changed files with 206 additions and 131 deletions
--- a/tools/writer/config/S1030/device.info
+++ 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
--- a/tools/writer/config/S1030/img.params
+++ 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 ]
--- a/tools/writer/config/S1030/img.params.old
+++ b/tools/writer/config/S1030/img.params.old
--- a/tools/writer/config/S1030/imu.params
+++ 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 ]
--- a/tools/writer/config/S210A/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
--- a/tools/writer/config/S210A/img.params
+++ 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 ]
--- a/tools/writer/config/S210A/imu.params
+++ 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. ]
--- a/tools/writer/config/img.params
+++ b/tools/writer/config/img.params
@ -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 ]
--- a/tools/writer/config/imu.params
+++ b/tools/writer/config/imu.params
@ -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 ]
--- 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 <map>
 #include <vector>

 #include <opencv2/core/core.hpp>
@ -63,17 +62,22 @@ bool DeviceWriter::WriteDeviceInfo(const std::string &filepath) {
  return WriteDeviceInfo(LoadDeviceInfo(filepath));
 }

-bool DeviceWriter::WriteImgParams(const img_params_t &params) {
+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_t *>(&params), nullptr,
+          nullptr, const_cast<img_params_map_t *>(&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<Resolution, device::img_params_t>::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<Intrinsics> &vec) {
-  fs << "[";
-  for (auto &&in : vec)
-    fs << in;
-  fs << "]";
-  return fs;
-}
-
-cv::FileStorage &operator<<(
-    cv::FileStorage &fs, const std::map<Resolution, Intrinsics> &mapIn) {
-  fs << "[";
-  std::map<Resolution, Intrinsics>::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<double> 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 &params) {
+  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<Resolution, device::img_params_t>::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 &params,
+    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 << "version" << info.spec_version.to_string();
+  fs << "img_params_map" << img_params_map;
  fs.release();
  return true;
-  } else {
-    fs.release();
-    return false;
-  }
 }

 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 &paramas) {
+  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, &params.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, &params.in_right_map[Resolution::RES_752x480]);
-      to_extrinsics(R, T, &params.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<std::string>(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<std::string>(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(
--- 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 <map>
 #include <memory>
 #include <string>

@ -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<Resolution, device::img_params_t>;

  explicit DeviceWriter(std::shared_ptr<Device> 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 &params);
+  bool WriteImgParams(const img_params_map_t &img_params_map);
  bool WriteImgParams(const std::string &filepath);

  bool WriteImuParams(const imu_params_t &params);
@ -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 &params,
+      const dev_info_t &info, const img_params_map_t &img_params_map,
      const std::string &filepath);
  bool SaveImuParams(const imu_params_t &params, 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> device_;