From 21f2f91343f4bfc56c453416b2dbfcf7151934f8 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sat, 28 Apr 2018 12:44:15 +0800 Subject: [PATCH] Done rectify processor --- samples/api/camera.cc | 11 ++++- src/api/processor/rectify_processor.cc | 56 ++++++++++++++++++++++++-- src/api/processor/rectify_processor.h | 15 ++++++- src/api/synthetic.cc | 2 +- 4 files changed, 76 insertions(+), 8 deletions(-) diff --git a/samples/api/camera.cc b/samples/api/camera.cc index 41ca81f..4ba3938 100644 --- a/samples/api/camera.cc +++ b/samples/api/camera.cc @@ -51,6 +51,8 @@ int main(int argc, char *argv[]) { << ", temperature: " << data.imu->temperature; }); + api->EnableStreamData(Stream::LEFT_RECTIFIED); + api->EnableStreamData(Stream::RIGHT_RECTIFIED); // Enable this will cache the motion datas until you get them. api->EnableMotionDatas(); api->Start(Source::ALL); @@ -62,8 +64,13 @@ int main(int argc, char *argv[]) { while (true) { api->WaitForStreams(); - auto &&left_data = api->GetStreamData(Stream::LEFT); - auto &&right_data = api->GetStreamData(Stream::RIGHT); + // auto &&left_data = api->GetStreamData(Stream::LEFT); + // auto &&right_data = api->GetStreamData(Stream::RIGHT); + auto &&left_data = api->GetStreamData(Stream::LEFT_RECTIFIED); + auto &&right_data = api->GetStreamData(Stream::RIGHT_RECTIFIED); + if (left_data.frame.empty() || right_data.frame.empty()) { + continue; + } auto &&motion_datas = api->GetMotionDatas(); motion_count += motion_datas.size(); diff --git a/src/api/processor/rectify_processor.cc b/src/api/processor/rectify_processor.cc index ae9fff4..4a9b34b 100644 --- a/src/api/processor/rectify_processor.cc +++ b/src/api/processor/rectify_processor.cc @@ -1,11 +1,20 @@ #include "api/processor/rectify_processor.h" +#include +#include + #include +#include "device/device.h" + MYNTEYE_BEGIN_NAMESPACE -RectifyProcessor::RectifyProcessor() : Processor() { +RectifyProcessor::RectifyProcessor(std::shared_ptr device) + : Processor() { VLOG(2) << __func__; + InitParams( + device->GetIntrinsics(Stream::LEFT), device->GetIntrinsics(Stream::RIGHT), + device->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); } RectifyProcessor::~RectifyProcessor() { @@ -17,14 +26,53 @@ std::string RectifyProcessor::Name() { } Object *RectifyProcessor::OnCreateOutput() { - return nullptr; + return new ObjMat2(); } void RectifyProcessor::OnProcess( Object *const in, Object *const out, Processor *const parent) { - UNUSED(in) - UNUSED(out) 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); +} + +void RectifyProcessor::InitParams( + Intrinsics in_left, Intrinsics in_right, Extrinsics ex_left_to_right) { + 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_left_to_right.rotation[0][0], + ex_left_to_right.rotation[0][1], ex_left_to_right.rotation[0][2], + ex_left_to_right.rotation[1][0], ex_left_to_right.rotation[1][1], + ex_left_to_right.rotation[1][2], ex_left_to_right.rotation[2][0], + ex_left_to_right.rotation[2][1], ex_left_to_right.rotation[2][2]); + cv::Mat T(3, 1, CV_64F, ex_left_to_right.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/api/processor/rectify_processor.h b/src/api/processor/rectify_processor.h index 2896477..c79a1c0 100644 --- a/src/api/processor/rectify_processor.h +++ b/src/api/processor/rectify_processor.h @@ -2,17 +2,23 @@ #define MYNTEYE_RECTIFY_PROCESSOR_H_ #pragma once +#include + +#include #include #include "api/processor/processor.h" +#include "mynteye/types.h" MYNTEYE_BEGIN_NAMESPACE +class Device; + class RectifyProcessor : public Processor { public: static constexpr auto &&NAME = "RectifyProcessor"; - RectifyProcessor(); + explicit RectifyProcessor(std::shared_ptr device); virtual ~RectifyProcessor(); std::string Name() override; @@ -21,6 +27,13 @@ class RectifyProcessor : public Processor { Object *OnCreateOutput() override; void OnProcess( Object *const in, Object *const out, Processor *const parent) override; + + 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 29fa25b..12c0bf9 100644 --- a/src/api/synthetic.cc +++ b/src/api/synthetic.cc @@ -363,7 +363,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { } void Synthetic::InitProcessors() { - auto &&rectify_processor = std::make_shared(); + auto &&rectify_processor = std::make_shared(api_->device()); auto &&disparity_processor = std::make_shared(); auto &&disparitynormalized_processor = std::make_shared();