Select the img params by resolution

include/mynteye/types.h

@@ -264,8 +264,8 @@ enum class AddOns : std::uint8_t {
  * @brief Camera supported resolution.
  */
 enum class Resolution : std::uint8_t {
-  /** 480x752 */
+  /** 752x480 */
-  RES_480x752,
+  RES_752x480,
   /** 1280x400 */
   RES_1280x400,
   /** 2560x800 */
@@ -373,7 +373,7 @@ struct MYNTEYE_API StreamRequest {
   StreamRequest(Resolution res, Format format, FrameRate rate)
       : format(format) {
     switch (res) {
-      case Resolution::RES_480x752:
+      case Resolution::RES_752x480:
         width = 480, height = 752;
         break;
       case Resolution::RES_1280x400:

samples/device/CMakeLists.txt

@@ -14,6 +14,10 @@
 
 get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
 
+include_directories(
+  ${PRO_DIR}/src
+)
+
 set_outdir(
   "${OUT_DIR}/lib/${DIR_NAME}"
   "${OUT_DIR}/lib/${DIR_NAME}"

samples/tutorials/CMakeLists.txt

@@ -16,6 +16,7 @@ get_filename_component(DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
 
 include_directories(
   ${CMAKE_CURRENT_SOURCE_DIR}
+  ${PRO_DIR}/src
 )
 
 set_outdir(

src/api/api.cc

@@ -212,21 +212,7 @@ std::vector<std::string> get_plugin_paths() {
 
 API::API(std::shared_ptr<Device> device) : device_(device) {
   VLOG(2) << __func__;
-  if (std::dynamic_pointer_cast<StandardDevice>(device_) != nullptr) {
+  std::dynamic_pointer_cast<StandardDevice>(device_);
-    bool in_l_ok, in_r_ok, ex_l2r_ok;
-    device_->GetIntrinsics(Stream::LEFT, &in_l_ok);
-    device_->GetIntrinsics(Stream::RIGHT, &in_r_ok);
-    device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT, &ex_l2r_ok);
-    if (!in_l_ok || !in_r_ok || !ex_l2r_ok) {
-      LOG(FATAL) << "Image params not found, but we need it to process the "
-                    "images. Please `make tools` and use `img_params_writer` "
-                    "to write the image params. If you update the SDK from "
-                    "1.x, the `SN*.conf` is the file contains them. Besides, "
-                    "you could also calibrate them by yourself. Read the guide "
-                    "doc (https://github.com/slightech/MYNT-EYE-SDK-2-Guide) "
-                    "to learn more.";
-    }
-  }
   synthetic_.reset(new Synthetic(this));
 }
 
@@ -283,6 +269,7 @@ bool API::Supports(const AddOns &addon) const {
 void API::SetStreamRequest(
     const Resolution &res, const Format &format, const FrameRate &rate) {
   device_->SetStreamRequest(res, format, rate);
+  CheckImageParams();
 }
 
 const std::vector<StreamRequest> &API::GetStreamRequests(
@@ -452,4 +439,22 @@ std::shared_ptr<Device> API::device() {
   return device_;
 }
 
+void API::CheckImageParams() {
+  if (device_ != nullptr) {
+    bool in_l_ok, in_r_ok, ex_l2r_ok;
+    device_->GetIntrinsics(Stream::LEFT, &in_l_ok);
+    device_->GetIntrinsics(Stream::RIGHT, &in_r_ok);
+    device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT, &ex_l2r_ok);
+    if (!in_l_ok || !in_r_ok || !ex_l2r_ok) {
+      LOG(FATAL) << "Image params not found, but we need it to process the "
+                    "images. Please `make tools` and use `img_params_writer` "
+                    "to write the image params. If you update the SDK from "
+                    "1.x, the `SN*.conf` is the file contains them. Besides, "
+                    "you could also calibrate them by yourself. Read the guide "
+                    "doc (https://github.com/slightech/MYNT-EYE-SDK-2-Guide) "
+                    "to learn more.";
+    }
+  }
+}
+
 MYNTEYE_END_NAMESPACE

src/api/api.h

@@ -280,6 +280,8 @@ class MYNTEYE_API API {
   std::shared_ptr<Device> device_;
 
   std::unique_ptr<Synthetic> synthetic_;
+
+  void CheckImageParams();
 };
 
 MYNTEYE_END_NAMESPACE

src/device/device.cc

@@ -435,6 +435,7 @@ std::vector<device::MotionData> Device::GetMotionDatas() {
 void Device::SetStreamRequest(
     const Resolution &res, const Format &format, const FrameRate &rate) {
   StreamRequest request(res, format, rate);
+  ConfigIntrinsics(res);
   request_ = request;
 }
 
@@ -548,9 +549,8 @@ void Device::ReadAllInfos() {
   device_info_ = std::make_shared<DeviceInfo>();
 
   CHECK_NOTNULL(channels_);
-  Channels::img_params_t img_params;
   Channels::imu_params_t imu_params;
-  if (!channels_->GetFiles(device_info_.get(), &img_params, &imu_params)) {
+  if (!channels_->GetFiles(device_info_.get(), &img_params_, &imu_params)) {
     LOG(FATAL) << "Read device infos failed. Please upgrade your firmware to "
                   "the latest version.";
   }
@@ -566,12 +566,8 @@ void Device::ReadAllInfos() {
           << ", nominal_baseline: " << device_info_->nominal_baseline << "}";
 
   device_info_->name = uvc::get_name(*device_);
-  if (img_params.ok) {
+  if (img_params_.ok) {
-    SetIntrinsics(Stream::LEFT, img_params.in_left);
+    SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params_.ex_left_to_right);
-    SetIntrinsics(Stream::RIGHT, img_params.in_right);
-    SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params.ex_left_to_right);
-    VLOG(2) << "Intrinsics left: {" << GetIntrinsics(Stream::LEFT) << "}";
-    VLOG(2) << "Intrinsics right: {" << GetIntrinsics(Stream::RIGHT) << "}";
     VLOG(2) << "Extrinsics left to right: {"
             << GetExtrinsics(Stream::LEFT, Stream::RIGHT) << "}";
   } else {
@@ -588,6 +584,15 @@ void Device::ReadAllInfos() {
   }
 }
 
+void Device::ConfigIntrinsics(const Resolution &res) {
+  if (img_params_.ok) {
+    SetIntrinsics(Stream::LEFT, img_params_.in_left_map[res]);
+    SetIntrinsics(Stream::RIGHT, img_params_.in_right_map[res]);
+    VLOG(2) << "Intrinsics left: {" << GetIntrinsics(Stream::LEFT) << "}";
+    VLOG(2) << "Intrinsics right: {" << GetIntrinsics(Stream::RIGHT) << "}";
+  }
+}
+
 void Device::CallbackPushedStreamData(const Stream &stream) {
   if (HasStreamCallback(stream)) {
     auto &&datas = streams_->stream_datas(stream);
@@ -611,4 +616,8 @@ void Device::CallbackMotionData(const device::MotionData &data) {
   }
 }
 
+Channels::img_params_t Device::GetImgParams() {
+  return img_params_;
+}
+
 MYNTEYE_END_NAMESPACE

src/device/device.h

@@ -22,6 +22,7 @@
 #include <string>
 #include <vector>
 
+#include "internal/channels.h"
 #include "mynteye/callbacks.h"
 #include "mynteye/mynteye.h"
 #include "mynteye/types.h"
@@ -256,6 +257,10 @@ class MYNTEYE_API Device {
    * Get the motion datas.
    */
   std::vector<device::MotionData> GetMotionDatas();
+  /**
+   * Get the device img params
+   */
+  Channels::img_params_t GetImgParams();
 
  protected:
   std::shared_ptr<uvc::device> device() const {
@@ -301,6 +306,7 @@ class MYNTEYE_API Device {
   std::shared_ptr<MotionIntrinsics> motion_intrinsics_;
   std::map<Stream, Extrinsics> motion_from_extrinsics_;
 
+  Channels::img_params_t img_params_;
   stream_callbacks_t stream_callbacks_;
   motion_callback_t motion_callback_;
 
@@ -319,6 +325,8 @@ class MYNTEYE_API Device {
 
   void ReadAllInfos();
 
+  void ConfigIntrinsics(const Resolution &res);
+
   void CallbackPushedStreamData(const Stream &stream);
   void CallbackMotionData(const device::MotionData &data);
 

src/internal/channels.cc

@@ -104,7 +104,7 @@ void CheckSpecVersion(const Version *spec_version) {
 
   std::vector<std::string> spec_versions{"1.0"};
   for (auto &&spec_ver : spec_versions) {
-    if (*spec_version == Version(spec_ver)) {
+    if (*spec_version >= Version(spec_ver)) {
       return;  // supported
     }
   }
@@ -554,8 +554,33 @@ std::size_t from_data(
     Channels::img_params_t *img_params, const std::uint8_t *data,
     const Version *spec_version) {
   std::size_t i = 0;
-  i += from_data(&img_params->in_left, data + i, spec_version);
+
-  i += from_data(&img_params->in_right, data + i, spec_version);
+  if (spec_version->major() == 1) {
+    if (spec_version->minor() == 0) {
+      i += from_data(
+          &img_params->in_left_map[Resolution::RES_752x480], data + i,
+          spec_version);
+      i += from_data(
+          &img_params->in_right_map[Resolution::RES_752x480], data + i,
+          spec_version);
+    }
+
+    if (spec_version->minor() == 1) {
+      i += from_data(
+          &img_params->in_left_map[Resolution::RES_1280x400], data + i,
+          spec_version);
+      i += from_data(
+          &img_params->in_right_map[Resolution::RES_1280x400], data + i,
+          spec_version);
+      i += from_data(
+          &img_params->in_left_map[Resolution::RES_2560x800], data + i,
+          spec_version);
+      i += from_data(
+          &img_params->in_right_map[Resolution::RES_2560x800], data + i,
+          spec_version);
+    }
+  }
+
   i += from_data(&img_params->ex_left_to_right, data + i, spec_version);
   return i;
 }
@@ -826,8 +851,34 @@ std::size_t to_data(
     const Channels::img_params_t *img_params, std::uint8_t *data,
     const Version *spec_version) {
   std::size_t i = 3;  // skip id, size
-  i += to_data(&img_params->in_left, data + i, spec_version);
+
-  i += to_data(&img_params->in_right, data + i, spec_version);
+  if (spec_version->major() == 1) {
+    if (spec_version->minor() == 0) {
+      i += to_data(
+          &img_params->in_left_map.at(Resolution::RES_752x480), data + i,
+          spec_version);
+      i += to_data(
+          &img_params->in_right_map.at(Resolution::RES_752x480), data + i,
+          spec_version);
+    }
+
+    if (spec_version->minor() == 1) {
+      i += to_data(
+          &img_params->in_left_map.at(Resolution::RES_1280x400), data + i,
+          spec_version);
+      i += to_data(
+          &img_params->in_right_map.at(Resolution::RES_1280x400), data + i,
+          spec_version);
+
+      i += to_data(
+          &img_params->in_left_map.at(Resolution::RES_2560x800), data + i,
+          spec_version);
+      i += to_data(
+          &img_params->in_right_map.at(Resolution::RES_2560x800), data + i,
+          spec_version);
+    }
+  }
+
   i += to_data(&img_params->ex_left_to_right, data + i, spec_version);
   // others
   std::size_t size = i - 3;

src/internal/channels.h

@@ -70,8 +70,8 @@ class MYNTEYE_API Channels {
 
   typedef struct ImgParams {
     bool ok;
-    Intrinsics in_left;
+    std::map<Resolution, Intrinsics> in_left_map;
-    Intrinsics in_right;
+    std::map<Resolution, Intrinsics> in_right_map;
     Extrinsics ex_left_to_right;
   } img_params_t;
 

src/internal/config.cc

@@ -23,10 +23,9 @@ const std::map<Model, CapabilitiesSupports> capabilities_supports_map = {
 
 const std::map<Model, OptionSupports> option_supports_map = {
     {Model::STANDARD,
-     {Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST, Option::EXPOSURE_MODE,
+     {Option::BRIGHTNESS, Option::EXPOSURE_MODE, Option::ERASE_CHIP,
-      Option::MAX_EXPOSURE_TIME, Option::ERASE_CHIP, Option::MIN_EXPOSURE_TIME,
+      Option::MIN_EXPOSURE_TIME, Option::ACCELEROMETER_RANGE,
-      Option::ACCELEROMETER_RANGE, Option::GYROSCOPE_RANGE,
+      Option::GYROSCOPE_RANGE, Option::ACCELEROMETER_LOW_PASS_FILTER,
-      Option::ACCELEROMETER_LOW_PASS_FILTER,
       Option::GYROSCOPE_LOW_PASS_FILTER}}};
 
 const std::map<Model, std::map<Capabilities, StreamRequests>>

src/public/types.cc

@@ -114,6 +114,11 @@ const char *to_string(const Option &value) {
     CASE(HDR_MODE)
     CASE(ZERO_DRIFT_CALIBRATION)
     CASE(ERASE_CHIP)
+    CASE(MIN_EXPOSURE_TIME)
+    CASE(ACCELEROMETER_RANGE)
+    CASE(GYROSCOPE_RANGE)
+    CASE(ACCELEROMETER_LOW_PASS_FILTER)
+    CASE(GYROSCOPE_LOW_PASS_FILTER)
     default:
       CHECK(is_valid(value));
       return "Option::UNKNOWN";

tools/dataset/CMakeLists.txt

@@ -20,6 +20,10 @@ set_outdir(
   "${OUT_DIR}/bin/${DIR_NAME}"
 )
 
+include_directories(
+  ${PRO_DIR}/src
+)
+
 ## record
 
 make_executable(record

tools/writer/config/img.params

@@ -1,6 +1,6 @@
 %YAML:1.0
 ---
-in_left:
+in_left_map:
    -
       width: 640
       height: 400
@@ -11,7 +11,17 @@ in_left:
       model: 0
       coeffs: [ 1.2135236310725651e-01, -8.5442776049177036e-02,
           2.4914898631983504e-03, -3.7752063658256863e-03, 0. ]
-in_right:
+   -
+      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
@@ -22,6 +32,16 @@ in_right:
       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_left_to_right:
    rotation: [ 9.9998850083695123e-01, -1.9263678722299450e-03,
        -4.3917309443490191e-03, 1.8166060642710027e-03,

tools/writer/device_writer.cc

@@ -17,10 +17,14 @@
 
 #include <glog/logging.h>
 
+#include <map>
 #include <vector>
 
 #include "mynteye/device.h"
 #include "mynteye/files.h"
+#include "mynteye/types.h"
+
+#include "internal/types.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
@@ -70,8 +74,8 @@ bool DeviceWriter::WriteImgParams(const img_params_t &params) {
           nullptr, const_cast<img_params_t *>(&params), nullptr,
           &dev_info->spec_version)) {
     LOG(INFO) << "Write img params success";
-    LOG(INFO) << "Intrinsics left: {" << params.in_left << "}";
+    // LOG(INFO) << "Intrinsics left: {" << params.in_left << "}";
-    LOG(INFO) << "Intrinsics right: {" << params.in_right << "}";
+    // LOG(INFO) << "Intrinsics right: {" << params.in_right << "}";
     LOG(INFO) << "Extrinsics left to right: {" << params.ex_left_to_right
               << "}";
     return true;
@@ -184,11 +188,33 @@ bool DeviceWriter::SaveImgParams(
     LOG(ERROR) << "Failed to save file: " << filepath;
     return false;
   }
-  fs << "in_left" << std::vector<Intrinsics>{params.in_left} << "in_right"
+
-     << std::vector<Intrinsics>{params.in_right} << "ex_left_to_right"
+  std::vector<Intrinsics> v_left;
-     << params.ex_left_to_right;
+  std::vector<Intrinsics> v_right;
+  std::map<Resolution, Intrinsics>::const_iterator it;
+
+  for (it = params.in_left_map.begin(); it != params.in_left_map.end(); it++)
+    v_left.push_back((*it).second);
+
+  for (it = params.in_right_map.begin(); it != params.in_right_map.end(); it++)
+    v_right.push_back((*it).second);
+
+  if (v_left.size() == v_right.size() && v_left.size() > 0) {
+    fs << "in_left_map";
+    for (unsigned int i = 0; i < v_left.size(); i++)
+      fs << v_left[i];
+
+    fs << "in_right_map";
+    for (unsigned int i = 0; i < v_right.size(); i++)
+      fs << v_right[i];
+
+    fs << "ex_left_to_right" << params.ex_left_to_right;
     fs.release();
     return true;
+  } else {
+    fs.release();
+    return false;
+  }
 }
 
 bool DeviceWriter::SaveImuParams(
@@ -210,11 +236,7 @@ void DeviceWriter::SaveAllInfos(const std::string &dir) {
     LOG(FATAL) << "Create directory failed: " << dir;
   }
   SaveDeviceInfo(*device_->GetInfo(), dir + OS_SEP "device.info");
-  SaveImgParams(
+  SaveImgParams(device_->GetImgParams(), dir + OS_SEP "img.params");
-      {false, device_->GetIntrinsics(Stream::LEFT),
-       device_->GetIntrinsics(Stream::RIGHT),
-       device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)},
-      dir + OS_SEP "img.params");
   auto &&m_in = device_->GetMotionIntrinsics();
   SaveImuParams(
       {
@@ -327,9 +349,10 @@ DeviceWriter::img_params_t DeviceWriter::LoadImgParams(
   }
 
   img_params_t params;
+  if (fs["version"].isNone()) {
     if (fs["in_left"].isNone()) {
-    std::uint16_t w = 640;
+      std::uint16_t w = 752;
-    std::uint16_t h = 400;
+      std::uint16_t h = 480;
       std::uint8_t m = 0;
       if (!fs["width"].isNone())
         w = static_cast<int>(fs["width"]);
@@ -346,15 +369,30 @@ DeviceWriter::img_params_t DeviceWriter::LoadImgParams(
       fs["R"] >> R;
       fs["T"] >> T;
 
-    to_intrinsics(w, h, m, M1, D1, &params.in_left);
+      to_intrinsics(
-    to_intrinsics(w, h, m, M2, D2, &params.in_right);
+          w, h, m, M1, D1, &params.in_left_map[Resolution::RES_752x480]);
+      to_intrinsics(
+          w, h, m, M2, D2, &params.in_right_map[Resolution::RES_752x480]);
       to_extrinsics(R, T, &params.ex_left_to_right);
     } else {
-    fs["in_left"][0] >> params.in_left;
+      fs["in_left"][0] >> params.in_left_map[Resolution::RES_752x480];
-    fs["in_right"][0] >> params.in_right;
+      fs["in_right"][0] >> params.in_right_map[Resolution::RES_752x480];
       fs["ex_left_to_right"] >> params.ex_left_to_right;
     }
-
+  } else {
+    if (static_cast<double>(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_left_to_right"] >> params.ex_left_to_right;
+    }
+    if (static_cast<double>(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_left_map[Resolution::RES_2560x800];
+      fs["ex_left_to_right"] >> params.ex_left_to_right;
+    }
+  }
   fs.release();
   return params;
 }