feat(tools): enhance device writer with new format

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)

src/mynteye/device/types.cc

@@ -43,6 +43,7 @@ std::vector<std::string> 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]);
 }
 

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<const IntrinsicsPinhole &>(in);
+    case CalibrationModel::KANNALA_BRANDT:
+      return os << dynamic_cast<const IntrinsicsEquidistant &>(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

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 ]

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 ]

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 ]

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 ]

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

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

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 ]

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 ]

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. ]
+   noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
-   bias: [ 0., 0., 0. ]
+       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. ]
+   noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
-   bias: [ 0., 0., 0. ]
+       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. ]
+   rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
-   translation: [ 0., 0., 0. ]
+       -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/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(
+  img_params_map_t *img_params_new =
-          nullptr, const_cast<img_params_map_t *>(&img_params_map), nullptr)) {
+      const_cast<img_params_map_t *>(&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<Resolution, device::img_params_t>::const_iterator it;
+    for (auto it = img_params_new->begin(); it != img_params_new->end(); it++) {
-    for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
+      LOG(INFO) << "Resolution: {width: " << (*it).first.width
-      LOG(INFO) << "Image params for resolution "
+                << ", height: " << (*it).first.height << "}";
-                << (*it).first.width << "x" << (*it).first.height << " :";
+      LOG(INFO) << "Intrinsics left: {" << *(*it).second.in_left << "}";
-      LOG(INFO) << "Intrinsics left: {" << (*it).second.in_left << "}";
+      LOG(INFO) << "Intrinsics right: {" << *(*it).second.in_right << "}";
-      LOG(INFO) << "Intrinsics right: {" << (*it).second.in_right << "}";
+      LOG(INFO) << "Extrinsics right to left: {"
-      LOG(INFO) << "Extrinsics left to right: {"
                 << (*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 &params) {
   auto &&channels = device_->channels();
-  auto &&dev_info = device_->GetInfo();
   if (channels->SetFiles(
           nullptr, nullptr, const_cast<imu_params_t *>(&params))) {
     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<double>(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 &paramas) {
+// old
+void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t &params) {
   auto in_left = std::make_shared<IntrinsicsPinhole>();
   auto in_right = std::make_shared<IntrinsicsPinhole>();
-  paramas.in_left = in_left;
+  params.in_left = in_left;
-  paramas.in_right = in_right;
+  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<IntrinsicsPinhole> 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<IntrinsicsPinhole>();
+  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<IntrinsicsEquidistant> 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<IntrinsicsEquidistant>();
+  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<CalibrationModel>(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<IntrinsicsPinhole>();
     auto in_right = std::make_shared<IntrinsicsPinhole>();
-    img_params_map[{752, 480}].in_left = in_left;
+    Extrinsics ex_right_to_left;
-    img_params_map[{752, 480}].in_right = in_right;
     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(
+      to_intrinsics(w, h, m, M1, D1, in_left.get());
-          w, h, m, M1, D1, in_left.get());
+      to_intrinsics(w, h, m, M2, D2, in_right.get());
-      to_intrinsics(
+      to_extrinsics(R, T, &ex_right_to_left);
-          w, h, m, M2, D2, in_right.get());
-      to_extrinsics(R, T, &img_params_map[{752, 480}].ex_right_to_left);
     } else {
-      fs["in_left"][0] >> *in_left;
+      fs["in_left"] >> *in_left;
-      fs["in_right"][0] >> *in_right;
+      fs["in_right"] >> *in_right;
-      fs["ex_right_to_left"] >> img_params_map[{752, 480}].ex_right_to_left;
+      fs["ex_right_to_left"] >> ex_right_to_left;
-    }
-  } else {
-    if (static_cast<std::string>(fs["version"]) == "1.0") {
-      fs["img_params_map"][0] >> img_params_map[{752, 480}];
-    }
-    if (static_cast<std::string>(fs["version"]) == "1.1") {
-      fs["img_params_map"][0] >> img_params_map[{1280, 400}];
-      fs["img_params_map"][1] >> img_params_map[{2560, 800}];
     }
+    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;
+  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;
 }