feat(*): add depth resize

include/mynteye/types.h

@@ -424,6 +424,7 @@ struct MYNTEYE_API IntrinsicsBase {
     calib_model_ = CalibrationModel::UNKNOW;
   }
   virtual ~IntrinsicsBase() {}
+  virtual void ResizeIntrinsics() {}
 
   /** The calibration model */
   CalibrationModel calib_model() const {
@@ -433,7 +434,8 @@ struct MYNTEYE_API IntrinsicsBase {
   std::uint16_t width;
   /** The height of the image in pixels */
   std::uint16_t height;
-
+  /** Resize scale */
+  double resize_scale = 1.0;
  protected:
   CalibrationModel calib_model_;
 };
@@ -449,6 +451,14 @@ struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase {
   IntrinsicsPinhole() {
     calib_model_ = CalibrationModel::PINHOLE;
   }
+  void ResizeIntrinsics() {
+    width = static_cast<std::uint16_t>(width * resize_scale);
+    height = static_cast<std::uint16_t>(height * resize_scale);
+    fx *= resize_scale;
+    fy *= resize_scale;
+    cx *= resize_scale;
+    cy *= resize_scale;
+  }
   /** 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 */
@@ -481,6 +491,14 @@ struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
   }
   /** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */
   double coeffs[8];
+  void ResizeIntrinsics() {
+    width = static_cast<std::uint16_t>(width * resize_scale);
+    height = static_cast<std::uint16_t>(height * resize_scale);
+    coeffs[4] *= resize_scale;
+    coeffs[5] *= resize_scale;
+    coeffs[6] *= resize_scale;
+    coeffs[7] *= resize_scale;
+  }
 };
 
 MYNTEYE_API

samples/tutorials/data/get_img_params.cc

@@ -25,11 +25,22 @@ int main(int argc, char *argv[]) {
   auto &&request = api->SelectStreamRequest(&ok);
   if (!ok) return 1;
   api->ConfigStreamRequest(request);
-
+  auto in_left = api->GetIntrinsicsBase(Stream::LEFT);
-  LOG(INFO) << "Intrinsics left: {" << *api->GetIntrinsicsBase(Stream::LEFT)
+  auto in_right = api->GetIntrinsicsBase(Stream::RIGHT);
-            << "}";
+  if (in_left->calib_model() == CalibrationModel::PINHOLE) {
-  LOG(INFO) << "Intrinsics right: {" << *api->GetIntrinsicsBase(Stream::RIGHT)
+    in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left);
-            << "}";
+    in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right);
+  } else if (in_left->calib_model() == CalibrationModel::KANNALA_BRANDT) {
+    in_left = std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left);
+    in_right = std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right);
+  } else {
+    LOG(INFO) << "UNKNOW CALIB MODEL.";
+    return 0;
+  }
+  in_left -> ResizeIntrinsics();
+  in_right -> ResizeIntrinsics();
+  LOG(INFO) << "Intrinsics left: {" << *in_left << "}";
+  LOG(INFO) << "Intrinsics right: {" << *in_right << "}";
   LOG(INFO) << "Extrinsics right to left: {"
             << api->GetExtrinsics(Stream::RIGHT, Stream::LEFT) << "}";
 

src/mynteye/api/processor/rectify_processor.cc

@@ -335,6 +335,8 @@ void RectifyProcessor::InitParams(
     IntrinsicsEquidistant in_right,
     Extrinsics ex_right_to_left) {
   calib_model = CalibrationModel::KANNALA_BRANDT;
+  in_left.ResizeIntrinsics();
+  in_right.ResizeIntrinsics();
   camodocal::CameraPtr camera_odo_ptr_left =
       generateCameraFromIntrinsicsEquidistant(in_left);
   camodocal::CameraPtr camera_odo_ptr_right =

src/mynteye/api/processor/rectify_processor_ocv.cc

@@ -80,6 +80,8 @@ void RectifyProcessorOCV::InitParams(
     IntrinsicsPinhole in_right,
     Extrinsics ex_right_to_left) {
   calib_model = CalibrationModel::PINHOLE;
+  in_left.ResizeIntrinsics();
+  in_right.ResizeIntrinsics();
   cv::Size size{in_left.width, in_left.height};
 
   cv::Mat M1 =

src/mynteye/device/channel/channels.cc

@@ -483,7 +483,7 @@ bool Channels::GetFiles(
           if (imu_params->ok) {
             auto &&n = file_channel_.GetImuParamsFromData(
                 data + i, file_size, imu_params);
-            CHECK_EQ(n, file_size);
+            // CHECK_EQ(n, file_size);
           }
         } break;
         default:

src/mynteye/device/device.cc

@@ -656,7 +656,18 @@ void Device::UpdateStreamIntrinsics(
             img_res.height == request.GetResolution().height &&
             img_res.width == request.GetResolution().width / 2;
     } else if (capability == Capabilities::STEREO) {
-      ok = img_params.ok && img_res == request.GetResolution();
+      if (img_res == request.GetResolution()) {
+        ok = img_params.ok;
+      } else if (request.GetResolution().height / img_res.height ==
+                request.GetResolution().width / img_res.width) {
+        double scale = static_cast<double> (
+          1.0 * request.GetResolution().height / img_res.height);
+        img_params.in_left->resize_scale = scale;
+        img_params.in_right->resize_scale = scale;
+        ok = img_params.ok;
+      } else {
+        ok = false;
+      }
     }
     if (ok) {
       SetIntrinsics(Stream::LEFT, img_params.in_left);

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -54,7 +54,14 @@
   <!-- MYNTEYE-S2100/S2000/S210A, Reslution: 2560x800, Format: YUYV/BGR888, Fps: 30 -->
   <arg name="index_s2_6"   default="6" />
 
-  <arg name="request_index" default="$(arg index_s2_2)" />
+  <arg name="standard2/request_index" default="$(arg index_s2_2)" />
+
+  <!-- MYNTEYE-S1030, Reslution: 752x480, Format: YUYV -->
+  <arg name="index_s_0"   default="0" />
+  <!-- MYNTEYE-S1030, Reslution: 376x240, Format: YUYV -->
+  <arg name="index_s_1"   default="1" />
+
+  <arg name="standard/request_index" default="$(arg index_s_0)" />
 
   <!-- disparity computing method type -->
   <arg name="sgbm"   default="0" />
@@ -259,7 +266,8 @@
       <param name="enable_depth" value="$(arg enable_depth)" />
 
       <!-- stream request index -->
-      <param name="request_index" value="$(arg request_index)" />
+      <param name="standard/request_index" value="$(arg standard/request_index)" />
+      <param name="standard2/request_index" value="$(arg standard2/request_index)" />
 
       <!-- device options of standard-->
 

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

@@ -1062,7 +1062,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     model_ = api_->GetModel();
     if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
-      private_nh_.getParam("request_index", request_index);
+      private_nh_.getParam("standard2/request_index", request_index);
       switch (request_index) {
         case 0:
         case 4:
@@ -1082,7 +1082,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
     }
     if (model_ == Model::STANDARD) {
-      request_index = 0;
+      private_nh_.getParam("standard/request_index", request_index);
       frame_rate_ = api_->GetOptionValue(Option::FRAME_RATE);
     }
 
@@ -1217,11 +1217,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     }
 
     camera_info->header.frame_id = frame_ids_[stream];
-    camera_info->width = in_base->width;
-    camera_info->height = in_base->height;
 
     if (in_base->calib_model() == CalibrationModel::PINHOLE) {
       auto in = std::dynamic_pointer_cast<IntrinsicsPinhole>(in_base);
+      in -> ResizeIntrinsics();
+      camera_info->width = in_base->width;
+      camera_info->height = in_base->height;
       //     [fx  0 cx]
       // K = [ 0 fy cy]
       //     [ 0  0  1]
@@ -1253,7 +1254,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
     } else if (in_base->calib_model() == CalibrationModel::KANNALA_BRANDT) {
       auto in = std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_base);
-
+      in -> ResizeIntrinsics();
+      camera_info->width = in_base->width;
+      camera_info->height = in_base->height;
       camera_info->distortion_model = "kannala_brandt";
 
       // coeffs: k2,k3,k4,k5,mu,mv,u0,v0