diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 992fd31..444c80c 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -315,6 +315,13 @@ std::ostream &operator<<(std::ostream &os, const MotionIntrinsics &in); struct MYNTEYE_API Extrinsics { double rotation[3][3]; /**< rotation matrix */ 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 diff --git a/src/device/device.cc b/src/device/device.cc index 4ac51e2..b97dafe 100644 --- a/src/device/device.cc +++ b/src/device/device.cc @@ -164,8 +164,13 @@ Extrinsics Device::GetExtrinsics(const Stream &from, const Stream &to) const { try { return stream_from_extrinsics_.at(from).at(to); } catch (const std::out_of_range &e) { - LOG(WARNING) << "Extrinsics from " << from << " to " << to << " not found"; - return {}; + try { + 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 {}; + } } } 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 be2068a..1e06be9 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 @@ -426,12 +426,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet { camera_info->P.at(11) = 0; if (stream == Stream::RIGHT) { - /* auto &&ex = api_->GetExtrinsics(stream, Stream::LEFT); camera_info->P.at(3) = ex.translation[0]; camera_info->P.at(7) = ex.translation[1]; camera_info->P.at(11) = ex.translation[2]; - */ } camera_info->distortion_model = "plumb_bob";