From 2111a183d0ef05aac4555bb5e18d33f17b5f1531 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 28 Apr 2018 14:27:43 +0800 Subject: [PATCH] Done points and depth processor --- samples/api/camera.cc | 17 ++++++++++++++--- src/api/processor/depth_processor.cc | 9 ++++++--- src/api/processor/points_processor.cc | 13 +++++++++---- src/api/processor/points_processor.h | 7 ++++++- src/api/processor/rectify_processor.h | 6 +++--- src/api/synthetic.cc | 3 ++- 6 files changed, 40 insertions(+), 15 deletions(-) diff --git a/samples/api/camera.cc b/samples/api/camera.cc index 29261f2..085c0ce 100644 --- a/samples/api/camera.cc +++ b/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(); diff --git a/src/api/processor/depth_processor.cc b/src/api/processor/depth_processor.cc index 0031c70..92dd5a1 100644 --- a/src/api/processor/depth_processor.cc +++ b/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(in); + ObjMat *output = Object::Cast(out); + cv::Mat channels[3 /*input->value.channels()*/]; + cv::split(input->value, channels); + channels[2].convertTo(output->value, CV_16UC1); return true; } diff --git a/src/api/processor/points_processor.cc b/src/api/processor/points_processor.cc index a400339..ff003bd 100644 --- a/src/api/processor/points_processor.cc +++ b/src/api/processor/points_processor.cc @@ -1,10 +1,14 @@ #include "api/processor/points_processor.h" +#include + #include +#include + 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(in); + ObjMat *output = Object::Cast(out); + cv::reprojectImageTo3D(input->value, output->value, Q_, true); return true; } diff --git a/src/api/processor/points_processor.h b/src/api/processor/points_processor.h index e73f41c..4540288 100644 --- a/src/api/processor/points_processor.h +++ b/src/api/processor/points_processor.h @@ -2,6 +2,8 @@ #define MYNTEYE_POINTS_PROCESSOR_H_ #pragma once +#include + #include #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 diff --git a/src/api/processor/rectify_processor.h b/src/api/processor/rectify_processor.h index 987501a..725e4e5 100644 --- a/src/api/processor/rectify_processor.h +++ b/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 diff --git a/src/api/synthetic.cc b/src/api/synthetic.cc index 12c0bf9..f48e4f1 100644 --- a/src/api/synthetic.cc +++ b/src/api/synthetic.cc @@ -367,7 +367,8 @@ void Synthetic::InitProcessors() { auto &&disparity_processor = std::make_shared(); auto &&disparitynormalized_processor = std::make_shared(); - auto &&points_processor = std::make_shared(); + auto &&points_processor = + std::make_shared(rectify_processor->Q); auto &&depth_processor = std::make_shared(); using namespace std::placeholders; // NOLINT