Merge branch 'develop' into feature/android

* develop:
  fix(processor): fix reload image params bug
  fix(device): fix intrinsic updating bug
  fix(api.h): remove mynteye/device.h
  fix(mynteye.launch): add device options
  fix(device.cc): get img params bug
  fix(ros): rename rect topic name in rviz
  fix(depth): delete 1000
  fix(points cloud): show the points clopud in normal size
  (fix): points cloud img ptr null
  fix(depth): fix mistake
  fix(channels.cc): compatible s_color and s
  fix(depth): add disparity limit
  feat(calib models): float change to ushort ,fix the T_mul_f number

include/mynteye/api/api.h

@@ -25,7 +25,6 @@
 
 #include "mynteye/mynteye.h"
 #include "mynteye/types.h"
-#include "mynteye/device.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 

src/mynteye/api/processor/depth_processor.cc

@@ -21,6 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE
 
 const char DepthProcessor::NAME[] = "DepthProcessor";
 
+const int DISPARITY_MIN = 0;
+const int DISPARITY_MAX = 64;
+
 DepthProcessor::DepthProcessor(
     std::shared_ptr<struct camera_calib_info_pair> calib_infos,
     std::int32_t proc_period)
@@ -48,15 +51,16 @@ bool DepthProcessor::OnProcess(
   ObjMat *output = Object::Cast<ObjMat>(out);
   int rows = input->value.rows;
   int cols = input->value.cols;
-  std::cout << calib_infos_->T_mul_f << std::endl;
+  // std::cout << calib_infos_->T_mul_f << std::endl;
   // 0.0793434
-  // TODO(MYNTEYE): Put the corresponding parameters(T,f)
+  cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U);
-  cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F);
   for (int i = 0; i < rows; i++) {
     for (int j = 0; j < cols; j++) {
       float disparity_value = input->value.at<float>(i, j);
-      float depth = calib_infos_->T_mul_f * 1000.0 / disparity_value ;
+      if (disparity_value < DISPARITY_MAX && disparity_value > DISPARITY_MIN) {
-      depth_mat.at<float>(i, j) = depth;
+        float depth = calib_infos_->T_mul_f / disparity_value;
+        depth_mat.at<ushort>(i, j) = depth;
+      }
     }
   }
   output->value = depth_mat;

src/mynteye/api/processor/points_processor.cc

@@ -32,7 +32,7 @@ struct DepthTraits<uint16_t> {
   static inline bool valid(uint16_t depth) { return depth != 0; }
   static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
   static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
-  static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
+  static inline void initializeBuffer(std::vector<uint16_t>& buffer) {} // Do nothing - already zero-filled
 };
 
 template<>
@@ -100,19 +100,19 @@ bool PointsProcessor::OnProcess(
   for (int v = 0; v < height; ++v) {
     cv::Vec3f *dptr = output->value.ptr<cv::Vec3f>(v);
     for (int u = 0; u < width; ++u) {
-      float depth = input->value.at<float>(v, u);
+      float depth = input->value.at<uint16_t>(v, u);
 
       // Missing points denoted by NaNs
-      if (!DepthTraits<float>::valid(depth)) {
+      if (!DepthTraits<uint16_t>::valid(depth)) {
         continue;
       }
-
+      dptr[u][0] = (u - center_x) * depth * constant_x ;
-      dptr[u][0] = (u - center_x) * depth * constant_x;
+      dptr[u][1] = (v - center_y) * depth * constant_y ;
-      dptr[u][1] = (v - center_y) * depth * constant_y;
+      dptr[u][2] = depth ;
-      dptr[u][2] = DepthTraits<float>::toMeters(depth);
     }
   }
-
+  output->id = input->id;
+  output->data = input->data;
   return true;
 }
 

src/mynteye/api/processor/rectify_processor.cc

@@ -167,7 +167,7 @@ void RectifyProcessor::stereoRectify(camodocal::CameraPtr leftOdo,
   _pp[0][2] = cc_new[1].x;
   _pp[1][2] = cc_new[1].y;
   _pp[idx][3] = _t[idx]*fc_new;  // baseline * focal length
-  *T_mul_f = 0. - _t[idx];
+  *T_mul_f = 0. - _t[idx] * fc_new;
   cvConvert(&pp, _P2);
 
   alpha = MIN(alpha, 1.);
@@ -381,11 +381,11 @@ RectifyProcessor::RectifyProcessor(
       std::int32_t proc_period)
     : Processor(std::move(proc_period)),
       calib_model(CalibrationModel::UNKNOW) {
-  intr_left_ = intr_left;
-  intr_right_ = intr_right;
-  extr_ = extr;
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
-  NotifyImageParamsChanged();
+  InitParams(
+    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
+    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
+    *extr);
 }
 
 RectifyProcessor::~RectifyProcessor() {
@@ -396,11 +396,14 @@ std::string RectifyProcessor::Name() {
   return NAME;
 }
 
-void RectifyProcessor::NotifyImageParamsChanged() {
+void RectifyProcessor::ReloadImageParams(
+      std::shared_ptr<IntrinsicsBase> intr_left,
+      std::shared_ptr<IntrinsicsBase> intr_right,
+      std::shared_ptr<Extrinsics> extr) {
   InitParams(
-    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left_),
+    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
-    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right_),
+    *std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
-    *extr_);
+    *extr);
 }
 
 Object *RectifyProcessor::OnCreateOutput() {

src/mynteye/api/processor/rectify_processor.h

@@ -71,7 +71,10 @@ class RectifyProcessor : public Processor {
 
   std::string Name() override;
 
-  void NotifyImageParamsChanged();
+  void ReloadImageParams(
+      std::shared_ptr<IntrinsicsBase> intr_left,
+      std::shared_ptr<IntrinsicsBase> intr_right,
+      std::shared_ptr<Extrinsics> extr);
 
   cv::Mat R1, P1, R2, P2, Q;
   cv::Mat map11, map12, map21, map22;
@@ -117,9 +120,6 @@ class RectifyProcessor : public Processor {
   camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant(
       const mynteye::IntrinsicsEquidistant & in);
 
-  std::shared_ptr<IntrinsicsBase> intr_left_;
-  std::shared_ptr<IntrinsicsBase> intr_right_;
-  std::shared_ptr<Extrinsics> extr_;
   CalibrationModel calib_model;
   std::shared_ptr<struct camera_calib_info_pair> calib_infos;
 };

src/mynteye/api/processor/rectify_processor_ocv.cc

@@ -32,10 +32,10 @@ RectifyProcessorOCV::RectifyProcessorOCV(
     : Processor(std::move(proc_period)),
       calib_model(CalibrationModel::UNKNOW) {
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
-  intr_left_ = intr_left;
+  InitParams(
-  intr_right_ = intr_right;
+    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
-  extr_ = extr;
+    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
-  NotifyImageParamsChanged();
+    *extr);
 }
 
 RectifyProcessorOCV::~RectifyProcessorOCV() {
@@ -46,11 +46,14 @@ std::string RectifyProcessorOCV::Name() {
   return NAME;
 }
 
-void RectifyProcessorOCV::NotifyImageParamsChanged() {
+void RectifyProcessorOCV::ReloadImageParams(
+      std::shared_ptr<IntrinsicsBase> intr_left,
+      std::shared_ptr<IntrinsicsBase> intr_right,
+      std::shared_ptr<Extrinsics> extr) {
   InitParams(
-    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left_),
+    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_left),
-    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right_),
+    *std::dynamic_pointer_cast<IntrinsicsPinhole>(intr_right),
-    *extr_);
+    *extr);
 }
 
 Object *RectifyProcessorOCV::OnCreateOutput() {

src/mynteye/api/processor/rectify_processor_ocv.h

@@ -40,7 +40,10 @@ class RectifyProcessorOCV : public Processor {
 
   std::string Name() override;
 
-  void NotifyImageParamsChanged();
+  void ReloadImageParams(
+      std::shared_ptr<IntrinsicsBase> intr_left,
+      std::shared_ptr<IntrinsicsBase> intr_right,
+      std::shared_ptr<Extrinsics> extr);
 
   cv::Mat R1, P1, R2, P2, Q;
   cv::Mat map11, map12, map21, map22;
@@ -54,10 +57,6 @@ class RectifyProcessorOCV : public Processor {
   void InitParams(IntrinsicsPinhole in_left,
         IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
 
-  std::shared_ptr<IntrinsicsBase> intr_left_;
-  std::shared_ptr<IntrinsicsBase> intr_right_;
-  std::shared_ptr<Extrinsics> extr_;
-
   CalibrationModel calib_model;
 };
 

src/mynteye/api/synthetic.cc

@@ -111,19 +111,23 @@ Synthetic::~Synthetic() {
 }
 
 void Synthetic::NotifyImageParamsChanged() {
+    intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
+    intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT);
+    extr_ =  std::make_shared<Extrinsics>(
+        api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
   if (calib_model_ ==  CalibrationModel::PINHOLE) {
     auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
-    if (processor) processor->NotifyImageParamsChanged();
+    if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
 #ifdef WITH_CAM_MODELS
   } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
     auto &&processor = find_processor<RectifyProcessor>(processor_);
-    if (processor) processor->NotifyImageParamsChanged();
+    if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
 #endif
   } else {
     LOG(ERROR) << "Unknow calib model type in device: "
               << calib_model_ << ", use default pinhole model";
     auto &&processor = find_processor<RectifyProcessorOCV>(processor_);
-    if (processor) processor->NotifyImageParamsChanged();
+    if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_);
   }
 }
 

src/mynteye/device/channel/channels.cc

@@ -758,11 +758,14 @@ std::set<Resolution> ChannelsAdapter::GetResolutionSupports() {
   std::set<Resolution> res;
   auto requests_map = stream_requests_map.at(model_);
   for (auto&& r_map : requests_map) {
-    if (r_map.first == Capabilities::STEREO ||
+    if (r_map.first == Capabilities::STEREO) {
-        r_map.first == Capabilities::STEREO_COLOR) {
       for (auto&& r : r_map.second) {
         res.insert({r.width, r.height});
       }
+    } else if (r_map.first == Capabilities::STEREO_COLOR) {
+      for (auto&& r : r_map.second) {
+        res.insert({static_cast<std::uint16_t>(r.width / 2), r.height});
+      }
     }
   }
   return res;

src/mynteye/device/device.cc

@@ -640,7 +640,15 @@ void Device::UpdateStreamIntrinsics(
   for (auto &&params : all_img_params_) {
     auto &&img_res = params.first;
     auto &&img_params = params.second;
-    if (img_params.ok && img_res == request.GetResolution()) {
+    bool ok = false;
+    if (capability == Capabilities::STEREO_COLOR) {
+      ok = img_params.ok &&
+            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 (ok) {
       SetIntrinsics(Stream::LEFT, img_params.in_left);
       SetIntrinsics(Stream::RIGHT, img_params.in_right);
       SetExtrinsics(Stream::LEFT, Stream::RIGHT, img_params.ex_right_to_left);

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

@@ -254,6 +254,22 @@
       <!-- stream request index -->
       <param name="request_index" value="$(arg request_index)" />
 
+      <!-- device options of standard-->
+
+      <param name="sstandard/gain" value="$(arg standard/gain)" />
+      <param name="standard/brightness" value="$(arg standard/brightness)" />
+      <param name="standard/contrast" value="$(arg standard/contrast)" />
+      <param name="standard/frame_rate" value="$(arg standard/frame_rate)" />
+      <param name="standard/imu_frequency" value="$(arg standard/imu_frequency)" />
+      <param name="standard/exposure_mode" value="$(arg standard/exposure_mode)" />
+      <param name="standard/max_gain" value="$(arg standard/max_gain)" />
+      <param name="standard/max_exposure_time" value="$(arg standard/max_exposure_time)" />
+      <param name="standard/desired_brightness" value="$(arg standard/desired_brightness)" />
+      <param name="standard/ir_control" value="$(arg standard/ir_control)" />
+      <param name="standard/hdr_mode" value="$(arg standard/hdr_mode)" />
+      <param name="standard/accel_range" value="$(arg standard/accel_range)" />
+      <param name="standard/gyro_range" value="$(arg standard/gyro_range)" />
+
       <!-- device options of standard2-->
 
       <param name="standard2/brightness" value="$(arg standard2/brightness)" />
@@ -261,11 +277,26 @@
       <param name="standard2/max_gain" value="$(arg standard2/max_gain)" />
       <param name="standard2/max_exposure_time" value="$(arg standard2/max_exposure_time)" />
       <param name="standard2/desired_brightness" value="$(arg standard2/desired_brightness)" />
-      <param name="standard2/min_exposure_time" value="$(arg standard2/max_exposure_time)" />
+      <param name="standard2/min_exposure_time" value="$(arg standard2/min_exposure_time)" />
+      <param name="standard2/ir_control" value="$(arg standard2/ir_control)" />
       <param name="standard2/accel_range" value="$(arg standard2/accel_range)" />
       <param name="standard2/gyro_range" value="$(arg standard2/gyro_range)" />
       <param name="standard2/accel_low_filter" value="$(arg standard2/accel_low_filter)" />
       <param name="standard2/gyro_low_filter" value="$(arg standard2/gyro_low_filter)" />
+
+      <!-- device options of standard210a-->
+
+      <param name="standard210a/brightness" value="$(arg standard210a/brightness)" />
+      <param name="standard210a/exposure_mode" value="$(arg standard210a/exposure_mode)" />
+      <param name="standard210a/max_gain" value="$(arg standard210a/max_gain)" />
+      <param name="standard210a/max_exposure_time" value="$(arg standard210a/max_exposure_time)" />
+      <param name="standard210a/desired_brightness" value="$(arg standard210a/desired_brightness)" />
+      <param name="standard210a/min_exposure_time" value="$(arg standard210a/min_exposure_time)" />
+      <param name="standard210a/accel_range" value="$(arg standard210a/accel_range)" />
+      <param name="standard210a/gyro_range" value="$(arg standard210a/gyro_range)" />
+      <param name="standard210a/accel_low_filter" value="$(arg standard210a/accel_low_filter)" />
+      <param name="standard210a/gyro_low_filter" value="$(arg standard210a/gyro_low_filter)" />
+
     </node>
 
     <!-- disable compressed depth plugin for image topics -->

wrappers/ros/src/mynt_eye_ros_wrapper/rviz/mynteye.rviz

@@ -82,7 +82,7 @@ Visualization Manager:
           Displays:
             - Class: rviz/Image
               Enabled: true
-              Image Topic: /mynteye/left/image_rect
+              Image Topic: /mynteye/left_rect/image_rect
               Max Value: 1
               Median window: 5
               Min Value: 0
@@ -94,7 +94,7 @@ Visualization Manager:
               Value: true
             - Class: rviz/Image
               Enabled: true
-              Image Topic: /mynteye/right/image_rect
+              Image Topic: /mynteye/right_rect/image_rect
               Max Value: 1
               Median window: 5
               Min Value: 0