From b3bc96f12b15c784c10cf16195691194db994b43 Mon Sep 17 00:00:00 2001
From: TinyOh <tinyoh@slightech.com>
Date: Tue, 15 Jan 2019 17:18:16 +0800
Subject: [PATCH] fix(ros): add default intrinsics in ros node

---
 .../api/processor/disparity_processor.cc      | 14 +++-
 .../src/wrapper_nodelet.cc                    | 70 +++++++++++++++++--
 2 files changed, 75 insertions(+), 9 deletions(-)

diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc
index b209291..18134af 100644
--- a/src/mynteye/api/processor/disparity_processor.cc
+++ b/src/mynteye/api/processor/disparity_processor.cc
@@ -23,6 +23,8 @@
 
 #include "mynteye/logger.h"
 
+#define WITH_BM_SOBEL_FILTER
+
 MYNTEYE_BEGIN_NAMESPACE
 
 const char DisparityProcessor::NAME[] = "DisparityProcessor";
@@ -67,6 +69,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type,
 #endif
 #ifdef WITH_BM_SOBEL_FILTER
   } else if (type_ == DisparityProcessorType::BM) {
+    int bmWinSize = 3;
 #ifdef WITH_OPENCV2
     int bmWinSize = 3;
     // StereoBM
@@ -86,7 +89,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type,
     bm_matcher = cv::StereoBM::create(0, 3);
     bm_matcher->setPreFilterSize(9);
     bm_matcher->setPreFilterCap(31);
-    bm_matcher->setBlockSize(15);
+    bm_matcher->setBlockSize(bmWinSize);
     bm_matcher->setMinDisparity(0);
     bm_matcher->setNumDisparities(64);
     bm_matcher->setUniquenessRatio(15);
@@ -183,8 +186,13 @@ bool DisparityProcessor::OnProcess(
 #ifdef WITH_BM_SOBEL_FILTER
   } else if (type_ == DisparityProcessorType::BM) {
     cv::Mat tmp1, tmp2;
-    cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
-    cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
+    if (input->first.channels() == 1) {
+      // s1030
+    } else if (input->first.channels() == 3) {
+      // s210
+      cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
+      cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
+    }
     bm_matcher->compute(tmp1, tmp2, disparity);
 #endif
   } else {
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 5929cf9..fd433e8 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -924,18 +924,71 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     computeRectTransforms();
   }
 
+  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 computeRectTransforms() {
     ROS_ASSERT(api_);
     auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT);
     auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT);
-    if (in_left_base->calib_model() != CalibrationModel::PINHOLE ||
-        in_right_base->calib_model() != CalibrationModel::PINHOLE) {
-      return;
+    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 =
@@ -975,10 +1028,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info);
 
     std::shared_ptr<IntrinsicsBase> in_base;
-    if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
-      in_base = api_->GetIntrinsicsBase(Stream::RIGHT);
+    if (is_intrinsics_enable_) {
+      if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
+        in_base = api_->GetIntrinsicsBase(Stream::RIGHT);
+      } else {
+        in_base = api_->GetIntrinsicsBase(Stream::LEFT);
+      }
     } else {
-      in_base = api_->GetIntrinsicsBase(Stream::LEFT);
+      in_base = getDefaultIntrinsics();
     }
 
     camera_info->header.frame_id = frame_ids_[stream];
@@ -1275,6 +1332,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   bool is_motion_published_;
   bool is_started_;
   int frame_rate_;
+  bool is_intrinsics_enable_;
 };
 
 MYNTEYE_END_NAMESPACE