From 3978264c50cede9659252c366c1913a499e3415e Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 9 Jan 2019 10:53:05 +0800 Subject: [PATCH] refactor(calib models): add new calib model processer --- CMakeLists.txt | 2 + .../api/processor/depth_processor_ocv.cc | 54 ++++++ .../api/processor/depth_processor_ocv.h | 41 +++++ .../api/processor/points_processor_ocv.cc | 54 ++++++ .../api/processor/points_processor_ocv.h | 46 +++++ src/mynteye/api/synthetic.cc | 166 +++++++++++++++--- 6 files changed, 336 insertions(+), 27 deletions(-) create mode 100644 src/mynteye/api/processor/depth_processor_ocv.cc create mode 100644 src/mynteye/api/processor/depth_processor_ocv.h create mode 100644 src/mynteye/api/processor/points_processor_ocv.cc create mode 100644 src/mynteye/api/processor/points_processor_ocv.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e9a8135..724d1fa 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/mynteye/api/processor/depth_processor_ocv.cc b/src/mynteye/api/processor/depth_processor_ocv.cc new file mode 100644 index 0000000..8bdb6a9 --- /dev/null +++ b/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 + +#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(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); + output->id = input->id; + output->data = input->data; + return true; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/depth_processor_ocv.h b/src/mynteye/api/processor/depth_processor_ocv.h new file mode 100644 index 0000000..bd503a2 --- /dev/null +++ b/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 + +#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_ diff --git a/src/mynteye/api/processor/points_processor_ocv.cc b/src/mynteye/api/processor/points_processor_ocv.cc new file mode 100644 index 0000000..d5aeda0 --- /dev/null +++ b/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 + +#include + +#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(in); + ObjMat *output = Object::Cast(out); + cv::reprojectImageTo3D(input->value, output->value, Q_, true); + output->id = input->id; + output->data = input->data; + return true; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/points_processor_ocv.h b/src/mynteye/api/processor/points_processor_ocv.h new file mode 100644 index 0000000..cba2ee9 --- /dev/null +++ b/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 + +#include + +#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_ diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index bb3504b..a951511 100644 --- a/src/mynteye/api/synthetic.cc +++ b/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,22 +211,50 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { VLOG(2) << "Disparity normalized not ready now"; } break; case Stream::POINTS: { - auto &&processor = find_processor(processor_); - auto &&out = processor->GetOutput(); - if (out != nullptr) { - auto &&output = Object::Cast(out); - return {output->data, output->value, nullptr, output->id}; + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&processor = find_processor(processor_); + auto &&out = processor->GetOutput(); + if (out != nullptr) { + auto &&output = Object::Cast(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(processor_); + auto &&out = processor->GetOutput(); + if (out != nullptr) { + auto &&output = Object::Cast(out); + 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_; } - VLOG(2) << "Points not ready now"; } break; case Stream::DEPTH: { - auto &&processor = find_processor(processor_); - auto &&out = processor->GetOutput(); - if (out != nullptr) { - auto &&output = Object::Cast(out); - return {output->data, output->value, nullptr, output->id}; + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&processor = find_processor(processor_); + auto &&out = processor->GetOutput(); + if (out != nullptr) { + auto &&output = Object::Cast(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(processor_); + auto &&out = processor->GetOutput(); + if (out != nullptr) { + auto &&output = Object::Cast(out); + 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_; } - VLOG(2) << "Depth not ready now"; } 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); - CHECK(ActivateProcessor()); + if (calib_model_ == CalibrationModel::PINHOLE) { + CHECK(ActivateProcessor()); + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + CHECK(ActivateProcessor()); + } 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); - CHECK(ActivateProcessor()); + if (calib_model_ == CalibrationModel::PINHOLE) { + CHECK(ActivateProcessor()); + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + CHECK(ActivateProcessor()); + } 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(); } break; case Stream::POINTS: { - if (IsStreamEnabledSynthetic(Stream::DEPTH)) { - DisableStreamData(Stream::DEPTH, depth + 1); + if (calib_model_ == CalibrationModel::PINHOLE) { + if (IsStreamEnabledSynthetic(Stream::DEPTH)) { + DisableStreamData(Stream::DEPTH, depth + 1); + } + DeactivateProcessor(); + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + DeactivateProcessor(); + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_; } - DeactivateProcessor(); } break; case Stream::DEPTH: { - DeactivateProcessor(); + if (calib_model_ == CalibrationModel::PINHOLE) { + DeactivateProcessor(); + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + if (IsStreamEnabledSynthetic(Stream::DEPTH)) { + DisableStreamData(Stream::POINTS, depth + 1); + } + DeactivateProcessor(); + } 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( DISPARITY_NORM_PROC_PERIOD); - auto &&points_processor = std::make_shared( - rectify_processor->Q, POINTS_PROC_PERIOD); - auto &&depth_processor = std::make_shared(DEPTH_PROC_PERIOD); + std::shared_ptr points_processor = nullptr; + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&points_processor_pin = std::make_shared( + rectify_processor->Q, POINTS_PROC_PERIOD); + points_processor = points_processor_pin; + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + auto &&points_processor_kan = std::make_shared( + 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( + rectify_processor->Q, POINTS_PROC_PERIOD); + points_processor = points_processor_pin; + } + std::shared_ptr depth_processor = nullptr; + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&depth_processor_pin = + std::make_shared(DEPTH_PROC_PERIOD); + depth_processor = depth_processor_pin; + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + auto &&depth_processor_kan = + std::make_shared(DEPTH_PROC_PERIOD); + depth_processor = depth_processor_kan; + } else { + auto &&depth_processor_pin = + std::make_shared(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); - points_processor->AddChild(depth_processor); + disparity_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: { - process_childs(processor_, PointsProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + 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: { - process_childs(processor_, DepthProcessor::NAME, - ObjMat{data.frame, data.frame_id, data.img}); + 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;