Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into develop

CMakeLists.txt

@@ -14,7 +14,7 @@
 
 cmake_minimum_required(VERSION 3.0)
 
-project(mynteye VERSION 2.4.0 LANGUAGES C CXX)
+project(mynteye VERSION 2.4.1 LANGUAGES C CXX)
 
 include(cmake/Common.cmake)
 

README.md

@@ -1,6 +1,6 @@
 # MYNT® EYE S SDK
 
-[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.4.0-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
+[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.4.1-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
 
 ## Overview
 

docs/Doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.4.0
+PROJECT_NUMBER         = 2.4.1
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

docs/conf.py

@@ -24,7 +24,7 @@ copyright = '2019, MYNTAI'
 author = 'MYNTAI'
 
 # The short X.Y version
-version = '2.4.0'
+version = '2.4.1'
 # The full version, including alpha/beta/rc tags
 release = version
 

docs/src/sdk/sdk_changelog.rst

@@ -3,6 +3,11 @@
 Change Log
 ==========
 
+2019-08-17(v2.4.1)
+-------------------
+
+1. Optimize disparity calculation
+
 2019-08-09(v2.4.0)
 -------------------
 

include/mynteye/types.h

@@ -775,6 +775,7 @@ struct CameraROSMsgInfoPair {
   struct CameraROSMsgInfo left;
   struct CameraROSMsgInfo right;
   double T_mul_f = -1.f;
+  double cx1_minus_cx2 = 0.f;
   double  R[9] = {0};
   double  P[12] = {0};
 };

src/mynteye/api/processor/disparity_processor.cc

@@ -27,8 +27,14 @@ MYNTEYE_BEGIN_NAMESPACE
 const char DisparityProcessor::NAME[] = "DisparityProcessor";
 
 DisparityProcessor::DisparityProcessor(DisparityComputingMethod type,
+    std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
     std::int32_t proc_period)
     : Processor(std::move(proc_period)), type_(type) {
+  if (calib_infos) {
+    cx1_minus_cx2_ = calib_infos->cx1_minus_cx2;
+  } else {
+    cx1_minus_cx2_ = 1.f;
+  }
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
   int sgbmWinSize = 3;
   int numberOfDisparities = 64;
@@ -156,6 +162,7 @@ bool DisparityProcessor::OnProcess(
   } else if (type_ == DisparityComputingMethod::BM) {
     // LOG(ERROR) << "not supported in opencv 2.x";
     (*sgbm_matcher)(input->first, input->second, disparity);
+    disparity.convertTo(output->value, CV_32F, 1./16, 1);
     // cv::Mat tmp1, tmp2;
     // cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
     // cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
@@ -188,7 +195,7 @@ bool DisparityProcessor::OnProcess(
     sgbm_matcher->compute(input->first, input->second, disparity);
   }
 #endif
-  disparity.convertTo(output->value, CV_32F, 1./16, 1);
+  disparity.convertTo(output->value, CV_32F, 1./16, cx1_minus_cx2_);
   output->id = input->first_id;
   output->data = input->first_data;
   return true;

src/mynteye/api/processor/disparity_processor.h

@@ -32,6 +32,7 @@ class DisparityProcessor : public Processor {
   static const char NAME[];
 
   explicit DisparityProcessor(DisparityComputingMethod type,
+      std::shared_ptr<struct CameraROSMsgInfoPair> calib_infos,
       std::int32_t proc_period = 0);
   virtual ~DisparityProcessor();
 
@@ -54,6 +55,7 @@ class DisparityProcessor : public Processor {
   cv::Ptr<cv::StereoSGBM> sgbm_matcher;
   cv::Ptr<cv::StereoBM> bm_matcher;
   DisparityComputingMethod type_;
+  double cx1_minus_cx2_;
 };
 
 MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/rectify_processor.cc

@@ -42,6 +42,7 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
     const CvMat* D1, const CvMat* D2, CvSize imageSize,
     const CvMat* matR, const CvMat* matT,
     CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
+    double *cx1_min_cx2,
     int flags, double alpha, CvSize newImgSize) {
   // std::cout << _alpha << std::endl;
   alpha = _alpha;
@@ -218,6 +219,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
     cvmSet(_P2, 0, 2, cx2);
     cvmSet(_P2, 1, 2, cy2);
     cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
+
+    *cx1_min_cx2 = -(cx1 - cx2);
   }
 }
 
@@ -299,8 +302,10 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   CvMat c_K1 = K1, c_K2 = K2, c_D1 = D1, c_D2 = D2;
   CvMat c_R1 = R1, c_R2 = R2, c_P1 = P1, c_P2 = P2;
   double T_mul_f;
+  double cx1_min_cx2;
   stereoRectify(leftOdo, rightOdo, &c_K1, &c_K2, &c_D1, &c_D2,
-      image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f);
+      image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f,
+      &cx1_min_cx2);
 
   VLOG(2) << "K1: " << K1 << std::endl;
   VLOG(2) << "D1: " << D1 << std::endl;
@@ -334,6 +339,7 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
   info_pair.left = calib_mat_data_left;
   info_pair.right = calib_mat_data_right;
   info_pair.T_mul_f = T_mul_f;
+  info_pair.cx1_minus_cx2 = cx1_min_cx2;
   for (std::size_t i = 0; i< 3 * 4; i++) {
     info_pair.P[i] = calib_mat_data_left.P[i];
   }

src/mynteye/api/processor/rectify_processor.h

@@ -87,6 +87,7 @@ class RectifyProcessor : public Processor {
       const CvMat* D1, const CvMat* D2, CvSize imageSize,
       const CvMat* matR, const CvMat* matT,
       CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, double* T_mul_f,
+      double *cx1_min_cx2,
       int flags = cv::CALIB_ZERO_DISPARITY, double alpha = -1,
       CvSize newImgSize = cv::Size());
 

src/mynteye/api/synthetic.cc

@@ -313,10 +313,8 @@ void Synthetic::InitProcessors() {
   std::shared_ptr<Processor> rectify_processor = nullptr;
   std::shared_ptr<Processor> points_processor = nullptr;
   std::shared_ptr<Processor> depth_processor = nullptr;
+  std::shared_ptr<Processor> disparity_processor = nullptr;
 
-  auto &&disparity_processor =
-      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
-                                           DISPARITY_PROC_PERIOD);
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>(
           DISPARITY_NORM_PROC_PERIOD);
@@ -333,6 +331,10 @@ void Synthetic::InitProcessors() {
     points_processor = std::make_shared<PointsProcessorOCV>(
         rectify_processor_ocv->Q, POINTS_PROC_PERIOD);
     depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+    disparity_processor =
+      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
+                                           nullptr,
+                                           DISPARITY_PROC_PERIOD);
 
     root_processor->AddChild(rectify_processor);
     rectify_processor->AddChild(disparity_processor);
@@ -352,6 +354,10 @@ void Synthetic::InitProcessors() {
     depth_processor = std::make_shared<DepthProcessor>(
         rectify_processor_imp -> getCameraROSMsgInfoPair(),
         DEPTH_PROC_PERIOD);
+    disparity_processor =
+      std::make_shared<DisparityProcessor>(DisparityComputingMethod::BM,
+          rectify_processor_imp -> getCameraROSMsgInfoPair(),
+          DISPARITY_PROC_PERIOD);
 
     root_processor->AddChild(rectify_processor);
     rectify_processor->AddChild(disparity_processor);

src/mynteye/device/config.cc

@@ -70,8 +70,8 @@ const std::map<Model, std::map<Capabilities, StreamRequests>>
 stream_requests_map = {
   {Model::STANDARD,
     {{Capabilities::STEREO, {
-      {752, 480, Format::YUYV, 0},
-      {376, 240, Format::YUYV, 0}}
+      {752, 480, Format::YUYV, 60},
+      {376, 240, Format::YUYV, 60}}
     }}
   },
   {Model::STANDARD2,

wrappers/ros/src/mynt_eye_ros_wrapper/config/device/standard2.yaml

@@ -37,7 +37,7 @@ standard2/exposure_mode: -1
 standard2/max_gain: -1
 # standard2/max_gain: 8
 
-# standard2/max_exposure_time range: [0,1000]
+# standard2/max_exposure_time range: [0,655]
 standard2/max_exposure_time: -1
 # standard2/max_exposure_time: 333
 
@@ -79,7 +79,7 @@ standard210a/exposure_mode: -1
 standard210a/max_gain: -1
 # standard210a/max_gain: 8
 
-# standard210a/max_exposure_time range: [0,1000]
+# standard210a/max_exposure_time range: [0,655]
 standard210a/max_exposure_time: -1
 # standard210a/max_exposure_time: 333
 
@@ -125,7 +125,7 @@ standard200b/exposure_mode: -1
 standard200b/max_gain: -1
 # standard200b/max_gain: 8
 
-# standard200b/max_exposure_time range: [0,1000]
+# standard200b/max_exposure_time range: [0,655]
 standard200b/max_exposure_time: -1
 # standard200b/max_exposure_time: 333
 
@@ -151,4 +151,4 @@ standard200b/accel_low_filter: -1
 
 # standard200b/gyro_low_filter range: [23,64]
 standard200b/gyro_low_filter: -1
-# standard200b/gyro_low_filter: 64
+# standard200b/gyro_low_filter: 64