From 07570f330ba517f6dcda641a0fdfa7ff02a58e31 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Fri, 4 Jan 2019 13:29:06 +0800 Subject: [PATCH] refactor(intrinsics): apply base intrinsics --- include/mynteye/api/api.h | 17 ++++--- include/mynteye/device/callbacks.h | 4 +- include/mynteye/device/device.h | 22 +++----- include/mynteye/types.h | 4 +- src/mynteye/api/api.cc | 12 ++++- .../api/processor/rectify_processor.cc | 10 ++-- src/mynteye/api/processor/rectify_processor.h | 4 +- src/mynteye/device/channels.cc | 4 +- src/mynteye/device/channels.h | 4 +- src/mynteye/device/device.cc | 9 ++-- .../device/standard/channels_adapter_s.cc | 15 +++--- .../device/standard2/channels_adapter_s2.cc | 50 ++++++++++++------- .../standard2/channels_adapter_s210a.cc | 50 ++++++++++++------- tools/writer/device_writer.cc | 32 ++++++++---- 14 files changed, 143 insertions(+), 94 deletions(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index 66fd848..469e680 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -187,14 +187,18 @@ class MYNTEYE_API API { std::string GetInfo(const Info &info) const; /** - * Get the intrinsics of stream. + * @deprecated Get the intrinsics (pinhole) of stream. */ - Intrinsics GetIntrinsics(const Stream &stream) const; + IntrinsicsPinhole GetIntrinsics(const Stream &stream) const; /** * Get the intrinsics of stream. */ - template - T GetIntrinsics(const Stream &from) const; + template + T GetIntrinsics(const Stream &stream) const; + /** + * Get the intrinsics base of stream. + */ + std::shared_ptr GetIntrinsicsBase(const Stream &stream) const; /** * Get the extrinsics from one stream to another. */ @@ -311,8 +315,9 @@ class MYNTEYE_API API { }; template -T API::GetIntrinsics(const Stream &from) const { - return device_->GetIntrinsics(from); +T API::GetIntrinsics(const Stream &stream) const { + auto in = GetIntrinsicsBase(stream); + return *std::dynamic_pointer_cast(in); } MYNTEYE_END_NAMESPACE diff --git a/include/mynteye/device/callbacks.h b/include/mynteye/device/callbacks.h index 19730f6..91cf934 100644 --- a/include/mynteye/device/callbacks.h +++ b/include/mynteye/device/callbacks.h @@ -32,8 +32,8 @@ namespace device { typedef struct ImgParams { bool ok; - Intrinsics in_left; - Intrinsics in_right; + std::shared_ptr in_left; + std::shared_ptr in_right; Extrinsics ex_right_to_left; } img_params_t; diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index 3e8662f..e575dda 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -153,12 +153,7 @@ class MYNTEYE_API Device { /** * Get the intrinsics of stream. */ - Intrinsics GetIntrinsics(const Stream &stream) const; - /** - * Get the intrinsics of stream. - */ - template - T GetIntrinsics(const Stream &from) const; + std::shared_ptr GetIntrinsics(const Stream &stream) const; /** * Get the extrinsics from one stream to another. */ @@ -175,7 +170,8 @@ class MYNTEYE_API Device { /** * Get the intrinsics of stream. */ - Intrinsics GetIntrinsics(const Stream &stream, bool *ok) const; + std::shared_ptr GetIntrinsics( + const Stream &stream, bool *ok) const; /** * Get the extrinsics from one stream to another. */ @@ -193,7 +189,8 @@ class MYNTEYE_API Device { /** * Set the intrinsics of stream. */ - void SetIntrinsics(const Stream &stream, const Intrinsics &in); + void SetIntrinsics(const Stream &stream, + const std::shared_ptr &in); /** * Set the extrinsics from one stream to another. */ @@ -332,7 +329,7 @@ class MYNTEYE_API Device { std::map all_img_params_; device::imu_params_t imu_params_; - std::map stream_intrinsics_; + std::map> stream_intrinsics_; std::map> stream_from_extrinsics_; std::shared_ptr motion_intrinsics_; @@ -365,13 +362,6 @@ class MYNTEYE_API Device { friend tools::DeviceWriter; }; -template -T Device::GetIntrinsics(const Stream &from) const { - T res; - printf("type %d\n", res.calib_model_); - return res; -} - MYNTEYE_END_NAMESPACE #endif // MYNTEYE_DEVICE_DEVICE_H_ diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 201129f..25fd45f 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -414,6 +414,7 @@ struct MYNTEYE_API IntrinsicsBase { IntrinsicsBase() { calib_model = CalibrationModel::CALIB_MODEL_UNKNOW; } + virtual ~IntrinsicsBase() {} CalibrationModel calib_model; }; @@ -447,8 +448,7 @@ MYNTEYE_API std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in); /** - * @ingroup calibration - * Stream intrinsics (Pinhole) + * @deprecated Replaced by IntrinsicsPinhole. */ using Intrinsics = IntrinsicsPinhole; diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index adc70e2..f030e45 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -294,7 +294,17 @@ std::string API::GetInfo(const Info &info) const { return device_->GetInfo(info); } -Intrinsics API::GetIntrinsics(const Stream &stream) const { +IntrinsicsPinhole API::GetIntrinsics(const Stream &stream) const { + auto in = GetIntrinsicsBase(stream); + if (in->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) { + return *std::dynamic_pointer_cast(in); + } + throw std::runtime_error("Intrinsics is not pinhole model" + ", please use GetIntrinsicsBase() or GetIntrinsics() instead."); +} + +std::shared_ptr API::GetIntrinsicsBase( + const Stream &stream) const { return device_->GetIntrinsics(stream); } diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 86b0988..d985baf 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -41,9 +41,11 @@ std::string RectifyProcessor::Name() { } void RectifyProcessor::NotifyImageParamsChanged() { + auto in_left = device_->GetIntrinsics(Stream::LEFT); + auto in_right = device_->GetIntrinsics(Stream::RIGHT); InitParams( - device_->GetIntrinsics(Stream::LEFT), - device_->GetIntrinsics(Stream::RIGHT), + *std::dynamic_pointer_cast(in_left), + *std::dynamic_pointer_cast(in_right), device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT)); } @@ -66,7 +68,9 @@ bool RectifyProcessor::OnProcess( } void RectifyProcessor::InitParams( - Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left) { + IntrinsicsPinhole in_left, + IntrinsicsPinhole in_right, + Extrinsics ex_right_to_left) { cv::Size size{in_left.width, in_left.height}; cv::Mat M1 = diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 30b615b..94c7981 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -48,8 +48,8 @@ class RectifyProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - void InitParams( - Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left); + void InitParams(IntrinsicsPinhole in_left, + IntrinsicsPinhole in_right, Extrinsics ex_right_to_left); std::shared_ptr device_; }; diff --git a/src/mynteye/device/channels.cc b/src/mynteye/device/channels.cc index 7a02618..5e485d1 100644 --- a/src/mynteye/device/channels.cc +++ b/src/mynteye/device/channels.cc @@ -811,7 +811,7 @@ std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) { return i; } -std::size_t from_data(Intrinsics *in, const std::uint8_t *data, +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, const Version *spec_version) { std::size_t i = 0; @@ -954,7 +954,7 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data, return size + 3; } -std::size_t to_data(const Intrinsics *in, std::uint8_t *data, +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, const Version *spec_version) { std::size_t i = 0; diff --git a/src/mynteye/device/channels.h b/src/mynteye/device/channels.h index ec8298f..ad09ac0 100644 --- a/src/mynteye/device/channels.h +++ b/src/mynteye/device/channels.h @@ -204,7 +204,7 @@ std::string _from_data(const std::uint8_t *data, std::size_t count); std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data); -std::size_t from_data(Intrinsics *in, const std::uint8_t *data, +std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data, const Version *spec_version); std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data, @@ -236,7 +236,7 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count); std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data, const Version *spec_version); -std::size_t to_data(const Intrinsics *in, std::uint8_t *data, +std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data, const Version *spec_version); std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data, diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index e57dd48..3280bf1 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -240,7 +240,8 @@ std::string Device::GetInfo(const Info &info) const { } } -Intrinsics Device::GetIntrinsics(const Stream &stream) const { +std::shared_ptr Device::GetIntrinsics( + const Stream &stream) const { bool ok; return GetIntrinsics(stream, &ok); } @@ -260,7 +261,8 @@ Extrinsics Device::GetMotionExtrinsics(const Stream &from) const { return GetMotionExtrinsics(from, &ok); } -Intrinsics Device::GetIntrinsics(const Stream &stream, bool *ok) const { +std::shared_ptr Device::GetIntrinsics( + const Stream &stream, bool *ok) const { try { *ok = true; return stream_intrinsics_.at(stream); @@ -311,7 +313,8 @@ Extrinsics Device::GetMotionExtrinsics(const Stream &from, bool *ok) const { } } -void Device::SetIntrinsics(const Stream &stream, const Intrinsics &in) { +void Device::SetIntrinsics(const Stream &stream, + const std::shared_ptr &in) { stream_intrinsics_[stream] = in; } diff --git a/src/mynteye/device/standard/channels_adapter_s.cc b/src/mynteye/device/standard/channels_adapter_s.cc index 1b3c502..ea013c9 100644 --- a/src/mynteye/device/standard/channels_adapter_s.cc +++ b/src/mynteye/device/standard/channels_adapter_s.cc @@ -131,11 +131,12 @@ std::size_t StandardChannelsAdapter::GetImgParamsFromData( Channels::img_params_t *img_params) { std::size_t i = 0; - Intrinsics in_left, in_right; + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); Extrinsics ex_right_to_left; - i += bytes::from_data(&in_left, data + i, version); - i += bytes::from_data(&in_right, data + i, version); + i += bytes::from_data(in_left.get(), data + i, version); + i += bytes::from_data(in_right.get(), data + i, version); i += bytes::from_data(&ex_right_to_left, data + i, version); (*img_params)[{752, 480}] = {true, in_left, in_right, ex_right_to_left}; @@ -147,9 +148,11 @@ std::size_t StandardChannelsAdapter::SetImgParamsToData( std::uint8_t *data) { std::size_t i = 3; // skip id, size - auto &¶ms = (*img_params).at({752, 480}); - i += bytes::to_data(¶ms.in_left, data + i, version); - i += bytes::to_data(¶ms.in_right, data + i, version); + auto params = (*img_params).at({752, 480}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast(params.in_right); + i += bytes::to_data(in_left.get(), data + i, version); + i += bytes::to_data(in_right.get(), data + i, version); i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); // others diff --git a/src/mynteye/device/standard2/channels_adapter_s2.cc b/src/mynteye/device/standard2/channels_adapter_s2.cc index 0b88de4..dc91636 100644 --- a/src/mynteye/device/standard2/channels_adapter_s2.cc +++ b/src/mynteye/device/standard2/channels_adapter_s2.cc @@ -127,20 +127,26 @@ std::size_t Standard2ChannelsAdapter::GetImgParamsFromData( Channels::img_params_t *img_params) { std::size_t i = 0; - Intrinsics in_left, in_right; Extrinsics ex_right_to_left; - - i += bytes::from_data(&in_left, data + i, version); - i += bytes::from_data(&in_right, data + i, version); - (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; - - i += bytes::from_data(&in_left, data + i, version); - i += bytes::from_data(&in_right, data + i, version); - (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; - - i += bytes::from_data(&ex_right_to_left, data + i, version); - (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; - (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i, version); + i += bytes::from_data(in_right.get(), data + i, version); + (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; + } + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i, version); + i += bytes::from_data(in_right.get(), data + i, version); + (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; + } + { + i += bytes::from_data(&ex_right_to_left, data + i, version); + (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; + (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; + } return i; } @@ -151,14 +157,20 @@ std::size_t Standard2ChannelsAdapter::SetImgParamsToData( std::size_t i = 3; // skip id, size { - auto &¶ms = (*img_params).at({1280, 400}); - i += bytes::to_data(¶ms.in_left, data + i, version); - i += bytes::to_data(¶ms.in_right, data + i, version); + auto params = (*img_params).at({1280, 400}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i, version); + i += bytes::to_data(in_right.get(), data + i, version); } { - auto &¶ms = (*img_params).at({2560, 800}); - i += bytes::to_data(¶ms.in_left, data + i, version); - i += bytes::to_data(¶ms.in_right, data + i, version); + auto params = (*img_params).at({2560, 800}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i, version); + i += bytes::to_data(in_right.get(), data + i, version); i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); } diff --git a/src/mynteye/device/standard2/channels_adapter_s210a.cc b/src/mynteye/device/standard2/channels_adapter_s210a.cc index 22f5a69..6283c8a 100644 --- a/src/mynteye/device/standard2/channels_adapter_s210a.cc +++ b/src/mynteye/device/standard2/channels_adapter_s210a.cc @@ -127,20 +127,26 @@ std::size_t Standard210aChannelsAdapter::GetImgParamsFromData( Channels::img_params_t *img_params) { std::size_t i = 0; - Intrinsics in_left, in_right; Extrinsics ex_right_to_left; - - i += bytes::from_data(&in_left, data + i, version); - i += bytes::from_data(&in_right, data + i, version); - (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; - - i += bytes::from_data(&in_left, data + i, version); - i += bytes::from_data(&in_right, data + i, version); - (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; - - i += bytes::from_data(&ex_right_to_left, data + i, version); - (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; - (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i, version); + i += bytes::from_data(in_right.get(), data + i, version); + (*img_params)[{1280, 400}] = {true, in_left, in_right, ex_right_to_left}; + } + { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + i += bytes::from_data(in_left.get(), data + i, version); + i += bytes::from_data(in_right.get(), data + i, version); + (*img_params)[{2560, 800}] = {true, in_left, in_right, ex_right_to_left}; + } + { + i += bytes::from_data(&ex_right_to_left, data + i, version); + (*img_params)[{1280, 400}].ex_right_to_left = ex_right_to_left; + (*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left; + } return i; } @@ -151,14 +157,20 @@ std::size_t Standard210aChannelsAdapter::SetImgParamsToData( std::size_t i = 3; // skip id, size { - auto &¶ms = (*img_params).at({1280, 400}); - i += bytes::to_data(¶ms.in_left, data + i, version); - i += bytes::to_data(¶ms.in_right, data + i, version); + auto params = (*img_params).at({1280, 400}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i, version); + i += bytes::to_data(in_right.get(), data + i, version); } { - auto &¶ms = (*img_params).at({2560, 800}); - i += bytes::to_data(¶ms.in_left, data + i, version); - i += bytes::to_data(¶ms.in_right, data + i, version); + auto params = (*img_params).at({2560, 800}); + auto in_left = std::dynamic_pointer_cast(params.in_left); + auto in_right = std::dynamic_pointer_cast( + params.in_right); + i += bytes::to_data(in_left.get(), data + i, version); + i += bytes::to_data(in_right.get(), data + i, version); i += bytes::to_data(¶ms.ex_right_to_left, data + i, version); } diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index fb38c3e..9352c3c 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -113,7 +113,7 @@ bool DeviceWriter::WriteImuParams(const std::string &filepath) { namespace { -cv::FileStorage &operator<<(cv::FileStorage &fs, const Intrinsics &in) { +cv::FileStorage &operator<<(cv::FileStorage &fs, const IntrinsicsPinhole &in) { fs << "{" << "width" << in.width << "height" << in.height << "fx" << in.fx << "fy" << in.fy << "cx" << in.cx << "cy" << in.cy << "model" << in.model @@ -152,8 +152,10 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const Extrinsics &ex) { cv::FileStorage &operator<<( cv::FileStorage &fs, const device::img_params_t ¶ms) { fs << "{" - << "in_left" << params.in_left - << "in_right" << params.in_right + << "in_left" + << *std::dynamic_pointer_cast(params.in_left) + << "in_right" + << *std::dynamic_pointer_cast(params.in_right) << "ex_right_to_left" << params.ex_right_to_left << "}"; return fs; } @@ -241,7 +243,7 @@ namespace { void to_intrinsics( const std::uint16_t &width, const std::uint16_t &height, const std::uint8_t &model, const cv::Mat &M, const cv::Mat &D, - Intrinsics *in) { + IntrinsicsPinhole *in) { in->width = width; in->height = height; /* @@ -272,7 +274,7 @@ void to_extrinsics(const cv::Mat &R, const cv::Mat &T, Extrinsics *ex) { } } -void operator>>(const cv::FileNode &n, Intrinsics &in) { +void operator>>(const cv::FileNode &n, IntrinsicsPinhole &in) { n["width"] >> in.width; n["height"] >> in.height; n["fx"] >> in.fx; @@ -314,8 +316,12 @@ void operator>>(const cv::FileNode &n, Extrinsics &ex) { } void operator>>(const cv::FileNode &n, DeviceWriter::img_params_t ¶mas) { - n["in_left"] >> paramas.in_left; - n["in_right"] >> paramas.in_right; + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + paramas.in_left = in_left; + paramas.in_right = in_right; + n["in_left"] >> *in_left; + n["in_right"] >> *in_right; n["ex_right_to_left"] >> paramas.ex_right_to_left; } @@ -346,6 +352,10 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams( img_params_map_t img_params_map; if (fs["version"].isNone()) { + auto in_left = std::make_shared(); + auto in_right = std::make_shared(); + img_params_map[{752, 480}].in_left = in_left; + img_params_map[{752, 480}].in_right = in_right; if (fs["in_left"].isNone()) { std::uint16_t w = 752; std::uint16_t h = 480; @@ -366,13 +376,13 @@ DeviceWriter::img_params_map_t DeviceWriter::LoadImgParams( fs["T"] >> T; to_intrinsics( - w, h, m, M1, D1, &img_params_map[{752, 480}].in_left); + w, h, m, M1, D1, in_left.get()); to_intrinsics( - w, h, m, M2, D2, &img_params_map[{752, 480}].in_right); + w, h, m, M2, D2, in_right.get()); to_extrinsics(R, T, &img_params_map[{752, 480}].ex_right_to_left); } else { - fs["in_left"][0] >> img_params_map[{752, 480}].in_left; - fs["in_right"][0] >> img_params_map[{752, 480}].in_right; + fs["in_left"][0] >> *in_left; + fs["in_right"][0] >> *in_right; fs["ex_right_to_left"] >> img_params_map[{752, 480}].ex_right_to_left; } } else {