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

feat(channels): add img params new parser

Browse Source
This commit is contained in:
John Zhao 2019-01-05 22:04:59 +08:00
parent e539cb9fe0
commit c580f6e8dc
11 changed files with 329 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
include/mynteye/types.h
View File

@ -400,22 +400,33 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request);
* Camera calibration model.
*/
enum class CalibrationModel : std::uint8_t {
/** Unknow */
CALIB_MODEL_UNKNOW = 0,
/** Pinhole */
CALIB_MODEL_PINHOLE = 1,
PINHOLE = 0,
/** Equidistant: KANNALA_BRANDT */
CALIB_MODEL_KANNALA_BRANDT = 2,
// CALIB_MODEL_SCARAMUZZA,
// CALIB_MODEL_MEI,
KANNALA_BRANDT = 1,
/** Unknow */
UNKNOW
};
MYNTEYE_API const char *to_string(const CalibrationModel &model);
inline std::ostream &operator<<(std::ostream &os,
const CalibrationModel &model) {
return os << to_string(model);
}
struct MYNTEYE_API IntrinsicsBase {
IntrinsicsBase() {
calib_model = CalibrationModel::CALIB_MODEL_UNKNOW;
calib_model = CalibrationModel::UNKNOW;
}
virtual ~IntrinsicsBase() {}
/** The calibration model */
CalibrationModel calib_model;
/** The width of the image in pixels */
std::uint16_t width;
/** The height of the image in pixels */
std::uint16_t height;
};
/**
@ -424,12 +435,8 @@ struct MYNTEYE_API IntrinsicsBase {
*/
struct MYNTEYE_API IntrinsicsPinhole : public IntrinsicsBase {
IntrinsicsPinhole() {
calib_model = CalibrationModel::CALIB_MODEL_PINHOLE;
calib_model = CalibrationModel::PINHOLE;
}
/** The width of the image in pixels */
std::uint16_t width;
/** The height of the image in pixels */
std::uint16_t height;
/** The focal length of the image plane, as a multiple of pixel width */
double fx;
/** The focal length of the image plane, as a multiple of pixel height */
@ -458,12 +465,8 @@ using Intrinsics = IntrinsicsPinhole;
*/
struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
IntrinsicsEquidistant() {
calib_model = CalibrationModel::CALIB_MODEL_KANNALA_BRANDT;
calib_model = CalibrationModel::KANNALA_BRANDT;
}
/** The width of the image in pixels */
std::uint16_t width;
/** The height of the image in pixels */
std::uint16_t height;
/** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */
double coeffs[8];
};

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

@ -296,7 +296,7 @@ std::string API::GetInfo(const Info &info) const {
IntrinsicsPinhole API::GetIntrinsics(const Stream &stream) const {
auto in = GetIntrinsicsBase(stream);
if (in->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
if (in->calib_model == CalibrationModel::PINHOLE) {
return *std::dynamic_pointer_cast<IntrinsicsPinhole>(in);
}
throw std::runtime_error("Intrinsics is not pinhole model"

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

@ -43,13 +43,13 @@ std::string RectifyProcessor::Name() {
void RectifyProcessor::NotifyImageParamsChanged() {
auto in_left = device_->GetIntrinsics(Stream::LEFT);
auto in_right = device_->GetIntrinsics(Stream::RIGHT);
if (in_left->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
if (in_left->calib_model == CalibrationModel::PINHOLE) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right),
device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
} else if (in_left->calib_model ==
CalibrationModel::CALIB_MODEL_KANNALA_BRANDT) {
CalibrationModel::KANNALA_BRANDT) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right),

105
src/mynteye/device/channel/bytes.cc
View File

@ -13,6 +13,7 @@
// limitations under the License.
#include "mynteye/device/channel/bytes.h"
#include "mynteye/logger.h"
#include "mynteye/util/strings.h"
MYNTEYE_BEGIN_NAMESPACE
@ -29,15 +30,32 @@ std::string _from_data(const std::uint8_t *data, std::size_t count) {
// from types
std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) {
std::size_t from_data(IntrinsicsBase *in, const std::uint8_t *data,
bool get_size) {
switch (in->calib_model) {
case CalibrationModel::PINHOLE:
return from_data(dynamic_cast<IntrinsicsPinhole *>(in), data,
get_size);
case CalibrationModel::KANNALA_BRANDT:
return from_data(dynamic_cast<IntrinsicsEquidistant *>(in), data,
get_size);
default:
LOG(FATAL) << "Unknown calib model: " << in->calib_model;
}
}
std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
bool get_size) {
std::size_t i = 0;
// width, 2
in->width = _from_data<std::uint16_t>(data + i);
i += 2;
// height, 2
in->height = _from_data<std::uint16_t>(data + i);
i += 2;
if (get_size) {
// width, 2
in->width = _from_data<std::uint16_t>(data + i);
i += 2;
// height, 2
in->height = _from_data<std::uint16_t>(data + i);
i += 2;
}
// fx, 8
in->fx = _from_data<double>(data + i);
i += 8;
@ -62,6 +80,27 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data) {
return i;
}
std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
bool get_size) {
std::size_t i = 0;
if (get_size) {
// width, 2
in->width = _from_data<std::uint16_t>(data + i);
i += 2;
// height, 2
in->height = _from_data<std::uint16_t>(data + i);
i += 2;
}
// coeffs, 64
for (std::size_t j = 0; j < 8; j++) {
in->coeffs[j] = _from_data<double>(data + i + j * 8);
}
i += 64;
return i;
}
std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
std::size_t i = 0;
@ -122,15 +161,32 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) {
// to types
std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) {
std::size_t to_data(const IntrinsicsBase *in, std::uint8_t *data,
bool set_size) {
switch (in->calib_model) {
case CalibrationModel::PINHOLE:
return to_data(dynamic_cast<const IntrinsicsPinhole *>(in), data,
set_size);
case CalibrationModel::KANNALA_BRANDT:
return to_data(dynamic_cast<const IntrinsicsEquidistant *>(in), data,
set_size);
default:
LOG(FATAL) << "Unknown calib model: " << in->calib_model;
}
}
std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
bool set_size) {
std::size_t i = 0;
// width, 2
_to_data(in->width, data + i);
i += 2;
// height, 2
_to_data(in->height, data + i);
i += 2;
if (set_size) {
// width, 2
_to_data(in->width, data + i);
i += 2;
// height, 2
_to_data(in->height, data + i);
i += 2;
}
// fx, 8
_to_data(in->fx, data + i);
i += 8;
@ -155,6 +211,27 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data) {
return i;
}
std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
bool set_size) {
std::size_t i = 0;
if (set_size) {
// width, 2
_to_data(in->width, data + i);
i += 2;
// height, 2
_to_data(in->height, data + i);
i += 2;
}
// coeffs, 64
for (std::size_t j = 0; j < 8; j++) {
_to_data(in->coeffs[j], data + i + j * 8);
}
i += 64;
return i;
}
std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
std::size_t i = 0;

14
src/mynteye/device/channel/bytes.h
View File

@ -48,7 +48,12 @@ std::string _from_data(const std::uint8_t *data, std::size_t count);
// from types
std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data);
std::size_t from_data(IntrinsicsBase *in, const std::uint8_t *data,
bool get_size);
std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
bool get_size);
std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
bool get_size);
std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data);
@ -76,7 +81,12 @@ std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count);
// to types
std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data);
std::size_t to_data(const IntrinsicsBase *in, std::uint8_t *data,
bool set_size);
std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
bool set_size);
std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
bool set_size);
std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data);

69
src/mynteye/device/channel/channels.cc
View File

@ -97,26 +97,6 @@ int XuHalfDuplexId(Option option) {
}
}
void CheckSpecVersion(const Version *spec_version) {
if (spec_version == nullptr) {
LOG(FATAL) << "Spec version must be specified";
}
std::vector<std::string> spec_versions{"1.0", "1.1"};
for (auto &&spec_ver : spec_versions) {
if (*spec_version == Version(spec_ver)) {
return; // supported
}
}
std::ostringstream ss;
std::copy(
spec_versions.begin(), spec_versions.end(),
std::ostream_iterator<std::string>(ss, ","));
LOG(FATAL) << "Spec version " << spec_version->to_string()
<< " not supported, must in [" << ss.str() << "]";
}
} // namespace
Channels::Channels(const std::shared_ptr<uvc::device> &device,
@ -431,8 +411,7 @@ void Channels::StopImuTracking() {
}
bool Channels::GetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version) {
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) {
if (info == nullptr && img_params == nullptr && imu_params == nullptr) {
LOG(WARNING) << "Files are not provided to get";
return false;
@ -475,7 +454,6 @@ bool Channels::GetFiles(
return false;
}
Version *spec_ver = spec_version;
std::size_t i = 3;
std::size_t end = 3 + size;
while (i < end) {
@ -486,25 +464,24 @@ bool Channels::GetFiles(
i += 3;
switch (file_id) {
case FID_DEVICE_INFO: {
auto &&n = file_channel_.GetDeviceInfoFromData(data + i, info);
auto &&n = file_channel_.GetDeviceInfoFromData(
data + i, file_size, info);
CHECK_EQ(n, file_size)
<< "The firmware not support getting device info, you could "
"upgrade to latest";
spec_ver = &info->spec_version;
CheckSpecVersion(spec_ver);
} break;
case FID_IMG_PARAMS: {
if (file_size > 0) {
CheckSpecVersion(spec_ver);
auto &&n = file_channel_.GetImgParamsFromData(data + i, img_params);
auto &&n = file_channel_.GetImgParamsFromData(
data + i, file_size, img_params);
CHECK_EQ(n, file_size);
}
} break;
case FID_IMU_PARAMS: {
imu_params->ok = file_size > 0;
if (imu_params->ok) {
CheckSpecVersion(spec_ver);
auto &&n = file_channel_.GetImuParamsFromData(data + i, imu_params);
auto &&n = file_channel_.GetImuParamsFromData(
data + i, file_size, imu_params);
CHECK_EQ(n, file_size);
}
} break;
@ -523,17 +500,11 @@ bool Channels::GetFiles(
}
bool Channels::SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version) {
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) {
if (info == nullptr && img_params == nullptr && imu_params == nullptr) {
LOG(WARNING) << "Files are not provided to set";
return false;
}
Version *spec_ver = spec_version;
if (spec_ver == nullptr && info != nullptr) {
spec_ver = &info->spec_version;
}
CheckSpecVersion(spec_ver);
std::uint8_t data[2000]{};
@ -542,16 +513,28 @@ bool Channels::SetFiles(
std::uint16_t size = 0;
if (info != nullptr) {
header[0] = true;
size += file_channel_.SetDeviceInfoToData(info, data + 3 + size);
auto n = file_channel_.SetDeviceInfoToData(info, data + 3 + size);
if (n > 0) {
header[0] = true;
size += n;
}
}
if (img_params != nullptr) {
header[1] = true;
size += file_channel_.SetImgParamsToData(img_params, data + 3 + size);
auto n = file_channel_.SetImgParamsToData(img_params, data + 3 + size);
if (n > 0) {
header[1] = true;
size += n;
}
}
if (imu_params != nullptr) {
header[2] = true;
size += file_channel_.SetImuParamsToData(imu_params, data + 3 + size);
auto n = file_channel_.SetImuParamsToData(imu_params, data + 3 + size);
if (n > 0) {
header[2] = true;
size += n;
}
}
if (size + 3 > 2000) {
LOG(FATAL) << "SetFiles failed, data is too large: " << (size + 3);
}
data[0] = static_cast<std::uint8_t>(header.to_ulong());

6
src/mynteye/device/channel/channels.h
View File

@ -82,11 +82,9 @@ class MYNTEYE_API Channels {
void StopImuTracking();
bool GetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version = nullptr);
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
bool SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version = nullptr);
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
private:
bool PuControlRange(

179
src/mynteye/device/channel/file_channel.cc
View File

@ -29,8 +29,9 @@ FileChannel::~FileChannel() {
}
std::size_t FileChannel::GetDeviceInfoFromData(
const std::uint8_t *data, device_info_t *info) {
auto n = dev_info_parser_->GetFromData(data, info);
const std::uint8_t *data, const std::uint16_t &data_size,
device_info_t *info) {
auto n = dev_info_parser_->GetFromData(data, data_size, info);
auto spec_version = info->spec_version;
img_params_parser_->SetSpecVersion(spec_version);
imu_params_parser_->SetSpecVersion(spec_version);
@ -46,9 +47,10 @@ std::size_t FileChannel::SetDeviceInfoToData(
}
std::size_t FileChannel::GetImgParamsFromData(
const std::uint8_t *data, img_params_t *img_params) {
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) {
CHECK_NOTNULL(img_params_parser_);
return img_params_parser_->GetFromData(data, img_params);
return img_params_parser_->GetFromData(data, data_size, img_params);
}
std::size_t FileChannel::SetImgParamsToData(
@ -58,8 +60,9 @@ std::size_t FileChannel::SetImgParamsToData(
}
std::size_t FileChannel::GetImuParamsFromData(
const std::uint8_t *data, imu_params_t *imu_params) {
return imu_params_parser_->GetFromData(data, imu_params);
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) {
return imu_params_parser_->GetFromData(data, data_size, imu_params);
}
std::size_t FileChannel::SetImuParamsToData(
@ -76,7 +79,8 @@ DeviceInfoParser::~DeviceInfoParser() {
}
std::size_t DeviceInfoParser::GetFromData(
const std::uint8_t *data, device_info_t *info) const {
const std::uint8_t *data, const std::uint16_t &data_size,
device_info_t *info) const {
std::size_t i = 4; // skip vid, pid
// name, 16
info->name = bytes::_from_data(data + i, 16);
@ -111,6 +115,7 @@ std::size_t DeviceInfoParser::GetFromData(
// get other infos according to spec_version
MYNTEYE_UNUSED(data_size)
return i;
}
@ -169,17 +174,18 @@ ImgParamsParser::~ImgParamsParser() {
}
std::size_t ImgParamsParser::GetFromData(
const std::uint8_t *data, img_params_t *img_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const {
if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) {
// get img params without version header
if (spec_version_ == Version(1, 0)) {
return GetFromData_v1_0(data, img_params);
return GetFromData_v1_0(data, data_size, img_params);
} else {
return GetFromData_v1_1(data, img_params);
return GetFromData_v1_1(data, data_size, img_params);
}
} else {
// get img params with version header
return GetFromData_new(data, img_params);
return GetFromData_new(data, data_size, img_params);
}
}
@ -199,19 +205,21 @@ std::size_t ImgParamsParser::SetToData(
}
std::size_t ImgParamsParser::GetFromData_v1_0(
const std::uint8_t *data, img_params_t *img_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const {
std::size_t i = 0;
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.get(), data + i);
i += bytes::from_data(in_right.get(), data + i);
i += bytes::from_data(in_left.get(), data + i, true);
i += bytes::from_data(in_right.get(), data + i, true);
i += bytes::from_data(&ex_right_to_left, data + i);
(*img_params)[{752, 480}] = {true, spec_version_.to_string(),
in_left, in_right, ex_right_to_left};
MYNTEYE_UNUSED(data_size)
return i;
}
@ -220,10 +228,8 @@ std::size_t ImgParamsParser::SetToData_v1_0(
std::size_t i = 3; // skip id, size
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);
i += bytes::to_data(in_right.get(), data + i);
i += bytes::to_data(params.in_left.get(), data + i, true);
i += bytes::to_data(params.in_right.get(), data + i, true);
i += bytes::to_data(&params.ex_right_to_left, data + i);
// others
@ -235,23 +241,24 @@ std::size_t ImgParamsParser::SetToData_v1_0(
}
std::size_t ImgParamsParser::GetFromData_v1_1(
const std::uint8_t *data, img_params_t *img_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const {
std::size_t i = 0;
Extrinsics 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);
i += bytes::from_data(in_right.get(), data + i);
i += bytes::from_data(in_left.get(), data + i, true);
i += bytes::from_data(in_right.get(), data + i, true);
(*img_params)[{1280, 400}] = {true, spec_version_.to_string(),
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);
i += bytes::from_data(in_right.get(), data + i);
i += bytes::from_data(in_left.get(), data + i, true);
i += bytes::from_data(in_right.get(), data + i, true);
(*img_params)[{2560, 800}] = {true, spec_version_.to_string(),
in_left, in_right, ex_right_to_left};
}
@ -261,6 +268,7 @@ std::size_t ImgParamsParser::GetFromData_v1_1(
(*img_params)[{2560, 800}].ex_right_to_left = ex_right_to_left;
}
MYNTEYE_UNUSED(data_size)
return i;
}
@ -270,19 +278,13 @@ std::size_t ImgParamsParser::SetToData_v1_1(
{
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);
i += bytes::to_data(in_right.get(), data + i);
i += bytes::to_data(params.in_left.get(), data + i, true);
i += bytes::to_data(params.in_right.get(), data + i, true);
}
{
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);
i += bytes::to_data(in_right.get(), data + i);
i += bytes::to_data(params.in_left.get(), data + i, true);
i += bytes::to_data(params.in_right.get(), data + i, true);
i += bytes::to_data(&params.ex_right_to_left, data + i);
}
@ -295,12 +297,102 @@ std::size_t ImgParamsParser::SetToData_v1_1(
}
std::size_t ImgParamsParser::GetFromData_new(
const std::uint8_t *data, img_params_t *img_params) const {
return 0;
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const {
std::size_t i = 0;
// version, 2
Version version(data[i], data[i + 1]);
i += 2;
// get img params according to version
if (version == Version(1, 2)) { // v1.2
for (; i < data_size;) {
// calib_model, 1
auto calib_model = static_cast<CalibrationModel>(data[i]);
i += 1;
// width, 2
auto width = bytes::_from_data<std::uint16_t>(data + i);
i += 2;
// height, 2
auto height = bytes::_from_data<std::uint16_t>(data + i);
i += 2;
// calib_params
std::shared_ptr<IntrinsicsBase> in_left, in_right;
Extrinsics ex_right_to_left;
switch (calib_model) {
case CalibrationModel::PINHOLE:
in_left = std::make_shared<IntrinsicsPinhole>();
in_right = std::make_shared<IntrinsicsPinhole>();
break;
case CalibrationModel::KANNALA_BRANDT:
in_left = std::make_shared<IntrinsicsEquidistant>();
in_right = std::make_shared<IntrinsicsEquidistant>();
break;
default:
LOG(FATAL) << "Could not get img params as unknown calib model"
", please use latest SDK.";
}
i += bytes::from_data(in_left.get(), data + i, false);
i += bytes::from_data(in_right.get(), data + i, false);
i += bytes::from_data(&ex_right_to_left, data + i);
in_left->calib_model = calib_model;
in_left->width = width;
in_left->height = height;
in_right->calib_model = calib_model;
in_right->width = width;
in_right->height = height;
(*img_params)[{width, height}] = {true, version.to_string(),
in_left, in_right, ex_right_to_left};
}
} else {
LOG(FATAL) << "Could not get img params of version "
<< version.to_string() << ", please use latest SDK.";
}
MYNTEYE_UNUSED(data_size)
return i;
}
std::size_t ImgParamsParser::SetToData_new(
const img_params_t *img_params, std::uint8_t *data) const {
return 0;
if (img_params->empty()) {
return 0;
}
std::size_t i = 3; // skip id, size
// version, 2
Version version(img_params->begin()->second.version);
data[i] = version.major();
data[i + 1] = version.minor();
i += 2;
// set img params according to version
if (version == Version(1, 2)) { // v1.2
for (auto &&entry : *img_params) {
auto &&params = entry.second;
// calib_model, 1
data[i] = static_cast<std::uint8_t>(params.in_left->calib_model);
i += 1;
// width, 2
bytes::_to_data(params.in_left->width, data + i);
i += 2;
// height, 2
bytes::_to_data(params.in_left->height, data + i);
i += 2;
// calib_params
i += bytes::to_data(params.in_left.get(), data + i, false);
i += bytes::to_data(params.in_right.get(), data + i, false);
i += bytes::to_data(&params.ex_right_to_left, data + i);
}
} else {
LOG(FATAL) << "Could not set img params of version "
<< version.to_string() << ", please use latest SDK.";
}
// others
std::size_t size = i - 3;
data[0] = FID_IMG_PARAMS;
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
return size + 3;
}
// ImuParamsParser
@ -312,13 +404,14 @@ ImuParamsParser::~ImuParamsParser() {
}
std::size_t ImuParamsParser::GetFromData(
const std::uint8_t *data, imu_params_t *imu_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const {
if (spec_version_ == Version(1, 0) || spec_version_ == Version(1, 1)) {
// get imu params without version header
return GetFromData_old(data, imu_params);
return GetFromData_old(data, data_size, imu_params);
} else {
// get imu params with version header
return GetFromData_new(data, imu_params);
return GetFromData_new(data, data_size, imu_params);
}
}
@ -334,11 +427,13 @@ std::size_t ImuParamsParser::SetToData(
}
std::size_t ImuParamsParser::GetFromData_old(
const std::uint8_t *data, imu_params_t *imu_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const {
std::size_t i = 0;
i += bytes::from_data(&imu_params->in_accel, data + i);
i += bytes::from_data(&imu_params->in_gyro, data + i);
i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
MYNTEYE_UNUSED(data_size)
return i;
}
@ -357,7 +452,8 @@ std::size_t ImuParamsParser::SetToData_old(
}
std::size_t ImuParamsParser::GetFromData_new(
const std::uint8_t *data, imu_params_t *imu_params) const {
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const {
std::size_t i = 0;
// version, 2
Version version(data[i], data[i + 1]);
@ -372,6 +468,7 @@ std::size_t ImuParamsParser::GetFromData_new(
LOG(FATAL) << "Could not get imu params of version "
<< version.to_string() << ", please use latest SDK.";
}
MYNTEYE_UNUSED(data_size)
return i;
}

33
src/mynteye/device/channel/file_channel.h
View File

@ -39,17 +39,20 @@ class FileChannel {
~FileChannel();
std::size_t GetDeviceInfoFromData(
const std::uint8_t *data, device_info_t *info);
const std::uint8_t *data, const std::uint16_t &data_size,
device_info_t *info);
std::size_t SetDeviceInfoToData(
const device_info_t *info, std::uint8_t *data);
std::size_t GetImgParamsFromData(
const std::uint8_t *data, img_params_t *img_params);
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params);
std::size_t SetImgParamsToData(
const img_params_t *img_params, std::uint8_t *data);
std::size_t GetImuParamsFromData(
const std::uint8_t *data, imu_params_t *imu_params);
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params);
std::size_t SetImuParamsToData(
const imu_params_t *imu_params, std::uint8_t *data);
@ -67,7 +70,8 @@ class DeviceInfoParser {
~DeviceInfoParser();
std::size_t GetFromData(
const std::uint8_t *data, device_info_t *info) const;
const std::uint8_t *data, const std::uint16_t &data_size,
device_info_t *info) const;
std::size_t SetToData(
const device_info_t *info, std::uint8_t *data) const;
};
@ -84,22 +88,26 @@ class ImgParamsParser {
}
std::size_t GetFromData(
const std::uint8_t *data, img_params_t *img_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const;
std::size_t SetToData(
const img_params_t *img_params, std::uint8_t *data) const;
std::size_t GetFromData_v1_0(
const std::uint8_t *data, img_params_t *img_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const;
std::size_t SetToData_v1_0(
const img_params_t *img_params, std::uint8_t *data) const;
std::size_t GetFromData_v1_1(
const std::uint8_t *data, img_params_t *img_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const;
std::size_t SetToData_v1_1(
const img_params_t *img_params, std::uint8_t *data) const;
std::size_t GetFromData_new(
const std::uint8_t *data, img_params_t *img_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
img_params_t *img_params) const;
std::size_t SetToData_new(
const img_params_t *img_params, std::uint8_t *data) const;
@ -119,17 +127,20 @@ class ImuParamsParser {
}
std::size_t GetFromData(
const std::uint8_t *data, imu_params_t *imu_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const;
std::size_t SetToData(
const imu_params_t *imu_params, std::uint8_t *data) const;
std::size_t GetFromData_old(
const std::uint8_t *data, imu_params_t *imu_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const;
std::size_t SetToData_old(
const imu_params_t *imu_params, std::uint8_t *data) const;
std::size_t GetFromData_new(
const std::uint8_t *data, imu_params_t *imu_params) const;
const std::uint8_t *data, const std::uint16_t &data_size,
imu_params_t *imu_params) const;
std::size_t SetToData_new(
const imu_params_t *imu_params, std::uint8_t *data) const;

13
src/mynteye/types.cc
View File

@ -188,6 +188,19 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) {
<< ", format: " << request.format << ", fps: " << request.fps;
}
const char *to_string(const CalibrationModel &model) {
#define CASE(X) \
case CalibrationModel::X: \
return "CalibrationModel::" #X;
switch (model) {
CASE(PINHOLE)
CASE(KANNALA_BRANDT)
default:
return "CalibrationModel::UNKNOWN";
}
#undef CASE
}
std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) {
os << "pinhole, " << FULL_PRECISION
<< "width: " << in.width << ", height: " << in.height

6
tools/writer/device_writer.cc
View File

@ -66,8 +66,7 @@ bool DeviceWriter::WriteImgParams(const img_params_map_t &img_params_map) {
auto &&channels = device_->channels();
auto &&dev_info = device_->GetInfo();
if (channels->SetFiles(
nullptr, const_cast<img_params_map_t *>(&img_params_map), nullptr,
&dev_info->spec_version)) {
nullptr, const_cast<img_params_map_t *>(&img_params_map), nullptr)) {
LOG(INFO) << "Write img params success";
std::map<Resolution, device::img_params_t>::const_iterator it;
for (it = img_params_map.begin(); it != img_params_map.end(); it++) {
@ -93,8 +92,7 @@ bool DeviceWriter::WriteImuParams(const imu_params_t &params) {
auto &&channels = device_->channels();
auto &&dev_info = device_->GetInfo();
if (channels->SetFiles(
nullptr, nullptr, const_cast<imu_params_t *>(&params),
&dev_info->spec_version)) {
nullptr, nullptr, const_cast<imu_params_t *>(&params))) {
LOG(INFO) << "Write imu params success";
LOG(INFO) << "Imu intrinsics accel: {" << params.in_accel << "}";
LOG(INFO) << "Imu intrinsics gyro: {" << params.in_gyro << "}";