refactor(calib models): add new calib model processer

CMakeLists.txt

@@ -223,6 +223,8 @@ if(WITH_API)
     src/mynteye/api/processor/disparity_normalized_processor.cc
     src/mynteye/api/processor/depth_processor.cc
     src/mynteye/api/processor/points_processor.cc
+    src/mynteye/api/processor/points_processor_ocv.cc
+    src/mynteye/api/processor/depth_processor_ocv.cc
   )
 endif()
 if(NOT WITH_GLOG)

src/mynteye/api/processor/depth_processor_ocv.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_ocv.h"
+
+#include <utility>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char DepthProcessorOCV::NAME[] = "DepthProcessorOCV";
+
+DepthProcessorOCV::DepthProcessorOCV(std::int32_t proc_period)
+    : Processor(std::move(proc_period)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+}
+
+DepthProcessorOCV::~DepthProcessorOCV() {
+  VLOG(2) << __func__;
+}
+
+std::string DepthProcessorOCV::Name() {
+  return NAME;
+}
+
+Object *DepthProcessorOCV::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool DepthProcessorOCV::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_ocv.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_OCV_H_
+#define MYNTEYE_API_PROCESSOR_DEPTH_PROCESSOR_OCV_H_
+#pragma once
+
+#include <string>
+
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class DepthProcessorOCV : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit DepthProcessorOCV(std::int32_t proc_period = 0);
+  virtual ~DepthProcessorOCV();
+
+  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_OCV_H_

src/mynteye/api/processor/points_processor_ocv.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/points_processor_ocv.h"
+
+#include <utility>
+
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const char PointsProcessorOCV::NAME[] = "PointsProcessorOCV";
+
+PointsProcessorOCV::PointsProcessorOCV(cv::Mat Q, std::int32_t proc_period)
+    : Processor(std::move(proc_period)), Q_(std::move(Q)) {
+  VLOG(2) << __func__ << ": proc_period=" << proc_period;
+}
+
+PointsProcessorOCV::~PointsProcessorOCV() {
+  VLOG(2) << __func__;
+}
+
+std::string PointsProcessorOCV::Name() {
+  return NAME;
+}
+
+Object *PointsProcessorOCV::OnCreateOutput() {
+  return new ObjMat();
+}
+
+bool PointsProcessorOCV::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::reprojectImageTo3D(input->value, output->value, Q_, true);
+  output->id = input->id;
+  output->data = input->data;
+  return true;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/processor/points_processor_ocv.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_OCV_H_
+#define MYNTEYE_API_PROCESSOR_POINTS_PROCESSOR_OCV_H_
+#pragma once
+
+#include <string>
+
+#include <opencv2/core/core.hpp>
+
+#include "mynteye/api/processor.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class PointsProcessorOCV : public Processor {
+ public:
+  static const char NAME[];
+
+  explicit PointsProcessorOCV(cv::Mat Q, std::int32_t proc_period = 0);
+  virtual ~PointsProcessorOCV();
+
+  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_OCV_H_

src/mynteye/api/synthetic.cc

@@ -28,6 +28,8 @@
 #include "mynteye/api/processor/disparity_processor.h"
 #include "mynteye/api/processor/points_processor.h"
 #include "mynteye/api/processor/rectify_processor.h"
+#include "mynteye/api/processor/depth_processor_ocv.h"
+#include "mynteye/api/processor/points_processor_ocv.h"
 #include "mynteye/device/device.h"
 
 #define RECTIFY_PROC_PERIOD 0
@@ -209,6 +211,15 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
         VLOG(2) << "Disparity normalized not ready now";
       } break;
       case Stream::POINTS: {
+        if (calib_model_ ==  CalibrationModel::PINHOLE) {
+          auto &&processor = find_processor<PointsProcessorOCV>(processor_);
+          auto &&out = processor->GetOutput();
+          if (out != nullptr) {
+            auto &&output = Object::Cast<ObjMat>(out);
+            return {output->data, output->value, nullptr, output->id};
+          }
+          VLOG(2) << "Points not ready now";
+        } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
           auto &&processor = find_processor<PointsProcessor>(processor_);
           auto &&out = processor->GetOutput();
           if (out != nullptr) {
@@ -216,8 +227,22 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
             return {output->data, output->value, nullptr, output->id};
           }
           VLOG(2) << "Points not ready now";
+        } else {
+          // UNKNOW
+          LOG(ERROR) << "Unknow calib model type in device: "
+                     << calib_model_;
+        }
       } break;
       case Stream::DEPTH: {
+        if (calib_model_ ==  CalibrationModel::PINHOLE) {
+          auto &&processor = find_processor<DepthProcessorOCV>(processor_);
+          auto &&out = processor->GetOutput();
+          if (out != nullptr) {
+            auto &&output = Object::Cast<ObjMat>(out);
+            return {output->data, output->value, nullptr, output->id};
+          }
+          VLOG(2) << "Depth not ready now";
+        } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
           auto &&processor = find_processor<DepthProcessor>(processor_);
           auto &&out = processor->GetOutput();
           if (out != nullptr) {
@@ -225,6 +250,11 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
             return {output->data, output->value, nullptr, output->id};
           }
           VLOG(2) << "Depth not ready now";
+        } else {
+          // UNKNOW
+          LOG(ERROR) << "Unknow calib model type in device: "
+                     << calib_model_;
+        }
       } break;
       default:
         break;
@@ -338,12 +368,26 @@ void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) {
     case Stream::POINTS: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::DISPARITY, depth + 1);
+      if (calib_model_ ==  CalibrationModel::PINHOLE) {
+        CHECK(ActivateProcessor<PointsProcessorOCV>());
+      } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
         CHECK(ActivateProcessor<PointsProcessor>());
+      } else {
+        LOG(ERROR) << "Unknow calib model type in device: "
+                   << calib_model_;
+      }
     } return;
     case Stream::DEPTH: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::POINTS, depth + 1);
+      if (calib_model_ ==  CalibrationModel::PINHOLE) {
+        CHECK(ActivateProcessor<DepthProcessorOCV>());
+      } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
         CHECK(ActivateProcessor<DepthProcessor>());
+      } else {
+        LOG(ERROR) << "Unknow calib model type in device: "
+                   << calib_model_;
+      }
     } return;
     default: break;
   }
@@ -390,13 +434,30 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
         DeactivateProcessor<DisparityNormalizedProcessor>();
       } break;
       case Stream::POINTS: {
+        if (calib_model_ ==  CalibrationModel::PINHOLE) {
           if (IsStreamEnabledSynthetic(Stream::DEPTH)) {
             DisableStreamData(Stream::DEPTH, depth + 1);
           }
+          DeactivateProcessor<PointsProcessorOCV>();
+        } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
           DeactivateProcessor<PointsProcessor>();
+        } else {
+          LOG(ERROR) << "Unknow calib model type in device: "
+                    << calib_model_;
+        }
       } break;
       case Stream::DEPTH: {
+        if (calib_model_ ==  CalibrationModel::PINHOLE) {
+          DeactivateProcessor<DepthProcessorOCV>();
+        } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+          if (IsStreamEnabledSynthetic(Stream::DEPTH)) {
+            DisableStreamData(Stream::POINTS, depth + 1);
+          }
           DeactivateProcessor<DepthProcessor>();
+        } else {
+          LOG(ERROR) << "Unknow calib model type in device: "
+                    << calib_model_;
+        }
       } break;
       default: return;
     }
@@ -416,9 +477,36 @@ void Synthetic::InitProcessors() {
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>(
           DISPARITY_NORM_PROC_PERIOD);
-  auto &&points_processor = std::make_shared<PointsProcessor>(
+  std::shared_ptr<Processor> points_processor = nullptr;
+  if (calib_model_ ==  CalibrationModel::PINHOLE) {
+    auto &&points_processor_pin = std::make_shared<PointsProcessorOCV>(
         rectify_processor->Q, POINTS_PROC_PERIOD);
-  auto &&depth_processor = std::make_shared<DepthProcessor>(DEPTH_PROC_PERIOD);
+    points_processor = points_processor_pin;
+  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+    auto &&points_processor_kan = std::make_shared<PointsProcessor>(
+        rectify_processor->Q, POINTS_PROC_PERIOD);
+    points_processor = points_processor_kan;
+  } else {
+    LOG(ERROR) << "Unknow calib model type in device: "
+              << calib_model_ << ", use default pinhole model";
+    auto &&points_processor_pin = std::make_shared<PointsProcessorOCV>(
+        rectify_processor->Q, POINTS_PROC_PERIOD);
+    points_processor = points_processor_pin;
+  }
+  std::shared_ptr<Processor> depth_processor = nullptr;
+  if (calib_model_ ==  CalibrationModel::PINHOLE) {
+    auto &&depth_processor_pin =
+        std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+    depth_processor = depth_processor_pin;
+  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+    auto &&depth_processor_kan =
+        std::make_shared<DepthProcessor>(DEPTH_PROC_PERIOD);
+    depth_processor = depth_processor_kan;
+  } else {
+    auto &&depth_processor_pin =
+        std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+    depth_processor = depth_processor_pin;
+  }
 
   using namespace std::placeholders;  // NOLINT
   rectify_processor->SetProcessCallback(
@@ -453,10 +541,12 @@ void Synthetic::InitProcessors() {
     // KANNALA_BRANDT
     rectify_processor->AddChild(disparity_processor);
     disparity_processor->AddChild(disparitynormalized_processor);
-    disparity_processor->AddChild(points_processor);
+    disparity_processor->AddChild(depth_processor);
-    points_processor->AddChild(depth_processor);
+    depth_processor->AddChild(points_processor);
   } else {
     // UNKNOW
+    LOG(ERROR) << "Unknow calib model type in device: "
+               << calib_model_;
   }
 
   processor_ = rectify_processor;
@@ -509,12 +599,34 @@ void Synthetic::ProcessNativeStream(
           ObjMat{data.frame, data.frame_id, data.img});
     } break;
     case Stream::POINTS: {
+      if (calib_model_ == CalibrationModel::PINHOLE) {
+        // PINHOLE
+        process_childs(processor_, PointsProcessorOCV::NAME,
+            ObjMat{data.frame, data.frame_id, data.img});
+      } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+        // KANNALA_BRANDT
         process_childs(processor_, PointsProcessor::NAME,
             ObjMat{data.frame, data.frame_id, data.img});
+      } else {
+        // UNKNOW
+        LOG(ERROR) << "Unknow calib model type in device: "
+                  << calib_model_;
+      }
     } break;
     case Stream::DEPTH: {
+      if (calib_model_ == CalibrationModel::PINHOLE) {
+        // PINHOLE
+        process_childs(processor_, DepthProcessorOCV::NAME,
+            ObjMat{data.frame, data.frame_id, data.img});
+      } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+        // KANNALA_BRANDT
         process_childs(processor_, DepthProcessor::NAME,
             ObjMat{data.frame, data.frame_id, data.img});
+      } else {
+        // UNKNOW
+        LOG(ERROR) << "Unknow calib model type in device: "
+                  << calib_model_;
+      }
     } break;
     default:
       break;