feat(channels): add img params new parser

include/mynteye/types.h

@@ -400,22 +400,33 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request);
  * Camera calibration model.
  */
 enum class CalibrationModel : std::uint8_t {
-  /** Unknow */
-  CALIB_MODEL_UNKNOW = 0,
   /** Pinhole */
-  CALIB_MODEL_PINHOLE = 1,
+  PINHOLE = 0,
   /** Equidistant: KANNALA_BRANDT */
-  CALIB_MODEL_KANNALA_BRANDT = 2,
+  KANNALA_BRANDT = 1,
-  // CALIB_MODEL_SCARAMUZZA,
+  /** Unknow */
-  // CALIB_MODEL_MEI,
+  UNKNOW
 };
 
+MYNTEYE_API const char *to_string(const CalibrationModel &model);
+
+inline std::ostream &operator<<(std::ostream &os,
+    const CalibrationModel &model) {
+  return os << to_string(model);
+}
+
 struct MYNTEYE_API IntrinsicsBase {
   IntrinsicsBase() {
-    calib_model = CalibrationModel::CALIB_MODEL_UNKNOW;
+    calib_model = CalibrationModel::UNKNOW;
   }
   virtual ~IntrinsicsBase() {}
+
+  /** The calibration model */
   CalibrationModel calib_model;
+  /** The width of the image in pixels */
+  std::uint16_t width;
+  /** The height of the image in pixels */
+  std::uint16_t height;
 };
 
 /**
@@ -424,12 +435,8 @@ struct MYNTEYE_API IntrinsicsBase {
  */
 struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase {
   IntrinsicsPinhole() {
-    calib_model = CalibrationModel::CALIB_MODEL_PINHOLE;
+    calib_model = CalibrationModel::PINHOLE;
   }
-  /** The width of the image in pixels */
-  std::uint16_t width;
-  /** The height of the image in pixels */
-  std::uint16_t height;
   /** The focal length of the image plane, as a multiple of pixel width */
   double fx;
   /** The focal length of the image plane, as a multiple of pixel height */
@@ -458,12 +465,8 @@ using Intrinsics = IntrinsicsPinhole;
  */
 struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
   IntrinsicsEquidistant() {
-    calib_model = CalibrationModel::CALIB_MODEL_KANNALA_BRANDT;
+    calib_model = CalibrationModel::KANNALA_BRANDT;
   }
-  /** The width of the image in pixels */
-  std::uint16_t width;
-  /** The height of the image in pixels */
-  std::uint16_t height;
   /** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */
   double coeffs[8];
 };

src/mynteye/api/api.cc

@@ -296,7 +296,7 @@ std::string API::GetInfo(const Info &info) const {
 
 IntrinsicsPinhole API::GetIntrinsics(const Stream &stream) const {
   auto in = GetIntrinsicsBase(stream);
-  if (in->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
+  if (in->calib_model == CalibrationModel::PINHOLE) {
     return *std::dynamic_pointer_cast<IntrinsicsPinhole>(in);
   }
   throw std::runtime_error("Intrinsics is not pinhole model"

src/mynteye/api/processor/rectify_processor.cc

@@ -43,13 +43,13 @@ std::string RectifyProcessor::Name() {
 void RectifyProcessor::NotifyImageParamsChanged() {
   auto in_left = device_->GetIntrinsics(Stream::LEFT);
   auto in_right = device_->GetIntrinsics(Stream::RIGHT);
-  if (in_left->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
+  if (in_left->calib_model == CalibrationModel::PINHOLE) {
     InitParams(
       *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left),
       *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right),
       device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
   } else if (in_left->calib_model ==
-             CalibrationModel::CALIB_MODEL_KANNALA_BRANDT) {
+             CalibrationModel::KANNALA_BRANDT) {
     InitParams(
       *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left),
       *std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right),

src/mynteye/device/channel/bytes.cc

@@ -13,6 +13,7 @@
 // limitations under the License.
 #include "mynteye/device/channel/bytes.h"
 
+#include "mynteye/logger.h"
 #include "mynteye/util/strings.h"
 
 MYNTEYE_BEGIN_NAMESPACE
@@ -29,15 +30,32 @@ std::string _from_data(const std::uint8_t *data, std::size_t count) {
 
 // from types
 
-std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) {
+std::size_t from_data(IntrinsicsBase *in, const std::uint8_t *data,
+    bool get_size) {
+  switch (in->calib_model) {
+    case CalibrationModel::PINHOLE:
+      return from_data(dynamic_cast<IntrinsicsPinhole *>(in), data,
+          get_size);
+    case CalibrationModel::KANNALA_BRANDT:
+      return from_data(dynamic_cast<IntrinsicsEquidistant *>(in), data,
+          get_size);
+    default:
+      LOG(FATAL) << "Unknown calib model: " << in->calib_model;
+  }
+}
+
+std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
+    bool get_size) {
   std::size_t i = 0;
 
+  if (get_size) {
     // width, 2
     in->width = _from_data<std::uint16_t>(data + i);
     i += 2;
     // height, 2
     in->height = _from_data<std::uint16_t>(data + i);
     i += 2;
+  }
   // fx, 8
   in->fx = _from_data<double>(data + i);
   i += 8;
@@ -62,6 +80,27 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) {
   return i;
 }
 
+std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
+    bool get_size) {
+  std::size_t i = 0;
+
+  if (get_size) {
+    // width, 2
+    in->width = _from_data<std::uint16_t>(data + i);
+    i += 2;
+    // height, 2
+    in->height = _from_data<std::uint16_t>(data + i);
+    i += 2;
+  }
+  // coeffs, 64
+  for (std::size_t j = 0; j < 8; j++) {
+    in->coeffs[j] = _from_data<double>(data + i + j * 8);
+  }
+  i += 64;
+
+  return i;
+}
+
 std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
   std::size_t i = 0;
 
@@ -122,15 +161,32 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) {
 
 // to types
 
-std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) {
+std::size_t to_data(const IntrinsicsBase *in, std::uint8_t *data,
+    bool set_size) {
+  switch (in->calib_model) {
+    case CalibrationModel::PINHOLE:
+      return to_data(dynamic_cast<const IntrinsicsPinhole *>(in), data,
+          set_size);
+    case CalibrationModel::KANNALA_BRANDT:
+      return to_data(dynamic_cast<const IntrinsicsEquidistant *>(in), data,
+          set_size);
+    default:
+      LOG(FATAL) << "Unknown calib model: " << in->calib_model;
+  }
+}
+
+std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
+    bool set_size) {
   std::size_t i = 0;
 
+  if (set_size) {
     // width, 2
     _to_data(in->width, data + i);
     i += 2;
     // height, 2
     _to_data(in->height, data + i);
     i += 2;
+  }
   // fx, 8
   _to_data(in->fx, data + i);
   i += 8;
@@ -155,6 +211,27 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) {
   return i;
 }
 
+std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
+    bool set_size) {
+  std::size_t i = 0;
+
+  if (set_size) {
+    // width, 2
+    _to_data(in->width, data + i);
+    i += 2;
+    // height, 2
+    _to_data(in->height, data + i);
+    i += 2;
+  }
+  // coeffs, 64
+  for (std::size_t j = 0; j < 8; j++) {
+    _to_data(in->coeffs[j], data + i + j * 8);
+  }
+  i += 64;
+
+  return i;
+}
+
 std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
   std::size_t i = 0;
 

src/mynteye/device/channel/bytes.h

@@ -48,7 +48,12 @@ std::string _from_data(const std::uint8_t *data, std::size_t count);
 
 // from types
 
-std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data);
+std::size_t from_data(IntrinsicsBase *in, const std::uint8_t *data,
+    bool get_size);
+std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
+    bool get_size);
+std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
+    bool get_size);
 
 std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data);
 
@@ -76,7 +81,12 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count);
 
 // to types
 
-std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data);
+std::size_t to_data(const IntrinsicsBase *in, std::uint8_t *data,
+    bool set_size);
+std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
+    bool set_size);
+std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
+    bool set_size);
 
 std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data);
 

src/mynteye/device/channel/channels.cc

@@ -97,26 +97,6 @@ int XuHalfDuplexId(Option option) {
   }
 }
 
-void CheckSpecVersion(const Version *spec_version) {
-  if (spec_version == nullptr) {
-    LOG(FATAL) << "Spec version must be specified";
-  }
-
-  std::vector<std::string> spec_versions{"1.0", "1.1"};
-  for (auto &&spec_ver : spec_versions) {
-    if (*spec_version == Version(spec_ver)) {
-      return;  // supported
-    }
-  }
-
-  std::ostringstream ss;
-  std::copy(
-      spec_versions.begin(), spec_versions.end(),
-      std::ostream_iterator<std::string>(ss, ","));
-  LOG(FATAL) << "Spec version " << spec_version->to_string()
-             << " not supported, must in [" << ss.str() << "]";
-}
-
 }  // namespace
 
 Channels::Channels(const std::shared_ptr<uvc::device> &device,
@@ -431,8 +411,7 @@ void Channels::StopImuTracking() {
 }
 
 bool Channels::GetFiles(
-    device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+    device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) {
-    Version *spec_version) {
   if (info == nullptr && img_params == nullptr && imu_params == nullptr) {
     LOG(WARNING) << "Files are not provided to get";
     return false;
@@ -475,7 +454,6 @@ bool Channels::GetFiles(
       return false;
     }
 
-    Version *spec_ver = spec_version;
     std::size_t i = 3;
     std::size_t end = 3 + size;
     while (i < end) {
@@ -486,25 +464,24 @@ bool Channels::GetFiles(
       i += 3;
       switch (file_id) {
         case FID_DEVICE_INFO: {
-          auto &&n = file_channel_.GetDeviceInfoFromData(data + i, info);
+          auto &&n = file_channel_.GetDeviceInfoFromData(
+              data + i, file_size, info);
           CHECK_EQ(n, file_size)
               << "The firmware not support getting device info, you could "
                  "upgrade to latest";
-          spec_ver = &info->spec_version;
-          CheckSpecVersion(spec_ver);
         } break;
         case FID_IMG_PARAMS: {
           if (file_size > 0) {
-            CheckSpecVersion(spec_ver);
+            auto &&n = file_channel_.GetImgParamsFromData(
-            auto &&n = file_channel_.GetImgParamsFromData(data + i, img_params);
+                data + i, file_size, img_params);
             CHECK_EQ(n, file_size);
           }
         } break;
         case FID_IMU_PARAMS: {
           imu_params->ok = file_size > 0;
           if (imu_params->ok) {
-            CheckSpecVersion(spec_ver);
+            auto &&n = file_channel_.GetImuParamsFromData(
-            auto &&n = file_channel_.GetImuParamsFromData(data + i, imu_params);
+                data + i, file_size, imu_params);
             CHECK_EQ(n, file_size);
           }
         } break;
@@ -523,17 +500,11 @@ bool Channels::GetFiles(
 }
 
 bool Channels::SetFiles(
-    device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+    device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) {
-    Version *spec_version) {
   if (info == nullptr && img_params == nullptr && imu_params == nullptr) {
     LOG(WARNING) << "Files are not provided to set";
     return false;
   }
-  Version *spec_ver = spec_version;
-  if (spec_ver == nullptr && info != nullptr) {
-    spec_ver = &info->spec_version;
-  }
-  CheckSpecVersion(spec_ver);
 
   std::uint8_t data[2000]{};
 
@@ -542,16 +513,28 @@ bool Channels::SetFiles(
 
   std::uint16_t size = 0;
   if (info != nullptr) {
+    auto n = file_channel_.SetDeviceInfoToData(info, data + 3 + size);
+    if (n > 0) {
       header[0] = true;
-    size += file_channel_.SetDeviceInfoToData(info, data + 3 + size);
+      size += n;
+    }
   }
   if (img_params != nullptr) {
+    auto n = file_channel_.SetImgParamsToData(img_params, data + 3 + size);
+    if (n > 0) {
       header[1] = true;
-    size += file_channel_.SetImgParamsToData(img_params, data + 3 + size);
+      size += n;
+    }
   }
   if (imu_params != nullptr) {
+    auto n = file_channel_.SetImuParamsToData(imu_params, data + 3 + size);
+    if (n > 0) {
       header[2] = true;
-    size += file_channel_.SetImuParamsToData(imu_params, data + 3 + size);
+      size += n;
+    }
+  }
+  if (size + 3 > 2000) {
+    LOG(FATAL) << "SetFiles failed, data is too large: " << (size + 3);
   }
 
   data[0] = static_cast<std::uint8_t>(header.to_ulong());

src/mynteye/device/channel/channels.h

@@ -82,11 +82,9 @@ class MYNTEYE_API Channels {
   void StopImuTracking();
 
   bool GetFiles(
-      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
-      Version *spec_version = nullptr);
   bool SetFiles(
-      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
-      Version *spec_version = nullptr);
 
  private:
   bool PuControlRange(

src/mynteye/device/channel/file_channel.cc

@@ -29,8 +29,9 @@ FileChannel::~FileChannel() {
 }
 
 std::size_t FileChannel::GetDeviceInfoFromData(
-    const std::uint8_t *data, device_info_t *info) {
+    const std::uint8_t *data, const std::uint16_t &data_size,
-  auto n = dev_info_parser_->GetFromData(data, info);
+    device_info_t *info) {
+  auto n = dev_info_parser_->GetFromData(data, data_size, info);
   auto spec_version = info->spec_version;
   img_params_parser_->SetSpecVersion(spec_version);
   imu_params_parser_->SetSpecVersion(spec_version);
@@ -46,9 +47,10 @@ std::size_t FileChannel::SetDeviceInfoToData(
 }
 
 std::size_t FileChannel::GetImgParamsFromData(
-    const std::uint8_t *data, img_params_t *img_params) {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    img_params_t *img_params) {
   CHECK_NOTNULL(img_params_parser_);
-  return img_params_parser_->GetFromData(data, img_params);
+  return img_params_parser_->GetFromData(data, data_size, img_params);
 }
 
 std::size_t FileChannel::SetImgParamsToData(
@@ -58,8 +60,9 @@ std::size_t FileChannel::SetImgParamsToData(
 }
 
 std::size_t FileChannel::GetImuParamsFromData(
-    const std::uint8_t *data, imu_params_t *imu_params) {
+    const std::uint8_t *data, const std::uint16_t &data_size,
-  return imu_params_parser_->GetFromData(data, imu_params);
+    imu_params_t *imu_params) {
+  return imu_params_parser_->GetFromData(data, data_size, imu_params);
 }
 
 std::size_t FileChannel::SetImuParamsToData(
@@ -76,7 +79,8 @@ DeviceInfoParser::~DeviceInfoParser() {
 }
 
 std::size_t DeviceInfoParser::GetFromData(
-    const std::uint8_t *data, device_info_t *info) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    device_info_t *info) const {
   std::size_t i = 4;  // skip vid, pid
   // name, 16
   info->name = bytes::_from_data(data + i, 16);
@@ -111,6 +115,7 @@ std::size_t DeviceInfoParser::GetFromData(
 
   // get other infos according to spec_version
 
+  MYNTEYE_UNUSED(data_size)
   return i;
 }
 
@@ -169,17 +174,18 @@ ImgParamsParser::~ImgParamsParser() {
 }
 
 std::size_t ImgParamsParser::GetFromData(
-    const std::uint8_t *data, img_params_t *img_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    img_params_t *img_params) const {
   if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) {
     // get img params without version header
     if (spec_version_ == Version(1, 0)) {
-      return GetFromData_v1_0(data, img_params);
+      return GetFromData_v1_0(data, data_size, img_params);
     } else {
-      return GetFromData_v1_1(data, img_params);
+      return GetFromData_v1_1(data, data_size, img_params);
     }
   } else {
     // get img params with version header
-    return GetFromData_new(data, img_params);
+    return GetFromData_new(data, data_size, img_params);
   }
 }
 
@@ -199,19 +205,21 @@ std::size_t ImgParamsParser::SetToData(
 }
 
 std::size_t ImgParamsParser::GetFromData_v1_0(
-    const std::uint8_t *data, img_params_t *img_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    img_params_t *img_params) const {
   std::size_t i = 0;
 
   auto in_left = std::make_shared<IntrinsicsPinhole>();
   auto in_right = std::make_shared<IntrinsicsPinhole>();
   Extrinsics ex_right_to_left;
 
-  i += bytes::from_data(in_left.get(), data + i);
+  i += bytes::from_data(in_left.get(), data + i, true);
-  i += bytes::from_data(in_right.get(), data + i);
+  i += bytes::from_data(in_right.get(), data + i, true);
   i += bytes::from_data(&ex_right_to_left, data + i);
   (*img_params)[{752, 480}] = {true, spec_version_.to_string(),
       in_left, in_right, ex_right_to_left};
 
+  MYNTEYE_UNUSED(data_size)
   return i;
 }
 
@@ -220,10 +228,8 @@ std::size_t ImgParamsParser::SetToData_v1_0(
   std::size_t i = 3;  // skip id, size
 
   auto params = (*img_params).at({752, 480});
-  auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
+  i += bytes::to_data(params.in_left.get(), data + i, true);
-  auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_right);
+  i += bytes::to_data(params.in_right.get(), data + i, true);
-  i += bytes::to_data(in_left.get(), data + i);
-  i += bytes::to_data(in_right.get(), data + i);
   i += bytes::to_data(&params.ex_right_to_left, data + i);
 
   // others
@@ -235,23 +241,24 @@ std::size_t ImgParamsParser::SetToData_v1_0(
 }
 
 std::size_t ImgParamsParser::GetFromData_v1_1(
-    const std::uint8_t *data, img_params_t *img_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    img_params_t *img_params) const {
   std::size_t i = 0;
 
   Extrinsics ex_right_to_left;
   {
     auto in_left = std::make_shared<IntrinsicsPinhole>();
     auto in_right = std::make_shared<IntrinsicsPinhole>();
-    i += bytes::from_data(in_left.get(), data + i);
+    i += bytes::from_data(in_left.get(), data + i, true);
-    i += bytes::from_data(in_right.get(), data + i);
+    i += bytes::from_data(in_right.get(), data + i, true);
     (*img_params)[{1280, 400}] = {true, spec_version_.to_string(),
         in_left, in_right, ex_right_to_left};
   }
   {
     auto in_left = std::make_shared<IntrinsicsPinhole>();
     auto in_right = std::make_shared<IntrinsicsPinhole>();
-    i += bytes::from_data(in_left.get(), data + i);
+    i += bytes::from_data(in_left.get(), data + i, true);
-    i += bytes::from_data(in_right.get(), data + i);
+    i += bytes::from_data(in_right.get(), data + i, true);
     (*img_params)[{2560, 800}] = {true, spec_version_.to_string(),
         in_left, in_right, ex_right_to_left};
   }
@@ -261,6 +268,7 @@ std::size_t ImgParamsParser::GetFromData_v1_1(
     (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
   }
 
+  MYNTEYE_UNUSED(data_size)
   return i;
 }
 
@@ -270,19 +278,13 @@ std::size_t ImgParamsParser::SetToData_v1_1(
 
   {
     auto params = (*img_params).at({1280, 400});
-    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
+    i += bytes::to_data(params.in_left.get(), data + i, true);
-    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+    i += bytes::to_data(params.in_right.get(), data + i, true);
-        params.in_right);
-    i += bytes::to_data(in_left.get(), data + i);
-    i += bytes::to_data(in_right.get(), data + i);
   }
   {
     auto params = (*img_params).at({2560, 800});
-    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
+    i += bytes::to_data(params.in_left.get(), data + i, true);
-    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+    i += bytes::to_data(params.in_right.get(), data + i, true);
-        params.in_right);
-    i += bytes::to_data(in_left.get(), data + i);
-    i += bytes::to_data(in_right.get(), data + i);
     i += bytes::to_data(&params.ex_right_to_left, data + i);
   }
 
@@ -295,12 +297,102 @@ std::size_t ImgParamsParser::SetToData_v1_1(
 }
 
 std::size_t ImgParamsParser::GetFromData_new(
-    const std::uint8_t *data, img_params_t *img_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
-  return 0;
+    img_params_t *img_params) const {
+  std::size_t i = 0;
+  // version, 2
+  Version version(data[i], data[i + 1]);
+  i += 2;
+  // get img params according to version
+  if (version == Version(1, 2)) {  // v1.2
+    for (; i < data_size;) {
+      // calib_model, 1
+      auto calib_model = static_cast<CalibrationModel>(data[i]);
+      i += 1;
+      // width, 2
+      auto width = bytes::_from_data<std::uint16_t>(data + i);
+      i += 2;
+      // height, 2
+      auto height = bytes::_from_data<std::uint16_t>(data + i);
+      i += 2;
+      // calib_params
+      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:
+          in_left = std::make_shared<IntrinsicsEquidistant>();
+          in_right = std::make_shared<IntrinsicsEquidistant>();
+          break;
+        default:
+          LOG(FATAL) << "Could not get img params as unknown calib model"
+              ", please use latest SDK.";
+      }
+      i += bytes::from_data(in_left.get(), data + i, false);
+      i += bytes::from_data(in_right.get(), data + i, false);
+      i += bytes::from_data(&ex_right_to_left, data + i);
+      in_left->calib_model = calib_model;
+      in_left->width = width;
+      in_left->height = height;
+      in_right->calib_model = calib_model;
+      in_right->width = width;
+      in_right->height = height;
+      (*img_params)[{width, height}] = {true, version.to_string(),
+          in_left, in_right, ex_right_to_left};
+    }
+  } else {
+    LOG(FATAL) << "Could not get img params of version "
+        << version.to_string() << ", please use latest SDK.";
+  }
+
+  MYNTEYE_UNUSED(data_size)
+  return i;
 }
+
 std::size_t ImgParamsParser::SetToData_new(
     const img_params_t *img_params, std::uint8_t *data) const {
+  if (img_params->empty()) {
     return 0;
+  }
+
+  std::size_t i = 3;  // skip id, size
+  // version, 2
+  Version version(img_params->begin()->second.version);
+  data[i] = version.major();
+  data[i + 1] = version.minor();
+  i += 2;
+  // set img params according to version
+  if (version == Version(1, 2)) {  // v1.2
+    for (auto &&entry : *img_params) {
+      auto &&params = entry.second;
+      // calib_model, 1
+      data[i] = static_cast<std::uint8_t>(params.in_left->calib_model);
+      i += 1;
+      // width, 2
+      bytes::_to_data(params.in_left->width, data + i);
+      i += 2;
+      // height, 2
+      bytes::_to_data(params.in_left->height, data + i);
+      i += 2;
+      // calib_params
+      i += bytes::to_data(params.in_left.get(), data + i, false);
+      i += bytes::to_data(params.in_right.get(), data + i, false);
+      i += bytes::to_data(&params.ex_right_to_left, data + i);
+    }
+  } else {
+    LOG(FATAL) << "Could not set img params of version "
+        << version.to_string() << ", please use latest SDK.";
+  }
+
+  // others
+  std::size_t size = i - 3;
+  data[0] = FID_IMG_PARAMS;
+  data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
+  data[2] = static_cast<std::uint8_t>(size & 0xFF);
+  return size + 3;
 }
 
 // ImuParamsParser
@@ -312,13 +404,14 @@ ImuParamsParser::~ImuParamsParser() {
 }
 
 std::size_t ImuParamsParser::GetFromData(
-    const std::uint8_t *data, imu_params_t *imu_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    imu_params_t *imu_params) const {
   if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) {
     // get imu params without version header
-    return GetFromData_old(data, imu_params);
+    return GetFromData_old(data, data_size, imu_params);
   } else {
     // get imu params with version header
-    return GetFromData_new(data, imu_params);
+    return GetFromData_new(data, data_size, imu_params);
   }
 }
 
@@ -334,11 +427,13 @@ std::size_t ImuParamsParser::SetToData(
 }
 
 std::size_t ImuParamsParser::GetFromData_old(
-    const std::uint8_t *data, imu_params_t *imu_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    imu_params_t *imu_params) const {
   std::size_t i = 0;
   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);
+  MYNTEYE_UNUSED(data_size)
   return i;
 }
 
@@ -357,7 +452,8 @@ std::size_t ImuParamsParser::SetToData_old(
 }
 
 std::size_t ImuParamsParser::GetFromData_new(
-    const std::uint8_t *data, imu_params_t *imu_params) const {
+    const std::uint8_t *data, const std::uint16_t &data_size,
+    imu_params_t *imu_params) const {
   std::size_t i = 0;
   // version, 2
   Version version(data[i], data[i + 1]);
@@ -372,6 +468,7 @@ std::size_t ImuParamsParser::GetFromData_new(
     LOG(FATAL) << "Could not get imu params of version "
         << version.to_string() << ", please use latest SDK.";
   }
+  MYNTEYE_UNUSED(data_size)
   return i;
 }
 

src/mynteye/device/channel/file_channel.h

@@ -39,17 +39,20 @@ class FileChannel {
   ~FileChannel();
 
   std::size_t GetDeviceInfoFromData(
-      const std::uint8_t *data, device_info_t *info);
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      device_info_t *info);
   std::size_t SetDeviceInfoToData(
       const device_info_t *info, std::uint8_t *data);
 
   std::size_t GetImgParamsFromData(
-      const std::uint8_t *data, img_params_t *img_params);
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      img_params_t *img_params);
   std::size_t SetImgParamsToData(
       const img_params_t *img_params, std::uint8_t *data);
 
   std::size_t GetImuParamsFromData(
-      const std::uint8_t *data, imu_params_t *imu_params);
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      imu_params_t *imu_params);
   std::size_t SetImuParamsToData(
       const imu_params_t *imu_params, std::uint8_t *data);
 
@@ -67,7 +70,8 @@ class DeviceInfoParser {
   ~DeviceInfoParser();
 
   std::size_t GetFromData(
-      const std::uint8_t *data, device_info_t *info) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      device_info_t *info) const;
   std::size_t SetToData(
       const device_info_t *info, std::uint8_t *data) const;
 };
@@ -84,22 +88,26 @@ class ImgParamsParser {
   }
 
   std::size_t GetFromData(
-      const std::uint8_t *data, img_params_t *img_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      img_params_t *img_params) const;
   std::size_t SetToData(
       const img_params_t *img_params, std::uint8_t *data) const;
 
   std::size_t GetFromData_v1_0(
-      const std::uint8_t *data, img_params_t *img_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      img_params_t *img_params) const;
   std::size_t SetToData_v1_0(
       const img_params_t *img_params, std::uint8_t *data) const;
 
   std::size_t GetFromData_v1_1(
-      const std::uint8_t *data, img_params_t *img_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      img_params_t *img_params) const;
   std::size_t SetToData_v1_1(
       const img_params_t *img_params, std::uint8_t *data) const;
 
   std::size_t GetFromData_new(
-      const std::uint8_t *data, img_params_t *img_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      img_params_t *img_params) const;
   std::size_t SetToData_new(
       const img_params_t *img_params, std::uint8_t *data) const;
 
@@ -119,17 +127,20 @@ class ImuParamsParser {
   }
 
   std::size_t GetFromData(
-      const std::uint8_t *data, imu_params_t *imu_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      imu_params_t *imu_params) const;
   std::size_t SetToData(
       const imu_params_t *imu_params, std::uint8_t *data) const;
 
   std::size_t GetFromData_old(
-      const std::uint8_t *data, imu_params_t *imu_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      imu_params_t *imu_params) const;
   std::size_t SetToData_old(
       const imu_params_t *imu_params, std::uint8_t *data) const;
 
   std::size_t GetFromData_new(
-      const std::uint8_t *data, imu_params_t *imu_params) const;
+      const std::uint8_t *data, const std::uint16_t &data_size,
+      imu_params_t *imu_params) const;
   std::size_t SetToData_new(
       const imu_params_t *imu_params, std::uint8_t *data) const;
 

src/mynteye/types.cc

@@ -188,6 +188,19 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) {
             << ", format: " << request.format << ", fps: " << request.fps;
 }
 
+const char *to_string(const CalibrationModel &model) {
+#define CASE(X)   \
+  case CalibrationModel::X: \
+    return "CalibrationModel::" #X;
+  switch (model) {
+    CASE(PINHOLE)
+    CASE(KANNALA_BRANDT)
+    default:
+      return "CalibrationModel::UNKNOWN";
+  }
+#undef CASE
+}
+
 std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) {
   os << "pinhole, " << FULL_PRECISION
      << "width: " << in.width << ", height: " << in.height

tools/writer/device_writer.cc

@@ -66,8 +66,7 @@ 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_t *>(&img_params_map), nullptr,
+          nullptr, const_cast<img_params_map_t *>(&img_params_map), nullptr)) {
-          &dev_info->spec_version)) {
     LOG(INFO) << "Write img params success";
     std::map<Resolution, device::img_params_t>::const_iterator it;
     for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
@@ -93,8 +92,7 @@ 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),
+          nullptr, nullptr, const_cast<imu_params_t *>(&params))) {
-          &dev_info->spec_version)) {
     LOG(INFO) << "Write imu params success";
     LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";
     LOG(INFO) << "Imu intrinsics gyro: {" << params.in_gyro << "}";