feat(device_writer): always save params with latest version format

include/mynteye/types.h

@@ -450,7 +450,7 @@ struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase {
   double cx;
   /** The vertical coordinate of the principal point of the image */
   double cy;
-  /** The distortion model of the image */
+  /** @deprecated Replaced by calib_model_. The distortion model of the image */
   std::uint8_t model;
   /** The distortion coefficients: k1,k2,p1,p2,k3 */
   double coeffs[5];

src/mynteye/device/channel/bytes.cc

@@ -68,9 +68,11 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
   // cy, 8
   in->cy = _from_data<double>(data + i);
   i += 8;
-  // model, 1
-  in->model = data[i];
-  i += 1;
+  if (get_size) {
+    // model, 1
+    in->model = data[i];
+    i += 1;
+  }
   // coeffs, 40
   for (std::size_t j = 0; j < 5; j++) {
     in->coeffs[j] = _from_data<double>(data + i + j * 8);
@@ -199,9 +201,11 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
   // cy, 8
   _to_data(in->cy, data + i);
   i += 8;
-  // model, 1
-  data[i] = in->model;
-  i += 1;
+  if (set_size) {
+    // model, 1
+    data[i] = in->model;
+    i += 1;
+  }
   // coeffs, 40
   for (std::size_t j = 0; j < 5; j++) {
     _to_data(in->coeffs[j], data + i + j * 8);

src/mynteye/device/channel/file_channel.cc

@@ -297,7 +297,8 @@ std::size_t ImgParamsParser::GetFromData_new(
   if (version == Version(1, 2)) {  // v1.2
     for (; i < data_size;) {
       // calib_model, 1
-      auto calib_model = static_cast<CalibrationModel>(data[i]);
+      auto model = data[i];
+      auto calib_model = static_cast<CalibrationModel>(model);
       i += 1;
       // width, 2
       auto width = bytes::_from_data<std::uint16_t>(data + i);
@@ -309,14 +310,18 @@ std::size_t ImgParamsParser::GetFromData_new(
       std::shared_ptr<IntrinsicsBase> in_left, in_right;
       Extrinsics ex_right_to_left;
       switch (calib_model) {
-        case CalibrationModel::PINHOLE:
-          in_left = std::make_shared<IntrinsicsPinhole>();
-          in_right = std::make_shared<IntrinsicsPinhole>();
-          break;
-        case CalibrationModel::KANNALA_BRANDT:
+        case CalibrationModel::PINHOLE: {
+          auto in_left_p = std::make_shared<IntrinsicsPinhole>();
+          in_left_p->model = model;
+          in_left = in_left_p;
+          auto in_right_p = std::make_shared<IntrinsicsPinhole>();
+          in_right_p->model = model;
+          in_right = in_right_p;
+        } break;
+        case CalibrationModel::KANNALA_BRANDT: {
           in_left = std::make_shared<IntrinsicsEquidistant>();
           in_right = std::make_shared<IntrinsicsEquidistant>();
-          break;
+        } break;
         default:
           LOG(FATAL) << "Could not get img params as unknown calib model"
               ", please use latest SDK.";
@@ -421,6 +426,7 @@ std::size_t ImuParamsParser::GetFromData_old(
   i += bytes::from_data(&imu_params->in_accel, data + i);
   i += bytes::from_data(&imu_params->in_gyro, data + i);
   i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
+  imu_params->version = spec_version_.to_string();
   MYNTEYE_UNUSED(data_size)
   return i;
 }

src/mynteye/device/device.cc

@@ -626,6 +626,7 @@ void Device::ReadAllInfos() {
     VLOG(2) << "Motion extrinsics left to imu: {"
             << GetMotionExtrinsics(Stream::LEFT) << "}";
   } else {
+    imu_params_.ok = false;
     VLOG(2) << "Motion intrinsics & extrinsics not exist";
   }
 }

tools/writer/device_writer.cc

@@ -21,6 +21,8 @@
 #include "mynteye/device/device.h"
 #include "mynteye/util/files.h"
 
+#define SAVE_LATEST_VERSION Version(1, 2)
+
 MYNTEYE_BEGIN_NAMESPACE
 
 namespace tools {
@@ -113,12 +115,32 @@ namespace {
 
 cv::FileStorage &operator<<(cv::FileStorage &fs, const IntrinsicsPinhole &in) {
   fs << "{"
-     << "width" << in.width << "height" << in.height << "fx" << in.fx << "fy"
-     << in.fy << "cx" << in.cx << "cy" << in.cy << "model" << in.model
+     << "fx" << in.fx << "fy" << in.fy
+     << "cx" << in.cx << "cy" << in.cy
      << "coeffs" << std::vector<double>(in.coeffs, in.coeffs + 5) << "}";
   return fs;
 }
 
+cv::FileStorage &operator<<(cv::FileStorage &fs,
+    const IntrinsicsEquidistant &in) {
+  fs << "{"
+     << "coeffs" << std::vector<double>(in.coeffs, in.coeffs + 8) << "}";
+  return fs;
+}
+
+cv::FileStorage &operator<<(cv::FileStorage &fs,
+    const std::shared_ptr<IntrinsicsBase> &in) {
+  switch (in->calib_model()) {
+    case CalibrationModel::PINHOLE:
+      return fs << *std::dynamic_pointer_cast<IntrinsicsPinhole>(in);
+    case CalibrationModel::KANNALA_BRANDT:
+      return fs << *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in);
+    default:
+      LOG(FATAL) << "Unknown calib model: " << in->calib_model();
+      return fs;
+  }
+}
+
 cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) {
   std::vector<double> scales;
   for (std::size_t i = 0; i < 3; i++) {
@@ -150,10 +172,11 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) {
 cv::FileStorage &operator<<(
     cv::FileStorage &fs, const device::img_params_t &params) {
   fs << "{"
-     << "in_left"
-     << *std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left)
-     << "in_right"
-     << *std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_right)
+     << "model" << static_cast<std::uint8_t>(params.in_left->calib_model())
+     << "width" << params.in_left->width
+     << "height" << params.in_left->height
+     << "in_left" << params.in_left
+     << "in_right" << params.in_right
      << "ex_right_to_left" << params.ex_right_to_left << "}";
   return fs;
 }
@@ -162,8 +185,9 @@ 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++)
+  for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
     fs << (*it).second;
+  }
   fs << "]";
   return fs;
 }
@@ -186,34 +210,57 @@ bool DeviceWriter::SaveDeviceInfo(
   fs << "lens_type" << info.lens_type.to_string();
   fs << "imu_type" << info.imu_type.to_string();
   fs << "nominal_baseline" << info.nominal_baseline;
+  // save other infos according to spec_version
   fs.release();
   return true;
 }
 
 bool DeviceWriter::SaveImgParams(
-    const dev_info_t &info, const img_params_map_t &img_params_map,
+    const img_params_map_t &img_params_map,
     const std::string &filepath) {
+  if (img_params_map.empty()) {
+    return false;
+  }
+  std::string version = img_params_map.begin()->second.version;
+  if (Version(version) > SAVE_LATEST_VERSION) {
+    LOG(ERROR) << "Failed to save img params of version " << version
+        << ", please use latest SDK.";
+    return false;
+  }
+
+  // always save img params with latest version format
   using FileStorage = cv::FileStorage;
   FileStorage fs(filepath, FileStorage::WRITE);
   if (!fs.isOpened()) {
     LOG(ERROR) << "Failed to save file: " << filepath;
     return false;
   }
-  fs << "version" << info.spec_version.to_string();
-  fs << "img_params_map" << img_params_map;
+  fs << "version" << SAVE_LATEST_VERSION.to_string()
+     << "img_params" << img_params_map;
   fs.release();
   return true;
 }
 
 bool DeviceWriter::SaveImuParams(
     const imu_params_t &params, const std::string &filepath) {
+  if (!params.ok) return false;
+  std::string version = params.version;
+  if (Version(version) > SAVE_LATEST_VERSION) {
+    LOG(ERROR) << "Failed to save imu params of version " << version
+        << ", please use latest SDK.";
+    return false;
+  }
+
+  // always save imu params with latest version format
   using FileStorage = cv::FileStorage;
   FileStorage fs(filepath, FileStorage::WRITE);
   if (!fs.isOpened()) {
     LOG(ERROR) << "Failed to save file: " << filepath;
     return false;
   }
-  fs << "in_accel" << params.in_accel << "in_gyro" << params.in_gyro
+  fs << "version" << SAVE_LATEST_VERSION.to_string()
+     << "in_accel" << params.in_accel
+     << "in_gyro" << params.in_gyro
      << "ex_left_to_imu" << params.ex_left_to_imu;
   fs.release();
   return true;
@@ -224,18 +271,8 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) {
     LOG(FATAL) << "Create directory failed: " << dir;
   }
   SaveDeviceInfo(*device_->GetInfo(), dir + MYNTEYE_OS_SEP "device.info");
-  SaveImgParams(
-      *device_->GetInfo(), device_->GetImgParams(),
-      dir + MYNTEYE_OS_SEP "img.params");
-  auto &&m_in = device_->GetMotionIntrinsics();
-  SaveImuParams(
-      {
-          false,
-          device_->GetInfo()->spec_version.to_string(),
-          m_in.accel, m_in.gyro,
-          device_->GetMotionExtrinsics(Stream::LEFT),
-      },
-      dir + MYNTEYE_OS_SEP "imu.params");
+  SaveImgParams(device_->GetImgParams(), dir + MYNTEYE_OS_SEP "img.params");
+  SaveImuParams(device_->GetImuParams(), dir + MYNTEYE_OS_SEP "imu.params");
 }
 
 namespace {

tools/writer/device_writer.h

@@ -49,8 +49,7 @@ class DeviceWriter {
   bool WriteImuParams(const std::string &filepath);
 
   bool SaveDeviceInfo(const dev_info_t &info, const std::string &filepath);
-  bool SaveImgParams(
-      const dev_info_t &info, const img_params_map_t &img_params_map,
+  bool SaveImgParams(const img_params_map_t &img_params_map,
       const std::string &filepath);
   bool SaveImuParams(const imu_params_t &params, const std::string &filepath);
 

tools/writer/save_all_infos.cc

@@ -31,6 +31,7 @@ int main(int argc, char *argv[]) {
   if (!device)
     return 1;
 
+  dir.append(MYNTEYE_OS_SEP).append(device->GetInfo()->name);
   dir.append(MYNTEYE_OS_SEP "SN").append(device->GetInfo()->serial_number);
 
   tools::DeviceWriter writer(device);