tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(src): Determine the default intrinsics based on the resulution

Browse Source
This commit is contained in:
Osenberg 2019-08-06 20:04:38 +08:00
parent 1cda60d137
commit 2260f5c582
11 changed files with 128 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -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

4
include/mynteye/device/device.h
View File

@ -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<uvc::device> 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;
};

28
src/mynteye/api/api.cc
View File

@ -244,27 +244,15 @@ std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &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<API>(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<API>(device, CalibrationModel::UNKNOW);
return api;
} else {
api = std::make_shared<API>(device, left_intr->calib_model());
return api;
}
api = std::make_shared<API>(device, left_intr->calib_model());
return api;
}
} else {
LOG(ERROR) <<"no device!";
@ -573,11 +561,7 @@ std::shared_ptr<Device> 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 "

95
src/mynteye/api/config.cc
View File

@ -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<IntrinsicsBase> getDefaultIntrinsics() {
auto res = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsBase> getDefaultIntrinsics(const StreamRequest &request) {
auto res = getDefaultIntrinsics();
res->resize_scale = static_cast<double>(request.width / res->width);
res->ResizeIntrinsics();
return res;
}
std::shared_ptr<Extrinsics> getDefaultExtrinsics() {
auto res = std::make_shared<Extrinsics>();
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

29
src/mynteye/api/config.h
View File

@ -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<IntrinsicsBase> getDefaultIntrinsics();
std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const StreamRequest &request);
std::shared_ptr<Extrinsics> getDefaultExtrinsics();
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_API_CONFIG_H_

22
src/mynteye/api/synthetic.cc
View File

@ -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<Extrinsics>(
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<Extrinsics>(
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<Extrinsics>(
api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT));
}
Synthetic::Synthetic(API *api, CalibrationModel calib_model)

1
src/mynteye/api/synthetic.h
View File

@ -22,7 +22,6 @@
#include <mutex>
#include "mynteye/api/api.h"
#include "mynteye/api/config.h"
MYNTEYE_BEGIN_NAMESPACE

76
src/mynteye/device/config.cc
View File

@ -89,4 +89,80 @@ stream_requests_map = {
}
};
/**
* default intrinsics
*/
std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
auto res = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsBase> getDefaultIntrinsics(const Resolution &resolution) {
auto res = getDefaultIntrinsics();
res->resize_scale = static_cast<double>(resolution.width / res->width);
res->ResizeIntrinsics();
return res;
}
std::shared_ptr<Extrinsics> getDefaultExtrinsics() {
auto res = std::make_shared<Extrinsics>();
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

9
src/mynteye/device/config.h
View File

@ -18,6 +18,7 @@
#include <map>
#include <set>
#include <vector>
#include <memory>
#include "mynteye/mynteye.h"
#include "mynteye/types.h"
@ -37,6 +38,14 @@ using StreamRequests = std::vector<StreamRequest>;
extern const std::map<Model, std::map<Capabilities, StreamRequests>>
stream_requests_map;
const std::vector<Resolution> resolution_list = {{640, 400},
{1280, 800}};
std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics();
std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics(const Resolution &resolution);
std::shared_ptr<Extrinsics> getDefaultExtrinsics();
MYNTEYE_END_NAMESPACE
#endif // MYNTEYE_DEVICE_CONFIG_H_

31
src/mynteye/device/device.cc
View File

@ -89,7 +89,8 @@ Device::Device(const Model &model,
device_(device),
streams_(std::make_shared<Streams>(streams_adapter)),
channels_(std::make_shared<Channels>(device_, channels_adapter)),
motions_(std::make_shared<Motions>(channels_)) {
motions_(std::make_shared<Motions>(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 &&params : 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

57
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -1200,48 +1200,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
return nullptr;
}
std::shared_ptr<IntrinsicsBase> getDefaultIntrinsics() {
auto res = std::make_shared<IntrinsicsPinhole>();
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<Extrinsics> getDefaultExtrinsics() {
auto res = std::make_shared<Extrinsics>();
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<IntrinsicsPinhole>(in_left_base);
auto in_right = *std::dynamic_pointer_cast<IntrinsicsPinhole>(
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 =