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
Download Patch File Download Diff File Expand all files Collapse all files

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 =