fix(src): Determine the default intrinsics based on the resulution

CMakeLists.txt

@@ -218,7 +218,6 @@ if(WITH_API)
     src/mynteye/api/processor/points_processor_ocv.cc
     src/mynteye/api/processor/depth_processor_ocv.cc
     src/mynteye/api/processor/rectify_processor_ocv.cc
-    src/mynteye/api/config.cc
     src/mynteye/api/correspondence.cc
     src/mynteye/api/version_checker.cc
     src/mynteye/api/data_tools.cc

include/mynteye/device/device.h

@@ -300,6 +300,8 @@ class MYNTEYE_API Device {
   /** Enable process mode, e.g. imu assembly, temp_drift */
   void EnableProcessMode(const std::int32_t& mode);
 
+  bool CheckImageParams();
+
  protected:
   std::shared_ptr<uvc::device> device() const {
     return device_;
@@ -379,6 +381,8 @@ class MYNTEYE_API Device {
   bool SetFiles(
       DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params);
 
+  bool is_default_intrinsics_;
+
   friend API;
   friend tools::DeviceWriter;
 };

src/mynteye/api/api.cc

@@ -244,18 +244,7 @@ std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &device) {
     bool in_l_ok, in_r_ok;
     auto left_intr  = device->GetIntrinsics(Stream::LEFT, &in_l_ok);
     auto right_intr = device->GetIntrinsics(Stream::RIGHT, &in_r_ok);
-    if (!in_l_ok || !in_r_ok) {
+
-      LOG(ERROR) << "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.";
-      LOG(WARNING) << "use pinhole as default";
-      api = std::make_shared<API>(device, CalibrationModel::UNKNOW);
-      return api;
-    } else {
     if (left_intr->calib_model() != right_intr->calib_model()) {
       LOG(ERROR) << "left camera and right camera use different calib models!";
       LOG(WARNING) << "use pinhole as default";
@@ -265,7 +254,6 @@ std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &device) {
       api = std::make_shared<API>(device, left_intr->calib_model());
       return api;
     }
-    }
   } else {
     LOG(ERROR) <<"no device!";
     return nullptr;
@@ -573,11 +561,7 @@ std::shared_ptr<Device> API::device() {
 // TODO(Kalman): Call this function in the appropriate place
 void API::CheckImageParams() {
   if (device_ != nullptr) {
-    bool in_l_ok, in_r_ok, ex_l2r_ok;
+    if (device_->CheckImageParams()) {
-    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 "

src/mynteye/api/config.cc

@@ -1,95 +0,0 @@
-// Copyright 2018 Slightech Co., Ltd. All rights reserved.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "mynteye/api/config.h"
-
-MYNTEYE_BEGIN_NAMESPACE
-/**
- * default intrinsics
- */
-
-std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
-  auto res = std::make_shared<IntrinsicsPinhole>();
-  res->width = 640;
-  res->height = 400;
-  res->model = 0;
-  res->fx = 3.6220059643202876e+02;
-  res->fy = 3.6350065250745848e+02;
-  res->cx = 4.0658699068023441e+02;
-  res->cy = 2.3435161110061483e+02;
-  double codffs[5] = {
-    -2.5034765682756088e-01,
-    5.0579399202897619e-02,
-    -7.0536676161976066e-04,
-    -8.5255451307033846e-03,
-    0.
-  };
-  for (unsigned int i = 0; i < 5; i++) {
-    res->coeffs[i] = codffs[i];
-  }
-  return res;
-}
-
-std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const StreamRequest &request) {
-  auto res = getDefaultIntrinsics();
-  res->resize_scale = static_cast<double>(request.width / res->width);
-  res->ResizeIntrinsics();
-  return res;
-}
-
-std::shared_ptr<Extrinsics> getDefaultExtrinsics() {
-  auto res = std::make_shared<Extrinsics>();
-  double rotation[9] = {
-    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
-  };
-  double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
-      -5.2058205159996538e+00};
-  for (unsigned int i = 0; i < 3; i++) {
-    for (unsigned int j = 0; j < 3; j++) {
-      res->rotation[i][j] = rotation[i*3 + j];
-    }
-  }
-  for (unsigned int i = 0; i < 3; i++) {
-    res->translation[i] = translation[i];
-  }
-  return res;
-}
-
-// sgbm_matcher = cv::StereoSGBM::create(0, 16, 3);
-// sgbm_matcher->setPreFilterCap(63);
-// sgbm_matcher->setBlockSize(sgbmWinSize);
-// sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize);
-// sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize);
-// sgbm_matcher->setMinDisparity(0);
-// sgbm_matcher->setNumDisparities(numberOfDisparities);
-// sgbm_matcher->setUniquenessRatio(10);
-// sgbm_matcher->setSpeckleWindowSize(100);
-// sgbm_matcher->setSpeckleRange(32);
-// sgbm_matcher->setDisp12MaxDiff(1);
-
-// bm_matcher = cv::StereoBM::create(0, 3);
-// bm_matcher->setPreFilterSize(9);
-// bm_matcher->setPreFilterCap(31);
-// bm_matcher->setBlockSize(15);
-// bm_matcher->setMinDisparity(0);
-// bm_matcher->setNumDisparities(64);
-// bm_matcher->setUniquenessRatio(60);
-// bm_matcher->setTextureThreshold(10);
-// bm_matcher->setSpeckleWindowSize(100);
-// bm_matcher->setSpeckleRange(4);
-
-
-MYNTEYE_END_NAMESPACE

src/mynteye/api/config.h

@@ -1,29 +0,0 @@
-// Copyright 2018 Slightech Co., Ltd. All rights reserved.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-#ifndef MYNTEYE_API_CONFIG_H_
-#define MYNTEYE_API_CONFIG_H_
-#pragma once
-
-#include "mynteye/api/api.h"
-
-MYNTEYE_BEGIN_NAMESPACE
-
-std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics();
-std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const StreamRequest &request);
-
-std::shared_ptr<Extrinsics> getDefaultExtrinsics();
-
-MYNTEYE_END_NAMESPACE
-
-#endif  // MYNTEYE_API_CONFIG_H_

src/mynteye/api/synthetic.cc

@@ -29,7 +29,6 @@
 #include "mynteye/api/processor/rectify_processor_ocv.h"
 #include "mynteye/api/processor/depth_processor_ocv.h"
 #include "mynteye/api/processor/points_processor_ocv.h"
-#include "mynteye/api/config.h"
 #ifdef WITH_CAM_MODELS
 #include "mynteye/api/processor/depth_processor.h"
 #include "mynteye/api/processor/points_processor.h"
@@ -50,27 +49,16 @@ MYNTEYE_BEGIN_NAMESPACE
 void Synthetic::InitCalibInfo() {
   if (calib_model_ == CalibrationModel::PINHOLE) {
     LOG(INFO) << "camera calib model: pinhole";
-    intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
-    intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT);
-    extr_ =  std::make_shared<Extrinsics>(
-        api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
 #ifdef WITH_CAM_MODELS
   } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
     LOG(INFO) << "camera calib model: kannala_brandt";
+#endif
+  }
+
   intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
   intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT);
   extr_ = std::make_shared<Extrinsics>(
       api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
-#endif
-  } else {
-    calib_default_tag_ = true;
-    calib_model_ = CalibrationModel::PINHOLE;
-    LOG(INFO) << "camera calib model: unknow, use default pinhole data";
-    auto stream_request = api_->GetStreamRequest();
-    intr_left_ = getDefaultIntrinsics(stream_request);
-    intr_right_ = getDefaultIntrinsics(stream_request);
-    extr_ =  getDefaultExtrinsics();
-  }
 }
 
 Synthetic::Synthetic(API *api, CalibrationModel calib_model)

src/mynteye/api/synthetic.h

@@ -22,7 +22,6 @@
 #include <mutex>
 
 #include "mynteye/api/api.h"
-#include "mynteye/api/config.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 

src/mynteye/device/config.cc

@@ -89,4 +89,80 @@ stream_requests_map = {
   }
 };
 
+/**
+ * default intrinsics
+ */
+
+std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
+  auto res = std::make_shared<IntrinsicsPinhole>();
+  res->width = 640;
+  res->height = 400;
+  res->model = 0;
+  res->fx = 3.6220059643202876e+02;
+  res->fy = 3.6350065250745848e+02;
+  res->cx = 4.0658699068023441e+02;
+  res->cy = 2.3435161110061483e+02;
+  double codffs[5] = {
+    -2.5034765682756088e-01,
+    5.0579399202897619e-02,
+    -7.0536676161976066e-04,
+    -8.5255451307033846e-03,
+    0.
+  };
+  for (unsigned int i = 0; i < 5; i++) {
+    res->coeffs[i] = codffs[i];
+  }
+  return res;
+}
+
+std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const Resolution &resolution) {
+  auto res = getDefaultIntrinsics();
+  res->resize_scale = static_cast<double>(resolution.width / res->width);
+  res->ResizeIntrinsics();
+  return res;
+}
+
+std::shared_ptr<Extrinsics> getDefaultExtrinsics() {
+  auto res = std::make_shared<Extrinsics>();
+  double rotation[9] = {
+    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
+  };
+  double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
+      -5.2058205159996538e+00};
+  for (unsigned int i = 0; i < 3; i++) {
+    for (unsigned int j = 0; j < 3; j++) {
+      res->rotation[i][j] = rotation[i*3 + j];
+    }
+  }
+  for (unsigned int i = 0; i < 3; i++) {
+    res->translation[i] = translation[i];
+  }
+  return res;
+}
+
+// sgbm_matcher = cv::StereoSGBM::create(0, 16, 3);
+// sgbm_matcher->setPreFilterCap(63);
+// sgbm_matcher->setBlockSize(sgbmWinSize);
+// sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize);
+// sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize);
+// sgbm_matcher->setMinDisparity(0);
+// sgbm_matcher->setNumDisparities(numberOfDisparities);
+// sgbm_matcher->setUniquenessRatio(10);
+// sgbm_matcher->setSpeckleWindowSize(100);
+// sgbm_matcher->setSpeckleRange(32);
+// sgbm_matcher->setDisp12MaxDiff(1);
+
+// bm_matcher = cv::StereoBM::create(0, 3);
+// bm_matcher->setPreFilterSize(9);
+// bm_matcher->setPreFilterCap(31);
+// bm_matcher->setBlockSize(15);
+// bm_matcher->setMinDisparity(0);
+// bm_matcher->setNumDisparities(64);
+// bm_matcher->setUniquenessRatio(60);
+// bm_matcher->setTextureThreshold(10);
+// bm_matcher->setSpeckleWindowSize(100);
+// bm_matcher->setSpeckleRange(4);
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/config.h

@@ -18,6 +18,7 @@
 #include <map>
 #include <set>
 #include <vector>
+#include <memory>
 
 #include "mynteye/mynteye.h"
 #include "mynteye/types.h"
@@ -37,6 +38,14 @@ using StreamRequests = std::vector<StreamRequest>;
 extern const std::map<Model, std::map<Capabilities, StreamRequests>>
     stream_requests_map;
 
+const std::vector<Resolution> resolution_list = {{640, 400},
+                                                 {1280, 800}};
+
+std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics();
+std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const Resolution &resolution);
+
+std::shared_ptr<Extrinsics> getDefaultExtrinsics();
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_DEVICE_CONFIG_H_

src/mynteye/device/device.cc

@@ -89,7 +89,8 @@ Device::Device(const Model &model,
     device_(device),
     streams_(std::make_shared<Streams>(streams_adapter)),
     channels_(std::make_shared<Channels>(device_, channels_adapter)),
-    motions_(std::make_shared<Motions>(channels_)) {
+    motions_(std::make_shared<Motions>(channels_)),
+    is_default_intrinsics_(false) {
   VLOG(2) << __func__;
   ReadAllInfos();
 }
@@ -612,11 +613,28 @@ void Device::ReadAllInfos() {
 
   motions_->SetDeviceInfo(device_info_);
 
-  bool img_params_ok = false;
+  if (all_img_params_.empty()) {
+    is_default_intrinsics_ = true;
+    LOG(ERROR) << "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.";
+    LOG(WARNING) << "Intrinsics & extrinsics not exist. Use default intrinsics.";
+    LOG(INFO) << "camera calib model: unknow, use default pinhole data";
+    auto spec_string = device_info_->spec_version.to_string();
+    for (auto &&resolution : resolution_list) {
+      all_img_params_[resolution] = {
+          true, spec_string, getDefaultIntrinsics(resolution),
+          getDefaultIntrinsics(resolution), *getDefaultExtrinsics()};
+    }
+  }
+
   for (auto &&params : all_img_params_) {
     auto &&img_params = params.second;
     if (img_params.ok) {
-      img_params_ok = true;
       SetIntrinsics(Stream::LEFT, img_params.in_left);
       SetIntrinsics(Stream::RIGHT, img_params.in_right);
       SetExtrinsics(Stream::RIGHT, Stream::LEFT, img_params.ex_right_to_left);
@@ -627,9 +645,6 @@ void Device::ReadAllInfos() {
       break;
     }
   }
-  if (!img_params_ok) {
-    LOG(WARNING) << "Intrinsics & extrinsics not exist";
-  }
 
   if (imu_params.ok) {
     imu_params_ = imu_params;
@@ -726,4 +741,8 @@ void Device::EnableProcessMode(const std::int32_t& mode) {
   motions_->EnableProcessMode(mode);
 }
 
+bool Device::CheckImageParams() {
+  return is_default_intrinsics_;
+}
+
 MYNTEYE_END_NAMESPACE

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -1200,48 +1200,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     return nullptr;
   }
 
-  std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
-    auto res = std::make_shared<IntrinsicsPinhole>();
-    res->width = 640;
-    res->height = 400;
-    res->model = 0;
-    res->fx = 3.6220059643202876e+02;
-    res->fy = 3.6350065250745848e+02;
-    res->cx = 4.0658699068023441e+02;
-    res->cy = 2.3435161110061483e+02;
-    double codffs[5] = {
-      -2.5034765682756088e-01,
-      5.0579399202897619e-02,
-      -7.0536676161976066e-04,
-      -8.5255451307033846e-03,
-      0.
-    };
-    for (unsigned int i = 0; i < 5; i++) {
-      res->coeffs[i] = codffs[i];
-    }
-    return res;
-  }
-
-  std::shared_ptr<Extrinsics> getDefaultExtrinsics() {
-    auto res = std::make_shared<Extrinsics>();
-    double rotation[9] = {
-      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
-    };
-    double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
-        -5.2058205159996538e+00};
-    for (unsigned int i = 0; i < 3; i++) {
-      for (unsigned int j = 0; j < 3; j++) {
-        res->rotation[i][j] = rotation[i*3 + j];
-      }
-    }
-    for (unsigned int i = 0; i < 3; i++) {
-      res->translation[i] = translation[i];
-    }
-    return res;
-  }
-
   void publishMesh() {
     mesh_msg_.header.frame_id = base_frame_id_;
     mesh_msg_.header.stamp = ros::Time::now();
@@ -1279,24 +1237,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     ROS_ASSERT(api_);
     auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT);
     auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT);
-    is_intrinsics_enable_ = in_left_base && in_right_base;
-    if (is_intrinsics_enable_) {
     if (in_left_base->calib_model() != CalibrationModel::PINHOLE ||
         in_right_base->calib_model() != CalibrationModel::PINHOLE) {
       return;
     }
-    } else {
-      in_left_base = getDefaultIntrinsics();
-      in_right_base = getDefaultIntrinsics();
-    }
 
     auto in_left = *std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left_base);
     auto in_right = *std::dynamic_pointer_cast<IntrinsicsPinhole>(
         in_right_base);
     auto ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT);
-    if (!is_intrinsics_enable_) {
-      ex_right_to_left = *(getDefaultExtrinsics());
-    }
 
     cv::Size size{in_left.width, in_left.height};
     cv::Mat M1 =