diff --git a/src/mynteye/api/processor/rectify_processor_ocv.cc b/src/mynteye/api/processor/rectify_processor_ocv.cc new file mode 100644 index 0000000..bd94504 --- /dev/null +++ b/src/mynteye/api/processor/rectify_processor_ocv.cc @@ -0,0 +1,110 @@ +// 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_ocv.h" + +#include + +#include +#include +#include "mynteye/logger.h" +#include "mynteye/device/device.h" + +MYNTEYE_BEGIN_NAMESPACE + +const char RectifyProcessorOCV::NAME[] = "RectifyProcessorOCV"; + +RectifyProcessorOCV::RectifyProcessorOCV( + std::shared_ptr device, std::int32_t proc_period) + : Processor(std::move(proc_period)), device_(device) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; + calib_model = CalibrationModel::UNKNOW; + NotifyImageParamsChanged(); +} + +RectifyProcessorOCV::~RectifyProcessorOCV() { + VLOG(2) << __func__; +} + +std::string RectifyProcessorOCV::Name() { + return NAME; +} + +void RectifyProcessorOCV::NotifyImageParamsChanged() { + auto in_left = device_->GetIntrinsics(Stream::LEFT); + auto in_right = device_->GetIntrinsics(Stream::RIGHT); + InitParams( + *std::dynamic_pointer_cast(in_left), + *std::dynamic_pointer_cast(in_right), + device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); +} + +Object *RectifyProcessorOCV::OnCreateOutput() { + return new ObjMat2(); +} + +bool RectifyProcessorOCV::OnProcess( + Object *const in, Object *const out, Processor *const parent) { + MYNTEYE_UNUSED(parent) + const ObjMat2 *input = Object::Cast(in); + ObjMat2 *output = Object::Cast(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 RectifyProcessorOCV::InitParams( + IntrinsicsPinhole in_left, + IntrinsicsPinhole in_right, + Extrinsics ex_right_to_left) { + calib_model = CalibrationModel::PINHOLE; + cv::Size size{in_left.width, in_left.height}; + + cv::Mat M1 = + (cv::Mat_(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy, + in_left.cy, 0, 0, 1); + cv::Mat M2 = + (cv::Mat_(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_(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 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 diff --git a/src/mynteye/api/processor/rectify_processor_ocv.h b/src/mynteye/api/processor/rectify_processor_ocv.h new file mode 100644 index 0000000..c5baee3 --- /dev/null +++ b/src/mynteye/api/processor/rectify_processor_ocv.h @@ -0,0 +1,60 @@ +// 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_OCV_H_ +#define MYNTEYE_API_PROCESSOR_RECTIFY_PROCESSOR_OCV_H_ +#pragma once + +#include +#include + +#include + +#include "mynteye/types.h" +#include "mynteye/api/processor.h" + +MYNTEYE_BEGIN_NAMESPACE + +class Device; + +class RectifyProcessorOCV : public Processor { + public: + static const char NAME[]; + + RectifyProcessorOCV( + std::shared_ptr device, std::int32_t proc_period = 0); + virtual ~RectifyProcessorOCV(); + + std::string Name() override; + + void NotifyImageParamsChanged(); + + 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(IntrinsicsPinhole in_left, + IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); + + std::shared_ptr device_; + CalibrationModel calib_model; +}; + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_API_PROCESSOR_RECTIFY_PROCESSOR_OCV_H_