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

refactor(intrinsics): apply base intrinsics

Browse Source
This commit is contained in:
John Zhao 2019-01-04 13:29:06 +08:00
parent 938dc08654
commit 07570f330b
14 changed files with 143 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/mynteye/api/api.h
View File

@ -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<typename T>
T GetIntrinsics(const Stream &from) const;
template <typename T>
T GetIntrinsics(const Stream &stream) const;
/**
* Get the intrinsics base of stream.
*/
std::shared_ptr<IntrinsicsBase> GetIntrinsicsBase(const Stream &stream) const;
/**
* Get the extrinsics from one stream to another.
*/
@ -311,8 +315,9 @@ class MYNTEYE_API API {
};
template <typename T>
T API::GetIntrinsics(const Stream &from) const {
return device_->GetIntrinsics<T>(from);
T API::GetIntrinsics(const Stream &stream) const {
auto in = GetIntrinsicsBase(stream);
return *std::dynamic_pointer_cast<T>(in);
}
MYNTEYE_END_NAMESPACE

4
include/mynteye/device/callbacks.h
View File

@ -32,8 +32,8 @@ namespace device {
typedef struct ImgParams {
bool ok;
Intrinsics in_left;
Intrinsics in_right;
std::shared_ptr<IntrinsicsBase> in_left;
std::shared_ptr<IntrinsicsBase> in_right;
Extrinsics ex_right_to_left;
} img_params_t;

22
include/mynteye/device/device.h
View File

@ -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<typename T>
T GetIntrinsics(const Stream &from) const;
std::shared_ptr<IntrinsicsBase> 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<IntrinsicsBase> 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<IntrinsicsBase> &in);
/**
* Set the extrinsics from one stream to another.
*/
@ -332,7 +329,7 @@ class MYNTEYE_API Device {
std::map<Resolution, device::img_params_t> all_img_params_;
device::imu_params_t imu_params_;
std::map<Stream, Intrinsics> stream_intrinsics_;
std::map<Stream, std::shared_ptr<IntrinsicsBase>> stream_intrinsics_;
std::map<Stream, std::map<Stream, Extrinsics>> stream_from_extrinsics_;
std::shared_ptr<MotionIntrinsics> motion_intrinsics_;
@ -365,13 +362,6 @@ class MYNTEYE_API Device {
friend tools::DeviceWriter;
};
template <typename T>
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_

4
include/mynteye/types.h
View File

@ -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;

12
src/mynteye/api/api.cc
View File

@ -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<IntrinsicsPinhole>(in);
}
throw std::runtime_error("Intrinsics is not pinhole model"
", please use GetIntrinsicsBase() or GetIntrinsics<T>() instead.");
}
std::shared_ptr<IntrinsicsBase> API::GetIntrinsicsBase(
const Stream &stream) const {
return device_->GetIntrinsics(stream);
}

10
src/mynteye/api/processor/rectify_processor.cc
View File

@ -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<IntrinsicsPinhole>(in_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(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 =

4
src/mynteye/api/processor/rectify_processor.h
View File

@ -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> device_;
};

4
src/mynteye/device/channels.cc
View File

@ -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;

4
src/mynteye/device/channels.h
View File

@ -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,

9
src/mynteye/device/device.cc
View File

@ -240,7 +240,8 @@ std::string Device::GetInfo(const Info &info) const {
}
}
Intrinsics Device::GetIntrinsics(const Stream &stream) const {
std::shared_ptr<IntrinsicsBase> 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<IntrinsicsBase> 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<IntrinsicsBase> &in) {
stream_intrinsics_[stream] = in;
}

15
src/mynteye/device/standard/channels_adapter_s.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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 &&params = (*img_params).at({752, 480});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
auto params = (*img_params).at({752, 480});
auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(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(&params.ex_right_to_left, data + i, version);
// others

50
src/mynteye/device/standard2/channels_adapter_s2.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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 &&params = (*img_params).at({1280, 400});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
auto params = (*img_params).at({1280, 400});
auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
params.in_right);
i += bytes::to_data(in_left.get(), data + i, version);
i += bytes::to_data(in_right.get(), data + i, version);
}
{
auto &&params = (*img_params).at({2560, 800});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
auto params = (*img_params).at({2560, 800});
auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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(&params.ex_right_to_left, data + i, version);
}

50
src/mynteye/device/standard2/channels_adapter_s210a.cc
View File

@ -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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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 &&params = (*img_params).at({1280, 400});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
auto params = (*img_params).at({1280, 400});
auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
params.in_right);
i += bytes::to_data(in_left.get(), data + i, version);
i += bytes::to_data(in_right.get(), data + i, version);
}
{
auto &&params = (*img_params).at({2560, 800});
i += bytes::to_data(&params.in_left, data + i, version);
i += bytes::to_data(&params.in_right, data + i, version);
auto params = (*img_params).at({2560, 800});
auto in_left = std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left);
auto in_right = std::dynamic_pointer_cast<IntrinsicsPinhole>(
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(&params.ex_right_to_left, data + i, version);
}

32
tools/writer/device_writer.cc
View File

@ -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 &params) {
fs << "{"
<< "in_left" << params.in_left
<< "in_right" << params.in_right
<< "in_left"
<< *std::dynamic_pointer_cast<IntrinsicsPinhole>(params.in_left)
<< "in_right"
<< *std::dynamic_pointer_cast<IntrinsicsPinhole>(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 &paramas) {
n["in_left"] >> paramas.in_left;
n["in_right"] >> paramas.in_right;
auto in_left = std::make_shared<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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<IntrinsicsPinhole>();
auto in_right = std::make_shared<IntrinsicsPinhole>();
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 {