refactor(intrinsics): apply base intrinsics

include/mynteye/api/api.h

@@ -187,14 +187,18 @@ class MYNTEYE_API API {
   std::string GetInfo(const Info &info) const;
 
   /**
-   * Get the intrinsics of stream.
+   * @deprecated Get the intrinsics (pinhole) of stream.
    */
-  Intrinsics GetIntrinsics(const Stream &stream) const;
+  IntrinsicsPinhole GetIntrinsics(const Stream &stream) const;
   /**
    * Get the intrinsics of stream.
    */
   template <typename T>
-  T GetIntrinsics(const Stream &from) const;
+  T GetIntrinsics(const Stream &stream) const;
+  /**
+   * Get the intrinsics base of stream.
+   */
+  std::shared_ptr<IntrinsicsBase> GetIntrinsicsBase(const Stream &stream) const;
   /**
    * Get the extrinsics from one stream to another.
    */
@@ -311,8 +315,9 @@ class MYNTEYE_API API {
 };
 
 template <typename T>
-T API::GetIntrinsics(const Stream &from) const {
+T API::GetIntrinsics(const Stream &stream) const {
-  return device_->GetIntrinsics<T>(from);
+  auto in = GetIntrinsicsBase(stream);
+  return *std::dynamic_pointer_cast<T>(in);
 }
 
 MYNTEYE_END_NAMESPACE

include/mynteye/device/callbacks.h

@@ -32,8 +32,8 @@ namespace device {
 
 typedef struct ImgParams {
   bool ok;
-  Intrinsics in_left;
+  std::shared_ptr<IntrinsicsBase> in_left;
-  Intrinsics in_right;
+  std::shared_ptr<IntrinsicsBase> in_right;
   Extrinsics ex_right_to_left;
 } img_params_t;
 

include/mynteye/device/device.h

@@ -153,12 +153,7 @@ class MYNTEYE_API Device {
   /**
    * Get the intrinsics of stream.
    */
-  Intrinsics GetIntrinsics(const Stream &stream) const;
+  std::shared_ptr<IntrinsicsBase> GetIntrinsics(const Stream &stream) const;
-  /**
-   * Get the intrinsics of stream.
-   */
-  template<typename T>
-  T GetIntrinsics(const Stream &from) const;
   /**
    * Get the extrinsics from one stream to another.
    */
@@ -175,7 +170,8 @@ class MYNTEYE_API Device {
   /**
    * Get the intrinsics of stream.
    */
-  Intrinsics GetIntrinsics(const Stream &stream, bool *ok) const;
+  std::shared_ptr<IntrinsicsBase> GetIntrinsics(
+      const Stream &stream, bool *ok) const;
   /**
    * Get the extrinsics from one stream to another.
    */
@@ -193,7 +189,8 @@ class MYNTEYE_API Device {
   /**
    * Set the intrinsics of stream.
    */
-  void SetIntrinsics(const Stream &stream, const Intrinsics &in);
+  void SetIntrinsics(const Stream &stream,
+      const std::shared_ptr<IntrinsicsBase> &in);
   /**
    * Set the extrinsics from one stream to another.
    */
@@ -332,7 +329,7 @@ class MYNTEYE_API Device {
   std::map<Resolution, device::img_params_t> all_img_params_;
   device::imu_params_t imu_params_;
 
-  std::map<Stream, Intrinsics> stream_intrinsics_;
+  std::map<Stream, std::shared_ptr<IntrinsicsBase>> stream_intrinsics_;
   std::map<Stream, std::map<Stream, Extrinsics>> stream_from_extrinsics_;
 
   std::shared_ptr<MotionIntrinsics> motion_intrinsics_;
@@ -365,13 +362,6 @@ class MYNTEYE_API Device {
   friend tools::DeviceWriter;
 };
 
-template <typename T>
-T Device::GetIntrinsics(const Stream &from) const {
-  T res;
-  printf("type %d\n", res.calib_model_);
-  return res;
-}
-
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_DEVICE_DEVICE_H_

include/mynteye/types.h

@@ -414,6 +414,7 @@ struct MYNTEYE_API IntrinsicsBase {
   IntrinsicsBase() {
     calib_model = CalibrationModel::CALIB_MODEL_UNKNOW;
   }
+  virtual ~IntrinsicsBase() {}
   CalibrationModel calib_model;
 };
 
@@ -447,8 +448,7 @@ MYNTEYE_API
 std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in);
 
 /**
- * @ingroup calibration
+ * @deprecated Replaced by IntrinsicsPinhole.
- * Stream intrinsics (Pinhole)
  */
 using Intrinsics = IntrinsicsPinhole;
 

src/mynteye/api/api.cc

@@ -294,7 +294,17 @@ std::string API::GetInfo(const Info &info) const {
   return device_->GetInfo(info);
 }
 
-Intrinsics API::GetIntrinsics(const Stream &stream) const {
+IntrinsicsPinhole API::GetIntrinsics(const Stream &stream) const {
+  auto in = GetIntrinsicsBase(stream);
+  if (in->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
+    return *std::dynamic_pointer_cast<IntrinsicsPinhole>(in);
+  }
+  throw std::runtime_error("Intrinsics is not pinhole model"
+      ", please use GetIntrinsicsBase() or GetIntrinsics<T>() instead.");
+}
+
+std::shared_ptr<IntrinsicsBase> API::GetIntrinsicsBase(
+    const Stream &stream) const {
   return device_->GetIntrinsics(stream);
 }
 

src/mynteye/api/processor/rectify_processor.cc

@@ -41,9 +41,11 @@ std::string RectifyProcessor::Name() {
 }
 
 void RectifyProcessor::NotifyImageParamsChanged() {
+  auto in_left = device_->GetIntrinsics(Stream::LEFT);
+  auto in_right = device_->GetIntrinsics(Stream::RIGHT);
   InitParams(
-      device_->GetIntrinsics(Stream::LEFT),
+      *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left),
-      device_->GetIntrinsics(Stream::RIGHT),
+      *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right),
       device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
 }
 
@@ -66,7 +68,9 @@ bool RectifyProcessor::OnProcess(
 }
 
 void RectifyProcessor::InitParams(
-    Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left) {
+    IntrinsicsPinhole in_left,
+    IntrinsicsPinhole in_right,
+    Extrinsics ex_right_to_left) {
   cv::Size size{in_left.width, in_left.height};
 
   cv::Mat M1 =

src/mynteye/api/processor/rectify_processor.h

@@ -48,8 +48,8 @@ class RectifyProcessor : public Processor {
       Object *const in, Object *const out, Processor *const parent) override;
 
  private:
-  void InitParams(
+  void InitParams(IntrinsicsPinhole in_left,
-      Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left);
+        IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
 
   std::shared_ptr<Device> device_;
 };

src/mynteye/device/channels.cc

@@ -811,7 +811,7 @@ std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) {
   return i;
 }
 
-std::size_t from_data(Intrinsics *in, const std::uint8_t *data,
+std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
     const Version *spec_version) {
   std::size_t i = 0;
 
@@ -954,7 +954,7 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data,
   return size + 3;
 }
 
-std::size_t to_data(const Intrinsics *in, std::uint8_t *data,
+std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
     const Version *spec_version) {
   std::size_t i = 0;
 

src/mynteye/device/channels.h

@@ -204,7 +204,7 @@ std::string _from_data(const std::uint8_t *data, std::size_t count);
 
 std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data);
 
-std::size_t from_data(Intrinsics *in, const std::uint8_t *data,
+std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
     const Version *spec_version);
 
 std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
@@ -236,7 +236,7 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count);
 std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data,
     const Version *spec_version);
 
-std::size_t to_data(const Intrinsics *in, std::uint8_t *data,
+std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
     const Version *spec_version);
 
 std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data,

src/mynteye/device/device.cc

@@ -240,7 +240,8 @@ std::string Device::GetInfo(const Info &info) const {
   }
 }
 
-Intrinsics Device::GetIntrinsics(const Stream &stream) const {
+std::shared_ptr<IntrinsicsBase> Device::GetIntrinsics(
+    const Stream &stream) const {
   bool ok;
   return GetIntrinsics(stream, &ok);
 }
@@ -260,7 +261,8 @@ Extrinsics Device::GetMotionExtrinsics(const Stream &from) const {
   return GetMotionExtrinsics(from, &ok);
 }
 
-Intrinsics Device::GetIntrinsics(const Stream &stream, bool *ok) const {
+std::shared_ptr<IntrinsicsBase> Device::GetIntrinsics(
+    const Stream &stream, bool *ok) const {
   try {
     *ok = true;
     return stream_intrinsics_.at(stream);
@@ -311,7 +313,8 @@ Extrinsics Device::GetMotionExtrinsics(const Stream &from, bool *ok) const {
   }
 }
 
-void Device::SetIntrinsics(const Stream &stream, const Intrinsics &in) {
+void Device::SetIntrinsics(const Stream &stream,
+    const std::shared_ptr<IntrinsicsBase> &in) {
   stream_intrinsics_[stream] = in;
 }
 

src/mynteye/device/standard/channels_adapter_s.cc

@@ -131,11 +131,12 @@ std::size_t StandardChannelsAdapter::GetImgParamsFromData(
     Channels::img_params_t *img_params) {
   std::size_t i = 0;
 
-  Intrinsics in_left, in_right;
+  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, data + i, version);
+  i += bytes::from_data(in_left.get(), data + i, version);
-  i += bytes::from_data(&in_right, data + i, version);
+  i += bytes::from_data(in_right.get(), data + i, version);
   i += bytes::from_data(&ex_right_to_left, data + i, version);
   (*img_params)[{752, 480}] = {true, in_left, in_right, ex_right_to_left};
 
@@ -147,9 +148,11 @@ std::size_t StandardChannelsAdapter::SetImgParamsToData(
     std::uint8_t *data) {
   std::size_t i = 3;  // skip id, size
 
-  auto &&params = (*img_params).at({752, 480});
+  auto params = (*img_params).at({752, 480});
-  i += bytes::to_data(&params.in_left, data + i, version);
+  auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
-  i += bytes::to_data(&params.in_right, data + i, version);
+  auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_right);
+  i += bytes::to_data(in_left.get(), data + i, version);
+  i += bytes::to_data(in_right.get(), data + i, version);
   i += bytes::to_data(&params.ex_right_to_left, data + i, version);
 
   // others

src/mynteye/device/standard2/channels_adapter_s2.cc

@@ -127,20 +127,26 @@ std::size_t Standard2ChannelsAdapter::GetImgParamsFromData(
     Channels::img_params_t *img_params) {
   std::size_t i = 0;
 
-  Intrinsics in_left, in_right;
   Extrinsics ex_right_to_left;
-
+  {
-  i += bytes::from_data(&in_left, data + i, version);
+    auto in_left = std::make_shared<IntrinsicsPinhole>();
-  i += bytes::from_data(&in_right, data + i, version);
+    auto in_right = std::make_shared<IntrinsicsPinhole>();
+    i += bytes::from_data(in_left.get(), data + i, version);
+    i += bytes::from_data(in_right.get(), data + i, version);
     (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left};
-
+  }
-  i += bytes::from_data(&in_left, data + i, version);
+  {
-  i += bytes::from_data(&in_right, data + i, version);
+    auto in_left = std::make_shared<IntrinsicsPinhole>();
+    auto in_right = std::make_shared<IntrinsicsPinhole>();
+    i += bytes::from_data(in_left.get(), data + i, version);
+    i += bytes::from_data(in_right.get(), data + i, version);
     (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left};
-
+  }
+  {
     i += bytes::from_data(&ex_right_to_left, data + i, version);
     (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left;
     (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
+  }
 
   return i;
 }
@@ -151,14 +157,20 @@ std::size_t Standard2ChannelsAdapter::SetImgParamsToData(
   std::size_t i = 3;  // skip id, size
 
   {
-    auto &&params = (*img_params).at({1280, 400});
+    auto params = (*img_params).at({1280, 400});
-    i += bytes::to_data(&params.in_left, data + i, version);
+    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
-    i += bytes::to_data(&params.in_right, data + i, version);
+    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+        params.in_right);
+    i += bytes::to_data(in_left.get(), data + i, version);
+    i += bytes::to_data(in_right.get(), data + i, version);
   }
   {
-    auto &&params = (*img_params).at({2560, 800});
+    auto params = (*img_params).at({2560, 800});
-    i += bytes::to_data(&params.in_left, data + i, version);
+    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
-    i += bytes::to_data(&params.in_right, data + i, version);
+    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+        params.in_right);
+    i += bytes::to_data(in_left.get(), data + i, version);
+    i += bytes::to_data(in_right.get(), data + i, version);
     i += bytes::to_data(&params.ex_right_to_left, data + i, version);
   }
 

src/mynteye/device/standard2/channels_adapter_s210a.cc

@@ -127,20 +127,26 @@ std::size_t Standard210aChannelsAdapter::GetImgParamsFromData(
     Channels::img_params_t *img_params) {
   std::size_t i = 0;
 
-  Intrinsics in_left, in_right;
   Extrinsics ex_right_to_left;
-
+  {
-  i += bytes::from_data(&in_left, data + i, version);
+    auto in_left = std::make_shared<IntrinsicsPinhole>();
-  i += bytes::from_data(&in_right, data + i, version);
+    auto in_right = std::make_shared<IntrinsicsPinhole>();
+    i += bytes::from_data(in_left.get(), data + i, version);
+    i += bytes::from_data(in_right.get(), data + i, version);
     (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left};
-
+  }
-  i += bytes::from_data(&in_left, data + i, version);
+  {
-  i += bytes::from_data(&in_right, data + i, version);
+    auto in_left = std::make_shared<IntrinsicsPinhole>();
+    auto in_right = std::make_shared<IntrinsicsPinhole>();
+    i += bytes::from_data(in_left.get(), data + i, version);
+    i += bytes::from_data(in_right.get(), data + i, version);
     (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left};
-
+  }
+  {
     i += bytes::from_data(&ex_right_to_left, data + i, version);
     (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left;
     (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
+  }
 
   return i;
 }
@@ -151,14 +157,20 @@ std::size_t Standard210aChannelsAdapter::SetImgParamsToData(
   std::size_t i = 3;  // skip id, size
 
   {
-    auto &&params = (*img_params).at({1280, 400});
+    auto params = (*img_params).at({1280, 400});
-    i += bytes::to_data(&params.in_left, data + i, version);
+    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
-    i += bytes::to_data(&params.in_right, data + i, version);
+    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+        params.in_right);
+    i += bytes::to_data(in_left.get(), data + i, version);
+    i += bytes::to_data(in_right.get(), data + i, version);
   }
   {
-    auto &&params = (*img_params).at({2560, 800});
+    auto params = (*img_params).at({2560, 800});
-    i += bytes::to_data(&params.in_left, data + i, version);
+    auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
-    i += bytes::to_data(&params.in_right, data + i, version);
+    auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
+        params.in_right);
+    i += bytes::to_data(in_left.get(), data + i, version);
+    i += bytes::to_data(in_right.get(), data + i, version);
     i += bytes::to_data(&params.ex_right_to_left, data + i, version);
   }
 

tools/writer/device_writer.cc

@@ -113,7 +113,7 @@ bool DeviceWriter::WriteImuParams(const std::string &filepath) {
 
 namespace {
 
-cv::FileStorage &operator<<(cv::FileStorage &fs, const Intrinsics &in) {
+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
@@ -152,8 +152,10 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) {
 cv::FileStorage &operator<<(
     cv::FileStorage &fs, const device::img_params_t &params) {
   fs << "{"
-     << "in_left" << params.in_left
+     << "in_left"
-     << "in_right" << params.in_right
+     << *std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left)
+     << "in_right"
+     << *std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_right)
      << "ex_right_to_left" << params.ex_right_to_left << "}";
   return fs;
 }
@@ -241,7 +243,7 @@ namespace {
 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,
-    Intrinsics *in) {
+    IntrinsicsPinhole *in) {
   in->width = width;
   in->height = height;
   /*
@@ -272,7 +274,7 @@ void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) {
   }
 }
 
-void operator>>(const cv::FileNode &n, Intrinsics &in) {
+void operator>>(const cv::FileNode &n, IntrinsicsPinhole &in) {
   n["width"] >> in.width;
   n["height"] >> in.height;
   n["fx"] >> in.fx;
@@ -314,8 +316,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;
+  auto in_left = std::make_shared<IntrinsicsPinhole>();
-  n["in_right"] >> paramas.in_right;
+  auto in_right = std::make_shared<IntrinsicsPinhole>();
+  paramas.in_left = in_left;
+  paramas.in_right = in_right;
+  n["in_left"] >> *in_left;
+  n["in_right"] >> *in_right;
   n["ex_right_to_left"] >> paramas.ex_right_to_left;
 }
 
@@ -346,6 +352,10 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams(
 
   img_params_map_t img_params_map;
   if (fs["version"].isNone()) {
+    auto in_left = std::make_shared<IntrinsicsPinhole>();
+    auto in_right = std::make_shared<IntrinsicsPinhole>();
+    img_params_map[{752, 480}].in_left = in_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;
@@ -366,13 +376,13 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams(
       fs["T"] >> T;
 
       to_intrinsics(
-          w, h, m, M1, D1, &img_params_map[{752, 480}].in_left);
+          w, h, m, M1, D1, in_left.get());
       to_intrinsics(
-          w, h, m, M2, D2, &img_params_map[{752, 480}].in_right);
+          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] >> img_params_map[{752, 480}].in_left;
+      fs["in_left"][0] >> *in_left;
-      fs["in_right"][0] >> img_params_map[{752, 480}].in_right;
+      fs["in_right"][0] >> *in_right;
       fs["ex_right_to_left"] >> img_params_map[{752, 480}].ex_right_to_left;
     }
   } else {