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

fix(ros): add default intrinsics in ros node

Browse Source
This commit is contained in:
TinyOh 2019-01-15 17:18:16 +08:00
parent 41c882d0a7
commit b3bc96f12b
2 changed files with 75 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/mynteye/api/processor/disparity_processor.cc
View File

@ -23,6 +23,8 @@
#include "mynteye/logger.h"
#define WITH_BM_SOBEL_FILTER
MYNTEYE_BEGIN_NAMESPACE
const char DisparityProcessor::NAME[] = "DisparityProcessor";
@ -67,6 +69,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type,
#endif
#ifdef WITH_BM_SOBEL_FILTER
} else if (type_ == DisparityProcessorType::BM) {
int bmWinSize = 3;
#ifdef WITH_OPENCV2
int bmWinSize = 3;
// StereoBM
@ -86,7 +89,7 @@ DisparityProcessor::DisparityProcessor(DisparityProcessorType type,
bm_matcher = cv::StereoBM::create(0, 3);
bm_matcher->setPreFilterSize(9);
bm_matcher->setPreFilterCap(31);
bm_matcher->setBlockSize(15);
bm_matcher->setBlockSize(bmWinSize);
bm_matcher->setMinDisparity(0);
bm_matcher->setNumDisparities(64);
bm_matcher->setUniquenessRatio(15);
@ -183,8 +186,13 @@ bool DisparityProcessor::OnProcess(
#ifdef WITH_BM_SOBEL_FILTER
} else if (type_ == DisparityProcessorType::BM) {
cv::Mat tmp1, tmp2;
cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
if (input->first.channels() == 1) {
// s1030
} else if (input->first.channels() == 3) {
// s210
cv::cvtColor(input->first, tmp1, CV_RGB2GRAY);
cv::cvtColor(input->second, tmp2, CV_RGB2GRAY);
}
bm_matcher->compute(tmp1, tmp2, disparity);
#endif
} else {

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

@ -924,18 +924,71 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
computeRectTransforms();
}
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 computeRectTransforms() {
ROS_ASSERT(api_);
auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT);
auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT);
if (in_left_base->calib_model() != CalibrationModel::PINHOLE ||
in_right_base->calib_model() != CalibrationModel::PINHOLE) {
return;
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();
}
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 =
@ -975,10 +1028,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info);
std::shared_ptr<IntrinsicsBase> in_base;
if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
in_base = api_->GetIntrinsicsBase(Stream::RIGHT);
if (is_intrinsics_enable_) {
if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) {
in_base = api_->GetIntrinsicsBase(Stream::RIGHT);
} else {
in_base = api_->GetIntrinsicsBase(Stream::LEFT);
}
} else {
in_base = api_->GetIntrinsicsBase(Stream::LEFT);
in_base = getDefaultIntrinsics();
}
camera_info->header.frame_id = frame_ids_[stream];
@ -1275,6 +1332,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
bool is_motion_published_;
bool is_started_;
int frame_rate_;
bool is_intrinsics_enable_;
};
MYNTEYE_END_NAMESPACE