From 6ee7539b8e738175201fa58a2aab194ee922d38c Mon Sep 17 00:00:00 2001 From: TinyOh Date: Wed, 9 Jan 2019 16:12:43 +0800 Subject: [PATCH] refactor(calib models): add default calib model(pinhole) --- src/mynteye/api/api.cc | 34 ++++++++--- src/mynteye/api/config.cc | 61 +++++++++++++++++++ src/mynteye/api/config.h | 28 +++++++++ .../api/processor/rectify_processor.cc | 20 +++--- src/mynteye/api/processor/rectify_processor.h | 9 ++- .../api/processor/rectify_processor_ocv.cc | 20 +++--- .../api/processor/rectify_processor_ocv.h | 10 ++- src/mynteye/api/synthetic.cc | 30 +++++++-- src/mynteye/api/synthetic.h | 7 ++- 9 files changed, 187 insertions(+), 32 deletions(-) create mode 100644 src/mynteye/api/config.cc create mode 100644 src/mynteye/api/config.h diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index d7471e5..106a93a 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -231,14 +231,34 @@ std::shared_ptr API::Create( } std::shared_ptr API::Create(const std::shared_ptr &device) { - auto left_intr = device -> GetIntrinsics(Stream::LEFT); - auto right_intr = device -> GetIntrinsics(Stream::RIGHT); - if (left_intr->calib_model() != right_intr->calib_model()) { - VLOG(2) << __func__ - << "ERROR: " - <<"left camera and right camera use different calib models!"; + std::shared_ptr api = nullptr; + if (device != nullptr) { + bool in_l_ok, in_r_ok; + auto left_intr = device->GetIntrinsics(Stream::LEFT, &in_l_ok); + auto right_intr = device->GetIntrinsics(Stream::RIGHT, &in_r_ok); + if (!in_l_ok || !in_r_ok) { + LOG(ERROR) << "Image params not found, but we need it to process the " + "images. Please `make tools` and use `img_params_writer` " + "to write the image params. If you update the SDK from " + "1.x, the `SN*.conf` is the file contains them. Besides, " + "you could also calibrate them by yourself. Read the guide " + "doc (https://github.com/slightech/MYNT-EYE-SDK-2-Guide) " + "to learn more."; + LOG(WARNING) << "use pinhole as default"; + api = std::make_shared(device, CalibrationModel::UNKNOW); + } else { + if (left_intr->calib_model() != right_intr->calib_model()) { + LOG(ERROR) << "left camera and right camera use different calib models!"; + LOG(WARNING) << "use pinhole as default"; + api = std::make_shared(device, CalibrationModel::UNKNOW); + } else { + api = std::make_shared(device, left_intr->calib_model()); + } + } + } else { + LOG(ERROR) <<"no device!"; + api = std::make_shared(device, CalibrationModel::UNKNOW); } - auto api = std::make_shared(device, left_intr->calib_model()); return api; } diff --git a/src/mynteye/api/config.cc b/src/mynteye/api/config.cc new file mode 100644 index 0000000..e39af95 --- /dev/null +++ b/src/mynteye/api/config.cc @@ -0,0 +1,61 @@ +// 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/config.h" + +MYNTEYE_BEGIN_NAMESPACE +/** + * default intrinsics + */ + +std::shared_ptr getDefaultIntrinsics() { + auto res = std::make_shared(); + res->fx = 3.6220059643202876e+02; + res->fy = 3.6350065250745848e+02; + res->cx = 4.0658699068023441e+02; + res->cy = 2.3435161110061483e+02; + double codffs[5] = { + -2.5034765682756088e-01, + 5.0579399202897619e-02, + -7.0536676161976066e-04, + -8.5255451307033846e-03, + 0. + }; + for (unsigned int i = 0; i < 5; i++) { + res->coeffs[i] = codffs[i]; + } + return res; +} + +std::shared_ptr getDefaultExtrinsics() { + auto res = std::make_shared(); + double rotation[9] = { + 9.9867908939669447e-01, -6.3445566137485428e-03, 5.0988459509619687e-02, + 5.9890316389333252e-03, 9.9995670037792639e-01, 7.1224201868366971e-03, + -5.1031440326695092e-02, -6.8076406092671274e-03, 9.9867384471984544e-01 + }; + double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00, + -5.2058205159996538e+00}; + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + res->rotation[i][j] = rotation[i*3 + j]; + } + } + for (unsigned int i = 0; i < 3; i++) { + res->translation[i] = translation[i]; + } + return res; +} + +MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/config.h b/src/mynteye/api/config.h new file mode 100644 index 0000000..8376cdd --- /dev/null +++ b/src/mynteye/api/config.h @@ -0,0 +1,28 @@ +// 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_CONFIG_H_ +#define MYNTEYE_API_CONFIG_H_ +#pragma once + +#include "mynteye/api/api.h" + +MYNTEYE_BEGIN_NAMESPACE + +std::shared_ptr getDefaultIntrinsics(); + +std::shared_ptr getDefaultExtrinsics(); + +MYNTEYE_END_NAMESPACE + +#endif // MYNTEYE_API_CONFIG_H_ diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index f4556de..74c5aea 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -403,10 +403,16 @@ void RectifyProcessor::InitParams( const char RectifyProcessor::NAME[] = "RectifyProcessor"; RectifyProcessor::RectifyProcessor( - std::shared_ptr device, std::int32_t proc_period) - : Processor(std::move(proc_period)), device_(device) { + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr, + std::int32_t proc_period) + : Processor(std::move(proc_period)), + calib_model(CalibrationModel::UNKNOW) { + intr_left_ = intr_left; + intr_right_ = intr_right; + extr_ = extr; VLOG(2) << __func__ << ": proc_period=" << proc_period; - calib_model = CalibrationModel::UNKNOW; NotifyImageParamsChanged(); } @@ -419,12 +425,10 @@ std::string RectifyProcessor::Name() { } void RectifyProcessor::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)); + *std::dynamic_pointer_cast(intr_left_), + *std::dynamic_pointer_cast(intr_right_), + *extr_); } Object *RectifyProcessor::OnCreateOutput() { diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index c6744ed..1e2b054 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -32,7 +32,10 @@ class RectifyProcessor : public Processor { static const char NAME[]; RectifyProcessor( - std::shared_ptr device, std::int32_t proc_period = 0); + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr, + std::int32_t proc_period = 0); virtual ~RectifyProcessor(); std::string Name() override; @@ -51,7 +54,9 @@ class RectifyProcessor : public Processor { void InitParams(IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); - std::shared_ptr device_; + std::shared_ptr intr_left_; + std::shared_ptr intr_right_; + std::shared_ptr extr_; CalibrationModel calib_model; }; diff --git a/src/mynteye/api/processor/rectify_processor_ocv.cc b/src/mynteye/api/processor/rectify_processor_ocv.cc index bd94504..f662cd7 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.cc +++ b/src/mynteye/api/processor/rectify_processor_ocv.cc @@ -25,10 +25,16 @@ 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) { + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr, + std::int32_t proc_period) + : Processor(std::move(proc_period)), + calib_model(CalibrationModel::UNKNOW) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - calib_model = CalibrationModel::UNKNOW; + intr_left_ = intr_left; + intr_right_ = intr_right; + extr_ = extr; NotifyImageParamsChanged(); } @@ -41,12 +47,10 @@ std::string RectifyProcessorOCV::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)); + *std::dynamic_pointer_cast(intr_left_), + *std::dynamic_pointer_cast(intr_right_), + *extr_); } Object *RectifyProcessorOCV::OnCreateOutput() { diff --git a/src/mynteye/api/processor/rectify_processor_ocv.h b/src/mynteye/api/processor/rectify_processor_ocv.h index c5baee3..f9ea949 100644 --- a/src/mynteye/api/processor/rectify_processor_ocv.h +++ b/src/mynteye/api/processor/rectify_processor_ocv.h @@ -32,7 +32,10 @@ class RectifyProcessorOCV : public Processor { static const char NAME[]; RectifyProcessorOCV( - std::shared_ptr device, std::int32_t proc_period = 0); + std::shared_ptr intr_left, + std::shared_ptr intr_right, + std::shared_ptr extr, + std::int32_t proc_period = 0); virtual ~RectifyProcessorOCV(); std::string Name() override; @@ -51,7 +54,10 @@ class RectifyProcessorOCV : public Processor { void InitParams(IntrinsicsPinhole in_left, IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); - std::shared_ptr device_; + std::shared_ptr intr_left_; + std::shared_ptr intr_right_; + std::shared_ptr extr_; + CalibrationModel calib_model; }; diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 46a5601..1455e0f 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -73,10 +73,31 @@ void process_childs( } // namespace +void Synthetic::InitCalibInfo() { + if (calib_model_ == CalibrationModel::UNKNOW) { + calib_model_ = CalibrationModel::PINHOLE; + LOG(INFO) << "camera calib model: unknow"; + // use default + } else { + if (calib_model_ == CalibrationModel::PINHOLE) { + LOG(INFO) << "camera calib model: pinhole"; + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + LOG(INFO) << "camera calib model: kannala_brandt"; + } + intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); + intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); + extr_ = std::make_shared( + api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); + } +} + Synthetic::Synthetic(API *api, CalibrationModel calib_model) - : api_(api), plugin_(nullptr), calib_model_(calib_model) { + : api_(api), + plugin_(nullptr), + calib_model_(calib_model) { VLOG(2) << __func__; CHECK_NOTNULL(api_); + InitCalibInfo(); InitStreamSupports(); InitProcessors(); } @@ -552,20 +573,21 @@ void Synthetic::InitProcessors() { cv::Mat Q; if (calib_model_ == CalibrationModel::PINHOLE) { auto &&rectify_processor_ocv = - std::make_shared(api_->device(), + std::make_shared(intr_left_, intr_right_, extr_, RECTIFY_PROC_PERIOD); rectify_processor = rectify_processor_ocv; Q = rectify_processor_ocv->Q; #ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { rectify_processor = - std::make_shared(api_->device(), RECTIFY_PROC_PERIOD); + std::make_shared(intr_left_, intr_right_, extr_, + RECTIFY_PROC_PERIOD); #endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_ << ", use default pinhole model"; auto &&rectify_processor_ocv = - std::make_shared(api_->device(), + std::make_shared(intr_left_, intr_right_, extr_, RECTIFY_PROC_PERIOD); rectify_processor = rectify_processor_ocv; } diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index 7179d9a..eff452d 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -21,7 +21,7 @@ #include #include "mynteye/api/api.h" -// #include "" +#include "mynteye/api/config.h" MYNTEYE_BEGIN_NAMESPACE @@ -68,6 +68,7 @@ class Synthetic { bool HasPlugin() const; private: + void InitCalibInfo(); void InitStreamSupports(); mode_t GetStreamEnabledMode(const Stream &stream) const; @@ -115,6 +116,10 @@ class Synthetic { std::shared_ptr plugin_; CalibrationModel calib_model_; + + std::shared_ptr intr_left_; + std::shared_ptr intr_right_; + std::shared_ptr extr_; }; template