Done points and depth processor

samples/api/camera.cc

@@ -12,6 +12,10 @@ int main(int argc, char *argv[]) {
   glog_init _(argc, argv);
 
   auto &&api = API::Create();
+
+  // api->SetOptionValue(Option::FRAME_RATE, 25);
+  // api->SetOptionValue(Option::IMU_FREQUENCY, 500);
+  api->SetOptionValue(Option::IR_CONTROL, 80);
   api->LogOptionInfos();
 
   std::size_t left_count = 0;
@@ -51,15 +55,17 @@ int main(int argc, char *argv[]) {
             << ", temperature: " << data.imu->temperature;
   });
 
-  api->EnableStreamData(Stream::LEFT_RECTIFIED);
-  api->EnableStreamData(Stream::RIGHT_RECTIFIED);
+  // api->EnableStreamData(Stream::LEFT_RECTIFIED);
+  // api->EnableStreamData(Stream::RIGHT_RECTIFIED);
   api->EnableStreamData(Stream::DISPARITY_NORMALIZED);
+  api->EnableStreamData(Stream::DEPTH);
   // Enable this will cache the motion datas until you get them.
   api->EnableMotionDatas();
   api->Start(Source::ALL);
 
   cv::namedWindow("frame");
   cv::namedWindow("disparity");
+  cv::namedWindow("depth");
 
   std::size_t motion_count = 0;
   auto &&time_beg = times::now();
@@ -78,7 +84,12 @@ int main(int argc, char *argv[]) {
 
     auto &&disp_data = api->GetStreamData(Stream::DISPARITY_NORMALIZED);
     if (!disp_data.frame.empty()) {
-      cv::imshow("disparity", disp_data.frame);
+      cv::imshow("disparity", disp_data.frame);  // CV_8UC1
+    }
+
+    auto &&depth_data = api->GetStreamData(Stream::DEPTH);
+    if (!depth_data.frame.empty()) {
+      cv::imshow("depth", depth_data.frame);  // CV_16UC1
     }
 
     auto &&motion_datas = api->GetMotionDatas();

src/api/processor/depth_processor.cc

@@ -17,14 +17,17 @@ std::string DepthProcessor::Name() {
 }
 
 Object *DepthProcessor::OnCreateOutput() {
-  return nullptr;
+  return new ObjMat();
 }
 
 bool DepthProcessor::OnProcess(
     Object *const in, Object *const out, Processor *const parent) {
-  UNUSED(in)
-  UNUSED(out)
   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);
   return true;
 }
 

src/api/processor/points_processor.cc

@@ -1,10 +1,14 @@
 #include "api/processor/points_processor.h"
 
+#include <opencv2/calib3d/calib3d.hpp>
+
 #include <glog/logging.h>
 
+#include <utility>
+
 MYNTEYE_BEGIN_NAMESPACE
 
-PointsProcessor::PointsProcessor() : Processor() {
+PointsProcessor::PointsProcessor(cv::Mat Q) : Processor(), Q_(std::move(Q)) {
   VLOG(2) << __func__;
 }
 
@@ -17,14 +21,15 @@ std::string PointsProcessor::Name() {
 }
 
 Object *PointsProcessor::OnCreateOutput() {
-  return nullptr;
+  return new ObjMat();
 }
 
 bool PointsProcessor::OnProcess(
     Object *const in, Object *const out, Processor *const parent) {
-  UNUSED(in)
-  UNUSED(out)
   UNUSED(parent)
+  const ObjMat *input = Object::Cast<ObjMat>(in);
+  ObjMat *output = Object::Cast<ObjMat>(out);
+  cv::reprojectImageTo3D(input->value, output->value, Q_, true);
   return true;
 }
 

src/api/processor/points_processor.h

@@ -2,6 +2,8 @@
 #define MYNTEYE_POINTS_PROCESSOR_H_
 #pragma once
 
+#include <opencv2/core/core.hpp>
+
 #include <string>
 
 #include "api/processor/processor.h"
@@ -12,7 +14,7 @@ class PointsProcessor : public Processor {
  public:
   static constexpr auto &&NAME = "PointsProcessor";
 
-  PointsProcessor();
+  explicit PointsProcessor(cv::Mat Q);
   virtual ~PointsProcessor();
 
   std::string Name() override;
@@ -21,6 +23,9 @@ class PointsProcessor : public Processor {
   Object *OnCreateOutput() override;
   bool OnProcess(
       Object *const in, Object *const out, Processor *const parent) override;
+
+ private:
+  cv::Mat Q_;
 };
 
 MYNTEYE_END_NAMESPACE

src/api/processor/rectify_processor.h

@@ -23,6 +23,9 @@ class RectifyProcessor : public Processor {
 
   std::string Name() override;
 
+  cv::Mat R1, P1, R2, P2, Q;
+  cv::Mat map11, map12, map21, map22;
+
  protected:
   Object *OnCreateOutput() override;
   bool OnProcess(
@@ -31,9 +34,6 @@ class RectifyProcessor : public Processor {
  private:
   void InitParams(
       Intrinsics in_left, Intrinsics in_right, Extrinsics ex_left_to_right);
-
-  cv::Mat R1, P1, R2, P2, Q;
-  cv::Mat map11, map12, map21, map22;
 };
 
 MYNTEYE_END_NAMESPACE

src/api/synthetic.cc

@@ -367,7 +367,8 @@ void Synthetic::InitProcessors() {
   auto &&disparity_processor = std::make_shared<DisparityProcessor>();
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>();
-  auto &&points_processor = std::make_shared<PointsProcessor>();
+  auto &&points_processor =
+      std::make_shared<PointsProcessor>(rectify_processor->Q);
   auto &&depth_processor = std::make_shared<DepthProcessor>();
 
   using namespace std::placeholders;  // NOLINT