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

Could inverse extrinsics

Browse Source
This commit is contained in:
John Zhao 2018-04-29 14:47:35 +08:00
parent 77837933bc
commit f3d6fcdcb9
3 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/mynteye/types.h
View File

@ -315,6 +315,13 @@ std::ostream &operator<<(std::ostream &os, const MotionIntrinsics &in);
struct MYNTEYE_API Extrinsics { struct MYNTEYE_API Extrinsics {
double rotation[3][3]; /**< rotation matrix */ double rotation[3][3]; /**< rotation matrix */
double translation[3]; /**< translation vector */ double translation[3]; /**< translation vector */
Extrinsics Inverse() const {
return {{{rotation[0][0], rotation[1][0], rotation[2][0]},
{rotation[0][1], rotation[1][1], rotation[2][1]},
{rotation[0][2], rotation[1][2], rotation[2][2]}},
{-translation[0], -translation[1], -translation[2]}};
}
}; };
MYNTEYE_API MYNTEYE_API

9
src/device/device.cc
View File

@ -164,8 +164,13 @@ Extrinsics Device::GetExtrinsics(const Stream &from, const Stream &to) const {
try { try {
return stream_from_extrinsics_.at(from).at(to); return stream_from_extrinsics_.at(from).at(to);
} catch (const std::out_of_range &e) { } catch (const std::out_of_range &e) {
LOG(WARNING) << "Extrinsics from " << from << " to " << to << " not found"; try {
return {}; return stream_from_extrinsics_.at(to).at(from).Inverse();
} catch (const std::out_of_range &e) {
LOG(WARNING) << "Extrinsics from " << from << " to " << to
<< " not found";
return {};
}
} }
} }

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

@ -426,12 +426,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
camera_info->P.at(11) = 0; camera_info->P.at(11) = 0;
if (stream == Stream::RIGHT) { if (stream == Stream::RIGHT) {
/*
auto &&ex = api_->GetExtrinsics(stream, Stream::LEFT); auto &&ex = api_->GetExtrinsics(stream, Stream::LEFT);
camera_info->P.at(3) = ex.translation[0]; camera_info->P.at(3) = ex.translation[0];
camera_info->P.at(7) = ex.translation[1]; camera_info->P.at(7) = ex.translation[1];
camera_info->P.at(11) = ex.translation[2]; camera_info->P.at(11) = ex.translation[2];
*/
} }
camera_info->distortion_model = "plumb_bob"; camera_info->distortion_model = "plumb_bob";