Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-sdk-2 into HEAD

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

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 ]

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 ]

tools/writer/config/S210A/device.info

@@ -1,10 +1,10 @@
 %YAML:1.0
 ---
 device_name: MYNT-EYE-S210A
-serial_number: "0386322C0009070E"
+serial_number: "07C40D1C0009071F"
-firmware_version: "1.0"
+firmware_version: "0.1"
 hardware_version: "1.0"
 spec_version: "1.1"
 lens_type: "0000"
 imu_type: "0000"
-nominal_baseline: 120
+nominal_baseline: 0

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 ]

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. ]

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 ]

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 ]

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 << "}";
+    std::map<Resolution, device::img_params_t>::const_iterator it;
-    // LOG(INFO) << "Intrinsics right: {" << params.in_right << "}";
+    for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
-    LOG(INFO) << "Extrinsics left to right: {" << params.ex_right_to_left
+      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;
   }
-
+  fs << "version" << info.spec_version.to_string();
-  if (params.in_left_map.size() == params.in_right_map.size()) {
+  fs << "img_params_map" << img_params_map;
-    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;
-  }
 }
 
 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]);
+          w, h, m, M2, D2, &img_params_map[{752, 480}].in_right);
-      to_extrinsics(R, T, &params.ex_right_to_left);
+      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_left"][0] >> img_params_map[{752, 480}].in_left;
-      fs["in_right"][0] >> params.in_right_map[Resolution::RES_752x480];
+      fs["in_right"][0] >> img_params_map[{752, 480}].in_right;
-      fs["ex_right_to_left"] >> params.ex_right_to_left;
+      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["img_params_map"][0] >> img_params_map[{752, 480}];
-      fs["in_right_map"][0] >> params.in_right_map[Resolution::RES_752x480];
-      fs["ex_right_to_left"] >> params.ex_right_to_left;
     }
     if (static_cast<std::string>(fs["version"]) == "1.1") {
-      fs["in_left_map"][0] >> params.in_left_map[Resolution::RES_1280x400];
+      fs["img_params_map"][0] >> img_params_map[{1280, 400}];
-      fs["in_left_map"][1] >> params.in_left_map[Resolution::RES_2560x800];
+      fs["img_params_map"][1] >> img_params_map[{2560, 800}];
-      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.release();
-  return params;
+  return img_params_map;
 }
 
 DeviceWriter::imu_params_t DeviceWriter::LoadImuParams(

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_;