diff --git a/CMakeLists.txt b/CMakeLists.txt index 724d1fa..65fc184 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -94,6 +94,18 @@ if(OS_WIN) ) endif() +# rpath + +set(CMAKE_MACOSX_RPATH 1) +set(MYNTEYE_CMAKE_RPATH "") +if(WITH_OPENCV) + list(APPEND MYNTEYE_CMAKE_RPATH ${OpenCV_LIB_PATH}) +endif() +if(MYNTEYE_CMAKE_RPATH) + message(STATUS "RPATH: ${MYNTEYE_CMAKE_RPATH}") + set(CMAKE_INSTALL_RPATH "${MYNTEYE_CMAKE_RPATH}") +endif() + # targets add_definitions(-DMYNTEYE_EXPORTS) @@ -218,13 +230,19 @@ if(WITH_API) src/mynteye/api/dl.cc src/mynteye/api/processor.cc src/mynteye/api/synthetic.cc - src/mynteye/api/processor/rectify_processor.cc src/mynteye/api/processor/disparity_processor.cc 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 + src/mynteye/api/processor/rectify_processor_ocv.cc + ) +endif() + +if(WITH_CAM_MODELS) + list(APPEND MYNTEYE_SRCS + src/mynteye/api/processor/depth_processor.cc + src/mynteye/api/processor/points_processor.cc + src/mynteye/api/processor/rectify_processor.cc ) endif() if(NOT WITH_GLOG) diff --git a/cmake/DetectOpenCV.cmake b/cmake/DetectOpenCV.cmake index 34a5779..d166f50 100644 --- a/cmake/DetectOpenCV.cmake +++ b/cmake/DetectOpenCV.cmake @@ -41,6 +41,25 @@ if(${__index} GREATER -1) set(WITH_OPENCV_WORLD TRUE) endif() +if(NOT OpenCV_LIB_PATH) + list(LENGTH OpenCV_INCLUDE_DIRS __length) + if(${__length} GREATER 0) + list(GET OpenCV_INCLUDE_DIRS 0 __include_dir) + string(REGEX REPLACE "include.*$" "lib" __lib_dir "${__include_dir}") + find_library(__opencv_lib + NAMES opencv_core3 opencv_core opencv_world + PATHS "${__lib_dir}" "${__lib_dir}/x86_64-linux-gnu" + NO_DEFAULT_PATH) + #message(STATUS "__opencv_lib: ${__opencv_lib}") + if(__opencv_lib) + get_filename_component(OpenCV_LIB_PATH "${__opencv_lib}" DIRECTORY) + else() + set(OpenCV_LIB_PATH "${__lib_dir}") + endif() + #message(STATUS "OpenCV_LIB_PATH: ${OpenCV_LIB_PATH}") + endif() +endif() + if(MSVC OR MSYS OR MINGW) get_filename_component(OpenCV_LIB_SEARCH_PATH "${OpenCV_LIB_PATH}/../bin" ABSOLUTE) else() diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index e9ceea6..e6e5746 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -41,13 +41,23 @@ Object *DepthProcessor::OnCreateOutput() { bool DepthProcessor::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; + const ObjMat *input = Object::Cast(in); + ObjMat *output = Object::Cast(out); + int rows = input->value.rows; + int cols = input->value.cols; + float T = 0.08; + float f = 0.01; + cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_32F); + for (int i = 0; i < rows; i++) { + for (int j = 0; j < cols; j++) { + float disparity_value = input->value.at(i, j); + float depth = T * f / disparity_value; + depth_mat.at(i, j) = depth; + } + } + output->value = depth_mat; + output->id = input->id; + output->data = input->data; return true; } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 585ed12..f4556de 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -19,11 +19,6 @@ #include #include "mynteye/logger.h" #include "mynteye/device/device.h" - -// #define WITH_CAM_MODELS - -#ifdef WITH_CAM_MODELS - #include #include #include @@ -344,130 +339,6 @@ struct camera_mat_info_pair stereoRectify( return res; } -#endif - -MYNTEYE_BEGIN_NAMESPACE - -const char RectifyProcessor::NAME[] = "RectifyProcessor"; - -RectifyProcessor::RectifyProcessor( - 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(); -} - -RectifyProcessor::~RectifyProcessor() { - VLOG(2) << __func__; -} - -std::string RectifyProcessor::Name() { - return NAME; -} - -void RectifyProcessor::NotifyImageParamsChanged() { - auto in_left = device_->GetIntrinsics(Stream::LEFT); - auto in_right = device_->GetIntrinsics(Stream::RIGHT); - if (in_left->calib_model() == CalibrationModel::PINHOLE) { - InitParams( - *std::dynamic_pointer_cast(in_left), - *std::dynamic_pointer_cast(in_right), - device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); - } else if (in_left->calib_model() == - CalibrationModel::KANNALA_BRANDT) { -#ifdef WITH_CAM_MODELS - InitParams( - *std::dynamic_pointer_cast(in_left), - *std::dynamic_pointer_cast(in_right), - device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); -#else - VLOG(2) << "calib model type KANNALA_BRANDT" - << " is not been enabled."; -#endif - } else { - VLOG(2) << "calib model type " - << in_left->calib_model() - <<" is not been enabled."; - } -} - -Object *RectifyProcessor::OnCreateOutput() { - return new ObjMat2(); -} - -bool RectifyProcessor::OnProcess( - Object *const in, Object *const out, Processor *const parent) { - MYNTEYE_UNUSED(parent) - if (calib_model == CalibrationModel::PINHOLE) { - 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; - } else if (calib_model == CalibrationModel::KANNALA_BRANDT) { -#ifdef WITH_CAM_MODELS - 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; -#else - return false; -#endif - } -} - -void RectifyProcessor::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); -} - -#ifdef WITH_CAM_MODELS - camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( const mynteye::IntrinsicsEquidistant & in) { camodocal::EquidistantCameraPtr camera( @@ -485,6 +356,8 @@ camodocal::CameraPtr generateCameraFromIntrinsicsEquidistant( return camera; } +MYNTEYE_BEGIN_NAMESPACE + void RectifyProcessor::InitParams( IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, @@ -526,6 +399,50 @@ void RectifyProcessor::InitParams( cv::Size(0, 0), right_center[0], right_center[1], rect_R_r); } -#endif + +const char RectifyProcessor::NAME[] = "RectifyProcessor"; + +RectifyProcessor::RectifyProcessor( + 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(); +} + +RectifyProcessor::~RectifyProcessor() { + VLOG(2) << __func__; +} + +std::string RectifyProcessor::Name() { + return 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)); +} + +Object *RectifyProcessor::OnCreateOutput() { + return new ObjMat2(); +} + +bool RectifyProcessor::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; +} MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index eda5b6c..c6744ed 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -48,8 +48,6 @@ class RectifyProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - void InitParams(IntrinsicsPinhole in_left, - IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); void InitParams(IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left); 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_ diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index f2a3232..46a5601 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -23,13 +23,16 @@ #include "mynteye/api/object.h" #include "mynteye/api/plugin.h" #include "mynteye/api/processor.h" -#include "mynteye/api/processor/depth_processor.h" #include "mynteye/api/processor/disparity_normalized_processor.h" #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/rectify_processor_ocv.h" #include "mynteye/api/processor/depth_processor_ocv.h" #include "mynteye/api/processor/points_processor_ocv.h" +#ifdef WITH_CAM_MODELS +#include "mynteye/api/processor/depth_processor.h" +#include "mynteye/api/processor/points_processor.h" +#include "mynteye/api/processor/rectify_processor.h" +#endif #include "mynteye/device/device.h" #define RECTIFY_PROC_PERIOD 0 @@ -87,8 +90,20 @@ Synthetic::~Synthetic() { } void Synthetic::NotifyImageParamsChanged() { - auto &&processor = find_processor(processor_); - if (processor) processor->NotifyImageParamsChanged(); + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&processor = find_processor(processor_); + if (processor) processor->NotifyImageParamsChanged(); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + auto &&processor = find_processor(processor_); + if (processor) processor->NotifyImageParamsChanged(); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + auto &&processor = find_processor(processor_); + if (processor) processor->NotifyImageParamsChanged(); + } } bool Synthetic::Supports(const Stream &stream) const { @@ -173,7 +188,18 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { } else if (mode == MODE_SYNTHETIC) { if (stream == Stream::LEFT_RECTIFIED || stream == Stream::RIGHT_RECTIFIED) { static std::shared_ptr output = nullptr; - auto &&processor = find_processor(processor_); + std::shared_ptr processor = nullptr; + if (calib_model_ == CalibrationModel::PINHOLE) { + processor = find_processor(processor_); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + processor = find_processor(processor_); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + processor = find_processor(processor_); + } auto &&out = processor->GetOutput(); if (out != nullptr) { // Obtain the output, out will be nullptr if get again immediately. @@ -219,6 +245,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { return {output->data, output->value, nullptr, output->id}; } VLOG(2) << "Points not ready now"; +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { auto &&processor = find_processor(processor_); auto &&out = processor->GetOutput(); @@ -227,6 +254,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { return {output->data, output->value, nullptr, output->id}; } VLOG(2) << "Points not ready now"; +#endif } else { // UNKNOW LOG(ERROR) << "Unknow calib model type in device: " @@ -242,6 +270,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { return {output->data, output->value, nullptr, output->id}; } VLOG(2) << "Depth not ready now"; +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { auto &&processor = find_processor(processor_); auto &&out = processor->GetOutput(); @@ -250,6 +279,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { return {output->data, output->value, nullptr, output->id}; } VLOG(2) << "Depth not ready now"; +#endif } else { // UNKNOW LOG(ERROR) << "Unknow calib model type in device: " @@ -346,13 +376,33 @@ void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) { if (!IsStreamDataEnabled(Stream::LEFT)) break; stream_enabled_mode_[stream] = MODE_SYNTHETIC; - CHECK(ActivateProcessor()); + if (calib_model_ == CalibrationModel::PINHOLE) { + CHECK(ActivateProcessor()); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + CHECK(ActivateProcessor()); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + CHECK(ActivateProcessor()); + } } return; case Stream::RIGHT_RECTIFIED: { if (!IsStreamDataEnabled(Stream::RIGHT)) break; stream_enabled_mode_[stream] = MODE_SYNTHETIC; - CHECK(ActivateProcessor()); + if (calib_model_ == CalibrationModel::PINHOLE) { + CHECK(ActivateProcessor()); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + CHECK(ActivateProcessor()); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + CHECK(ActivateProcessor()); + } } return; case Stream::DISPARITY: { stream_enabled_mode_[stream] = MODE_SYNTHETIC; @@ -370,8 +420,10 @@ void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) { EnableStreamData(Stream::DISPARITY, depth + 1); if (calib_model_ == CalibrationModel::PINHOLE) { CHECK(ActivateProcessor()); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { CHECK(ActivateProcessor()); +#endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_; @@ -382,8 +434,10 @@ void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) { EnableStreamData(Stream::POINTS, depth + 1); if (calib_model_ == CalibrationModel::PINHOLE) { CHECK(ActivateProcessor()); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { CHECK(ActivateProcessor()); +#endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_; @@ -410,7 +464,17 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { DisableStreamData(Stream::DISPARITY, depth + 1); } - DeactivateProcessor(); + if (calib_model_ == CalibrationModel::PINHOLE) { + DeactivateProcessor(); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + DeactivateProcessor(); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + DeactivateProcessor(); + } } break; case Stream::RIGHT_RECTIFIED: { if (IsStreamEnabledSynthetic(Stream::LEFT_RECTIFIED)) { @@ -419,7 +483,17 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { DisableStreamData(Stream::DISPARITY, depth + 1); } - DeactivateProcessor(); + if (calib_model_ == CalibrationModel::PINHOLE) { + DeactivateProcessor(); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + DeactivateProcessor(); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + DeactivateProcessor(); + } } break; case Stream::DISPARITY: { if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { @@ -439,8 +513,10 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { DisableStreamData(Stream::DEPTH, depth + 1); } DeactivateProcessor(); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { DeactivateProcessor(); +#endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_; @@ -449,11 +525,13 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { case Stream::DEPTH: { if (calib_model_ == CalibrationModel::PINHOLE) { DeactivateProcessor(); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { if (IsStreamEnabledSynthetic(Stream::DEPTH)) { DisableStreamData(Stream::POINTS, depth + 1); } DeactivateProcessor(); +#endif } else { LOG(ERROR) << "Unknow calib model type in device: " << calib_model_; @@ -470,8 +548,27 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { } void Synthetic::InitProcessors() { - auto &&rectify_processor = - std::make_shared(api_->device(), RECTIFY_PROC_PERIOD); + std::shared_ptr rectify_processor = nullptr; + cv::Mat Q; + if (calib_model_ == CalibrationModel::PINHOLE) { + auto &&rectify_processor_ocv = + std::make_shared(api_->device(), + 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); +#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(), + RECTIFY_PROC_PERIOD); + rectify_processor = rectify_processor_ocv; + } auto &&disparity_processor = std::make_shared(DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = @@ -479,33 +576,26 @@ void Synthetic::InitProcessors() { DISPARITY_NORM_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; + points_processor = std::make_shared( + Q, POINTS_PROC_PERIOD); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - auto &&points_processor_kan = std::make_shared( + points_processor = std::make_shared( POINTS_PROC_PERIOD); - points_processor = points_processor_kan; +#endif } 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; + points_processor = std::make_shared( + Q, POINTS_PROC_PERIOD); } 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; + depth_processor = std::make_shared(DEPTH_PROC_PERIOD); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - auto &&depth_processor_kan = - std::make_shared(DEPTH_PROC_PERIOD); - depth_processor = depth_processor_kan; + depth_processor = std::make_shared(DEPTH_PROC_PERIOD); +#endif } else { - auto &&depth_processor_pin = - std::make_shared(DEPTH_PROC_PERIOD); - depth_processor = depth_processor_pin; + depth_processor = std::make_shared(DEPTH_PROC_PERIOD); } using namespace std::placeholders; // NOLINT @@ -563,7 +653,18 @@ void Synthetic::ProcessNativeStream( } if (left_data.img && right_data.img && left_data.img->frame_id == right_data.img->frame_id) { - auto &&processor = find_processor(processor_); + std::shared_ptr processor = nullptr; + if (calib_model_ == CalibrationModel::PINHOLE) { + processor = find_processor(processor_); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + processor = find_processor(processor_); +#endif + } else { + LOG(ERROR) << "Unknow calib model type in device: " + << calib_model_ << ", use default pinhole model"; + processor = find_processor(processor_); + } processor->Process(ObjMat2{ left_data.frame, left_data.frame_id, left_data.img, right_data.frame, right_data.frame_id, right_data.img}); @@ -580,8 +681,16 @@ void Synthetic::ProcessNativeStream( } if (left_rect_data.img && right_rect_data.img && left_rect_data.img->frame_id == right_rect_data.img->frame_id) { + std::string name = RectifyProcessorOCV::NAME; + if (calib_model_ == CalibrationModel::PINHOLE) { + name = RectifyProcessorOCV::NAME; +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + name = RectifyProcessor::NAME; +#endif + } process_childs( - processor_, RectifyProcessor::NAME, ObjMat2{ + processor_, name, ObjMat2{ left_rect_data.frame, left_rect_data.frame_id, left_rect_data.img, right_rect_data.frame, right_rect_data.frame_id, right_rect_data.img}); @@ -603,10 +712,12 @@ void Synthetic::ProcessNativeStream( // PINHOLE process_childs(processor_, PointsProcessorOCV::NAME, ObjMat{data.frame, data.frame_id, data.img}); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { // KANNALA_BRANDT process_childs(processor_, PointsProcessor::NAME, ObjMat{data.frame, data.frame_id, data.img}); +#endif } else { // UNKNOW LOG(ERROR) << "Unknow calib model type in device: " @@ -618,10 +729,12 @@ void Synthetic::ProcessNativeStream( // PINHOLE process_childs(processor_, DepthProcessorOCV::NAME, ObjMat{data.frame, data.frame_id, data.img}); +#ifdef WITH_CAM_MODELS } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { // KANNALA_BRANDT process_childs(processor_, DepthProcessor::NAME, ObjMat{data.frame, data.frame_id, data.img}); +#endif } else { // UNKNOW LOG(ERROR) << "Unknow calib model type in device: " diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc index 7b70e9d..6619cd7 100644 --- a/src/mynteye/device/channel/channels.cc +++ b/src/mynteye/device/channel/channels.cc @@ -22,6 +22,7 @@ #include #include +#include "mynteye/device/config.h" #include "mynteye/logger.h" #include "mynteye/util/times.h" @@ -520,10 +521,31 @@ bool Channels::SetFiles( } } if (img_params != nullptr) { - auto n = file_channel_.SetImgParamsToData(img_params, data + 3 + size); - if (n > 0) { - header[1] = true; - size += n; + // remove not supported resolution + auto&& res = adapter_->GetResolutionSupports(); + for (auto it = img_params->begin(); it != img_params->end(); ) { + if (res.find(it->first) == res.end()) { + LOG(WARNING) << "Image params of resolution " + << it->first.width << "x" << it->first.height << " not supported"; + it = img_params->erase(it); + } else { + ++it; + } + } + + if (img_params->empty()) { + std::ostringstream os; + os << "Image params resolution must be "; + for (auto&& r : res) { + os << r.width << "x" << r.height << " "; + } + LOG(WARNING) << os.str(); + } else { + auto n = file_channel_.SetImgParamsToData(img_params, data + 3 + size); + if (n > 0) { + header[1] = true; + size += n; + } } } if (imu_params != nullptr) { @@ -719,4 +741,31 @@ Channels::control_info_t Channels::XuControlInfo(Option option) const { return {min, max, def}; } +// ChannelsAdapter + +ChannelsAdapter::ChannelsAdapter(const Model &model) + : model_(model) { +} + +ChannelsAdapter::~ChannelsAdapter() { +} + +std::set