merge develop

src/mynteye/api/processor/depth_processor.cc

@@ -0,0 +1,54 @@
+// 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/processor/depth_processor.h"
+
+#include <utility>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char DepthProcessor::NAME[] = "DepthProcessor";
+
+DepthProcessor::DepthProcessor(std::int32_t proc_period)
+    : Processor(std::move(proc_period)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+}
+
+DepthProcessor::~DepthProcessor() {
+  VLOG(2) << __func__;
+}
+
+std::string DepthProcessor::Name() {
+  return NAME;
+}
+
+Object *DepthProcessor::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool DepthProcessor::OnProcess(
+    Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+  const ObjMat *input = Object::Cast<ObjMat>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+  cv::Mat channels[3 /*input->value.channels()*/];
+  cv::split(input->value, channels);
+  channels[2].convertTo(output->value, CV_16UC1);
+  output->id = input->id;
+  output->data = input->data;
+  return true;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/depth_processor.h

@@ -0,0 +1,41 @@
+// 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_PROCESSOR_DEPTH_PROCESSOR_H_
+#define MYNTEYE_API_PROCESSOR_DEPTH_PROCESSOR_H_
+#pragma once
+
+#include <string>
+
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class DepthProcessor : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit DepthProcessor(std::int32_t proc_period = 0);
+  virtual ~DepthProcessor();
+
+  std::string Name() override;
+
+ protected:
+  Object *OnCreateOutput() override;
+  bool OnProcess(
+      Object *const in, Object *const out, Processor *const parent) override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_PROCESSOR_DEPTH_PROCESSOR_H_

src/mynteye/api/processor/disparity_normalized_processor.cc

@@ -0,0 +1,57 @@
+// 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/processor/disparity_normalized_processor.h"
+
+#include <utility>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char DisparityNormalizedProcessor::NAME[] =
+    "DisparityNormalizedProcessor";
+
+DisparityNormalizedProcessor::DisparityNormalizedProcessor(
+    std::int32_t proc_period)
+    : Processor(std::move(proc_period)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+}
+
+DisparityNormalizedProcessor::~DisparityNormalizedProcessor() {
+  VLOG(2) << __func__;
+}
+
+std::string DisparityNormalizedProcessor::Name() {
+  return NAME;
+}
+
+Object *DisparityNormalizedProcessor::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool DisparityNormalizedProcessor::OnProcess(
+    Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+  const ObjMat *input = Object::Cast<ObjMat>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+  cv::normalize(input->value, output->value, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+  // cv::normalize maybe return empty ==
+  output->id = input->id;
+  output->data = input->data;
+  return !output->value.empty();
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/disparity_normalized_processor.h

@@ -0,0 +1,41 @@
+// 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_PROCESSOR_DISPARITY_NORMALIZED_PROCESSOR_H_
+#define MYNTEYE_API_PROCESSOR_DISPARITY_NORMALIZED_PROCESSOR_H_
+#pragma once
+
+#include <string>
+
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class DisparityNormalizedProcessor : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit DisparityNormalizedProcessor(std::int32_t proc_period = 0);
+  virtual ~DisparityNormalizedProcessor();
+
+  std::string Name() override;
+
+ protected:
+  Object *OnCreateOutput() override;
+  bool OnProcess(
+      Object *const in, Object *const out, Processor *const parent) override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_PROCESSOR_DISPARITY_NORMALIZED_PROCESSOR_H_

src/mynteye/api/processor/disparity_processor.cc

@@ -0,0 +1,89 @@
+// 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/processor/disparity_processor.h"
+
+#include <utility>
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char DisparityProcessor::NAME[] = "DisparityProcessor";
+
+DisparityProcessor::DisparityProcessor(std::int32_t proc_period)
+    : Processor(std::move(proc_period)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+
+	int blockSize_ = 15;           // 15
+	int numDisparities_ = 64;      // 64
+
+#ifdef WITH_OPENCV2
+	bm_ = cv::Ptr<cv::StereoBM>(
+			new cv::StereoBM(
+				cv::StereoBM::BASIC_PRESET,
+				numDisparities_,
+				blockSize_));
+#else
+	int minDisparity_ = 0;         // 0
+	int preFilterSize_ = 9;        // 9
+	int preFilterCap_ = 31;        // 31
+	int uniquenessRatio_ = 15;     // 15
+	int textureThreshold_ = 10;    // 10
+	int speckleWindowSize_ = 100;  // 100
+	int speckleRange_ = 4;         // 4
+
+	bm_ = cv::StereoBM::create(16, 9);
+	bm_->setBlockSize(blockSize_);
+	bm_->setMinDisparity(minDisparity_);
+	bm_->setNumDisparities(numDisparities_);
+	bm_->setPreFilterSize(preFilterSize_);
+	bm_->setPreFilterCap(preFilterCap_);
+	bm_->setUniquenessRatio(uniquenessRatio_);
+	bm_->setTextureThreshold(textureThreshold_);
+	bm_->setSpeckleWindowSize(speckleWindowSize_);
+	bm_->setSpeckleRange(speckleRange_);
+#endif
+}
+
+DisparityProcessor::~DisparityProcessor() {
+  VLOG(2) << __func__;
+}
+
+std::string DisparityProcessor::Name() {
+  return NAME;
+}
+
+Object *DisparityProcessor::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool DisparityProcessor::OnProcess(
+    Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+  const ObjMat2 *input = Object::Cast<ObjMat2>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+
+  cv::Mat disparity;
+#ifdef WITH_OPENCV2
+	(*bm_)(input->first, input->second, disparity);
+#else
+  bm_->compute(input->first, input->second, disparity);
+#endif
+	disparity.convertTo(output->value, CV_32F, 1./16);
+  return true;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/disparity_processor.h

@@ -0,0 +1,52 @@
+// 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_PROCESSOR_DISPARITY_PROCESSOR_H_
+#define MYNTEYE_API_PROCESSOR_DISPARITY_PROCESSOR_H_
+#pragma once
+
+#include <string>
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "mynteye/api/processor.h"
+
+namespace cv {
+
+class StereoBM;
+
+}  // namespace cv
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class DisparityProcessor : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit DisparityProcessor(std::int32_t proc_period = 0);
+  virtual ~DisparityProcessor();
+
+  std::string Name() override;
+
+ protected:
+  Object *OnCreateOutput() override;
+  bool OnProcess(
+      Object *const in, Object *const out, Processor *const parent) override;
+
+ private:
+  cv::Ptr<cv::StereoBM> bm_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_PROCESSOR_DISPARITY_PROCESSOR_H_

src/mynteye/api/processor/points_processor.cc

@@ -0,0 +1,83 @@
+// 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/processor/points_processor.h"
+
+#include <utility>
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char PointsProcessor::NAME[] = "PointsProcessor";
+
+PointsProcessor::PointsProcessor(cv::Mat Q, std::int32_t proc_period)
+    : Processor(std::move(proc_period)), Q_(std::move(Q)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+}
+
+PointsProcessor::~PointsProcessor() {
+  VLOG(2) << __func__;
+}
+
+std::string PointsProcessor::Name() {
+  return NAME;
+}
+
+Object *PointsProcessor::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool PointsProcessor::OnProcess(
+    Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+  const ObjMat *input = Object::Cast<ObjMat>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+
+  cv::Mat disparity = input->value;
+  output->value.create(disparity.size(), CV_MAKETYPE(CV_32FC3, 3));
+  cv::Mat _3dImage = output->value;
+
+  const float bigZ = 10000.f;
+  cv::Matx44d Q;
+  Q_.convertTo(Q, CV_64F);
+
+  int x, cols = disparity.cols;
+  CV_Assert(cols >= 0);
+
+  double minDisparity = FLT_MAX;
+
+  cv::minMaxIdx(disparity, &minDisparity, 0, 0, 0);
+
+  for (int y = 0; y < disparity.rows; y++) {
+    float *sptr = disparity.ptr<float>(y);
+    cv::Vec3f *dptr = _3dImage.ptr<cv::Vec3f>(y);
+
+    for (x = 0; x < cols; x++) {
+      double d = sptr[x];
+      cv::Vec4d homg_pt = Q * cv::Vec4d(x, y, d, 1.0);
+      dptr[x] = cv::Vec3d(homg_pt.val);
+      dptr[x] /= homg_pt[3];
+
+      if (fabs(d - minDisparity) <= FLT_EPSILON) {
+        dptr[x][2] = bigZ;
+      }
+    }
+  }
+
+  return true;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/points_processor.h

@@ -0,0 +1,46 @@
+// 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_PROCESSOR_POINTS_PROCESSOR_H_
+#define MYNTEYE_API_PROCESSOR_POINTS_PROCESSOR_H_
+#pragma once
+
+#include <string>
+
+#include <opencv2/core/core.hpp>
+
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class PointsProcessor : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit PointsProcessor(cv::Mat Q, std::int32_t proc_period = 0);
+  virtual ~PointsProcessor();
+
+  std::string Name() override;
+
+ protected:
+  Object *OnCreateOutput() override;
+  bool OnProcess(
+      Object *const in, Object *const out, Processor *const parent) override;
+
+ private:
+  cv::Mat Q_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_PROCESSOR_POINTS_PROCESSOR_H_

src/mynteye/api/processor/rectify_processor.cc

@@ -0,0 +1,104 @@
+// 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/processor/rectify_processor.h"
+
+#include <utility>
+
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include "mynteye/logger.h"
+#include "mynteye/device/device.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char RectifyProcessor::NAME[] = "RectifyProcessor";
+
+RectifyProcessor::RectifyProcessor(
+    std::shared_ptr<Device> device, std::int32_t proc_period)
+    : Processor(std::move(proc_period)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+  InitParams(
+      device->GetIntrinsics(Stream::LEFT), device->GetIntrinsics(Stream::RIGHT),
+      device->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
+}
+
+RectifyProcessor::~RectifyProcessor() {
+  VLOG(2) << __func__;
+}
+
+std::string RectifyProcessor::Name() {
+  return NAME;
+}
+
+Object *RectifyProcessor::OnCreateOutput() {
+  return new ObjMat2();
+}
+
+bool RectifyProcessor::OnProcess(
+    Object *const in, Object *const out, Processor *const parent) {
+  MYNTEYE_UNUSED(parent)
+  const ObjMat2 *input = Object::Cast<ObjMat2>(in);
+  ObjMat2 *output = Object::Cast<ObjMat2>(out);
+  cv::remap(input->first, output->first, map11, map12, cv::INTER_LINEAR);
+  cv::remap(input->second, output->second, map21, map22, cv::INTER_LINEAR);
+  output->first_id = input->first_id;
+  output->first_data = input->first_data;
+  output->second_id = input->second_id;
+  output->second_data = input->second_data;
+  return true;
+}
+
+void RectifyProcessor::InitParams(
+    Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left) {
+  cv::Size size{in_left.width, in_left.height};
+
+  cv::Mat M1 =
+      (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy,
+       in_left.cy, 0, 0, 1);
+  cv::Mat M2 =
+      (cv::Mat_<double>(3, 3) << in_right.fx, 0, in_right.cx, 0, in_right.fy,
+       in_right.cy, 0, 0, 1);
+  cv::Mat D1(1, 5, CV_64F, in_left.coeffs);
+  cv::Mat D2(1, 5, CV_64F, in_right.coeffs);
+	/*
+  cv::Mat R =
+      (cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0],
+       ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2],
+       ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1],
+       ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0],
+       ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
+			 */
+	cv::Mat R =
+      (cv::Mat_<double>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+  cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
+
+  VLOG(2) << "InitParams size: " << size;
+  VLOG(2) << "M1: " << M1;
+  VLOG(2) << "M2: " << M2;
+  VLOG(2) << "D1: " << D1;
+  VLOG(2) << "D2: " << D2;
+  VLOG(2) << "R: " << R;
+  VLOG(2) << "T: " << T;
+
+  cv::Rect left_roi, right_roi;
+  cv::stereoRectify(
+      M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY,
+      0, size, &left_roi, &right_roi);
+
+  cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12);
+  cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/rectify_processor.h

@@ -0,0 +1,55 @@
+// 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_PROCESSOR_RECTIFY_PROCESSOR_H_
+#define MYNTEYE_API_PROCESSOR_RECTIFY_PROCESSOR_H_
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <opencv2/core/core.hpp>
+
+#include "mynteye/types.h"
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Device;
+
+class RectifyProcessor : public Processor {
+ public:
+  static const char NAME[];
+
+  RectifyProcessor(
+      std::shared_ptr<Device> device, std::int32_t proc_period = 0);
+  virtual ~RectifyProcessor();
+
+  std::string Name() override;
+
+  cv::Mat R1, P1, R2, P2, Q;
+  cv::Mat map11, map12, map21, map22;
+
+ protected:
+  Object *OnCreateOutput() override;
+  bool OnProcess(
+      Object *const in, Object *const out, Processor *const parent) override;
+
+ private:
+  void InitParams(
+      Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left);
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_PROCESSOR_RECTIFY_PROCESSOR_H_