From 2260f5c5828971eeccb2279b6512e981ff593b03 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Tue, 6 Aug 2019 20:04:38 +0800 Subject: [PATCH] fix(src): Determine the default intrinsics based on the resulution --- CMakeLists.txt | 1 - include/mynteye/device/device.h | 4 + src/mynteye/api/api.cc | 28 ++---- src/mynteye/api/config.cc | 95 ------------------- src/mynteye/api/config.h | 29 ------ src/mynteye/api/synthetic.cc | 22 +---- src/mynteye/api/synthetic.h | 1 - src/mynteye/device/config.cc | 76 +++++++++++++++ src/mynteye/device/config.h | 9 ++ src/mynteye/device/device.cc | 31 ++++-- .../src/wrapper_nodelet.cc | 57 +---------- 11 files changed, 128 insertions(+), 225 deletions(-) delete mode 100644 src/mynteye/api/config.cc delete mode 100644 src/mynteye/api/config.h diff --git a/CMakeLists.txt b/CMakeLists.txt index be04407..d5dcb15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -218,7 +218,6 @@ if(WITH_API) src/mynteye/api/processor/points_processor_ocv.cc src/mynteye/api/processor/depth_processor_ocv.cc src/mynteye/api/processor/rectify_processor_ocv.cc - src/mynteye/api/config.cc src/mynteye/api/correspondence.cc src/mynteye/api/version_checker.cc src/mynteye/api/data_tools.cc diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index f991e88..19fbc1c 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -300,6 +300,8 @@ class MYNTEYE_API Device { /** Enable process mode, e.g. imu assembly, temp_drift */ void EnableProcessMode(const std::int32_t& mode); + bool CheckImageParams(); + protected: std::shared_ptr device() const { return device_; @@ -379,6 +381,8 @@ class MYNTEYE_API Device { bool SetFiles( DeviceInfo *info, img_params_map_t *img_params, imu_params_t *imu_params); + bool is_default_intrinsics_; + friend API; friend tools::DeviceWriter; }; diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index dcbfd47..4e67ac2 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -244,27 +244,15 @@ std::shared_ptr API::Create(const std::shared_ptr &device) { 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."; + + 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); return api; } 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); - return api; - } else { - api = std::make_shared(device, left_intr->calib_model()); - return api; - } + api = std::make_shared(device, left_intr->calib_model()); + return api; } } else { LOG(ERROR) <<"no device!"; @@ -573,11 +561,7 @@ std::shared_ptr API::device() { // TODO(Kalman): Call this function in the appropriate place void API::CheckImageParams() { if (device_ != nullptr) { - bool in_l_ok, in_r_ok, ex_l2r_ok; - device_->GetIntrinsics(Stream::LEFT, &in_l_ok); - device_->GetIntrinsics(Stream::RIGHT, &in_r_ok); - device_->GetExtrinsics(Stream::LEFT, Stream::RIGHT, &ex_l2r_ok); - if (!in_l_ok || !in_r_ok || !ex_l2r_ok) { + if (device_->CheckImageParams()) { LOG(FATAL) << "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 " diff --git a/src/mynteye/api/config.cc b/src/mynteye/api/config.cc deleted file mode 100644 index 4d20964..0000000 --- a/src/mynteye/api/config.cc +++ /dev/null @@ -1,95 +0,0 @@ -// 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->width = 640; - res->height = 400; - res->model = 0; - 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 getDefaultIntrinsics(const StreamRequest &request) { - auto res = getDefaultIntrinsics(); - res->resize_scale = static_cast(request.width / res->width); - res->ResizeIntrinsics(); - 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; -} - -// sgbm_matcher = cv::StereoSGBM::create(0, 16, 3); -// sgbm_matcher->setPreFilterCap(63); -// sgbm_matcher->setBlockSize(sgbmWinSize); -// sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize); -// sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize); -// sgbm_matcher->setMinDisparity(0); -// sgbm_matcher->setNumDisparities(numberOfDisparities); -// sgbm_matcher->setUniquenessRatio(10); -// sgbm_matcher->setSpeckleWindowSize(100); -// sgbm_matcher->setSpeckleRange(32); -// sgbm_matcher->setDisp12MaxDiff(1); - -// bm_matcher = cv::StereoBM::create(0, 3); -// bm_matcher->setPreFilterSize(9); -// bm_matcher->setPreFilterCap(31); -// bm_matcher->setBlockSize(15); -// bm_matcher->setMinDisparity(0); -// bm_matcher->setNumDisparities(64); -// bm_matcher->setUniquenessRatio(60); -// bm_matcher->setTextureThreshold(10); -// bm_matcher->setSpeckleWindowSize(100); -// bm_matcher->setSpeckleRange(4); - - -MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/config.h b/src/mynteye/api/config.h deleted file mode 100644 index 7768c22..0000000 --- a/src/mynteye/api/config.h +++ /dev/null @@ -1,29 +0,0 @@ -// 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 getDefaultIntrinsics(const StreamRequest &request); - -std::shared_ptr getDefaultExtrinsics(); - -MYNTEYE_END_NAMESPACE - -#endif // MYNTEYE_API_CONFIG_H_ diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 8aa83ab..d0782b0 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -29,7 +29,6 @@ #include "mynteye/api/processor/rectify_processor_ocv.h" #include "mynteye/api/processor/depth_processor_ocv.h" #include "mynteye/api/processor/points_processor_ocv.h" -#include "mynteye/api/config.h" #ifdef WITH_CAM_MODELS #include "mynteye/api/processor/depth_processor.h" #include "mynteye/api/processor/points_processor.h" @@ -50,27 +49,16 @@ MYNTEYE_BEGIN_NAMESPACE void Synthetic::InitCalibInfo() { if (calib_model_ == CalibrationModel::PINHOLE) { LOG(INFO) << "camera calib model: pinhole"; - intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); - intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); - extr_ = std::make_shared( - api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); #ifdef WITH_CAM_MODELS } 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)); #endif - } else { - calib_default_tag_ = true; - calib_model_ = CalibrationModel::PINHOLE; - LOG(INFO) << "camera calib model: unknow, use default pinhole data"; - auto stream_request = api_->GetStreamRequest(); - intr_left_ = getDefaultIntrinsics(stream_request); - intr_right_ = getDefaultIntrinsics(stream_request); - extr_ = getDefaultExtrinsics(); } + + 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) diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index 8ca33e1..1e78c12 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -22,7 +22,6 @@ #include #include "mynteye/api/api.h" -#include "mynteye/api/config.h" MYNTEYE_BEGIN_NAMESPACE diff --git a/src/mynteye/device/config.cc b/src/mynteye/device/config.cc index c44682a..c92cf8b 100644 --- a/src/mynteye/device/config.cc +++ b/src/mynteye/device/config.cc @@ -89,4 +89,80 @@ stream_requests_map = { } }; +/** + * default intrinsics + */ + +std::shared_ptr getDefaultIntrinsics() { + auto res = std::make_shared(); + res->width = 640; + res->height = 400; + res->model = 0; + 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 getDefaultIntrinsics(const Resolution &resolution) { + auto res = getDefaultIntrinsics(); + res->resize_scale = static_cast(resolution.width / res->width); + res->ResizeIntrinsics(); + 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; +} + +// sgbm_matcher = cv::StereoSGBM::create(0, 16, 3); +// sgbm_matcher->setPreFilterCap(63); +// sgbm_matcher->setBlockSize(sgbmWinSize); +// sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize); +// sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize); +// sgbm_matcher->setMinDisparity(0); +// sgbm_matcher->setNumDisparities(numberOfDisparities); +// sgbm_matcher->setUniquenessRatio(10); +// sgbm_matcher->setSpeckleWindowSize(100); +// sgbm_matcher->setSpeckleRange(32); +// sgbm_matcher->setDisp12MaxDiff(1); + +// bm_matcher = cv::StereoBM::create(0, 3); +// bm_matcher->setPreFilterSize(9); +// bm_matcher->setPreFilterCap(31); +// bm_matcher->setBlockSize(15); +// bm_matcher->setMinDisparity(0); +// bm_matcher->setNumDisparities(64); +// bm_matcher->setUniquenessRatio(60); +// bm_matcher->setTextureThreshold(10); +// bm_matcher->setSpeckleWindowSize(100); +// bm_matcher->setSpeckleRange(4); + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/config.h b/src/mynteye/device/config.h index f38dab0..fdef12a 100644 --- a/src/mynteye/device/config.h +++ b/src/mynteye/device/config.h @@ -18,6 +18,7 @@ #include #include #include +#include #include "mynteye/mynteye.h" #include "mynteye/types.h" @@ -37,6 +38,14 @@ using StreamRequests = std::vector; extern const std::map> stream_requests_map; +const std::vector resolution_list = {{640, 400}, + {1280, 800}}; + +std::shared_ptr getDefaultIntrinsics(); +std::shared_ptr getDefaultIntrinsics(const Resolution &resolution); + +std::shared_ptr getDefaultExtrinsics(); + MYNTEYE_END_NAMESPACE #endif // MYNTEYE_DEVICE_CONFIG_H_ diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 029a0bb..fba88ef 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -89,7 +89,8 @@ Device::Device(const Model &model, device_(device), streams_(std::make_shared(streams_adapter)), channels_(std::make_shared(device_, channels_adapter)), - motions_(std::make_shared(channels_)) { + motions_(std::make_shared(channels_)), + is_default_intrinsics_(false) { VLOG(2) << __func__; ReadAllInfos(); } @@ -612,11 +613,28 @@ void Device::ReadAllInfos() { motions_->SetDeviceInfo(device_info_); - bool img_params_ok = false; + if (all_img_params_.empty()) { + is_default_intrinsics_ = true; + 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) << "Intrinsics & extrinsics not exist. Use default intrinsics."; + LOG(INFO) << "camera calib model: unknow, use default pinhole data"; + auto spec_string = device_info_->spec_version.to_string(); + for (auto &&resolution : resolution_list) { + all_img_params_[resolution] = { + true, spec_string, getDefaultIntrinsics(resolution), + getDefaultIntrinsics(resolution), *getDefaultExtrinsics()}; + } + } + for (auto &¶ms : all_img_params_) { auto &&img_params = params.second; if (img_params.ok) { - img_params_ok = true; SetIntrinsics(Stream::LEFT, img_params.in_left); SetIntrinsics(Stream::RIGHT, img_params.in_right); SetExtrinsics(Stream::RIGHT, Stream::LEFT, img_params.ex_right_to_left); @@ -627,9 +645,6 @@ void Device::ReadAllInfos() { break; } } - if (!img_params_ok) { - LOG(WARNING) << "Intrinsics & extrinsics not exist"; - } if (imu_params.ok) { imu_params_ = imu_params; @@ -726,4 +741,8 @@ void Device::EnableProcessMode(const std::int32_t& mode) { motions_->EnableProcessMode(mode); } +bool Device::CheckImageParams() { + return is_default_intrinsics_; +} + MYNTEYE_END_NAMESPACE diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index af7a42e..059811b 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -1200,48 +1200,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet { return nullptr; } - std::shared_ptr getDefaultIntrinsics() { - auto res = std::make_shared(); - res->width = 640; - res->height = 400; - res->model = 0; - 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; - } - void publishMesh() { mesh_msg_.header.frame_id = base_frame_id_; mesh_msg_.header.stamp = ros::Time::now(); @@ -1279,24 +1237,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet { ROS_ASSERT(api_); auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT); auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT); - is_intrinsics_enable_ = in_left_base && in_right_base; - if (is_intrinsics_enable_) { - if (in_left_base->calib_model() != CalibrationModel::PINHOLE || - in_right_base->calib_model() != CalibrationModel::PINHOLE) { - return; - } - } else { - in_left_base = getDefaultIntrinsics(); - in_right_base = getDefaultIntrinsics(); + if (in_left_base->calib_model() != CalibrationModel::PINHOLE || + in_right_base->calib_model() != CalibrationModel::PINHOLE) { + return; } auto in_left = *std::dynamic_pointer_cast(in_left_base); auto in_right = *std::dynamic_pointer_cast( in_right_base); auto ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT); - if (!is_intrinsics_enable_) { - ex_right_to_left = *(getDefaultExtrinsics()); - } cv::Size size{in_left.width, in_left.height}; cv::Mat M1 =