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

Complete read/write files

Browse Source
This commit is contained in:
John Zhao 2018-04-19 17:23:02 +08:00
parent e50d9cec9b
commit e04a05f6f9
7 changed files with 244 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
doc/zh-Hans/spec_control_channel.md
View File

@ -48,6 +48,6 @@
| File | ID | Max Size |
| :--- | :- | :------- |
| 硬件信息 | 0 | 250 |
| 图像参数 | 1 | 250 |
| IMU 参数 | 2 | 500 |
| 硬件信息 | 1 | 250 |
| 图像参数 | 2 | 250 |
| IMU 参数 | 4 | 500 |

12
include/mynteye/types.h
View File

@ -269,6 +269,8 @@ struct MYNTEYE_API ImgIntrinsics {
double coeffs[5];
};
std::ostream &operator<<(std::ostream &os, const ImgIntrinsics &in);
/**
* @ingroup calibration
* IMU sensor intrinsics: scale, drift and variances.
@ -289,6 +291,8 @@ struct MYNTEYE_API ImuSensorIntrinsics {
double bias[3];
};
std::ostream &operator<<(std::ostream &os, const ImuSensorIntrinsics &in);
/**
* @ingroup calibration
* IMU intrinsics, including accelerometer and gyroscope.
@ -298,6 +302,8 @@ struct MYNTEYE_API ImuIntrinsics {
ImuSensorIntrinsics gyro;
};
std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in);
/**
* @ingroup calibration
* Extrinsics, represent how the different datas are connected.
@ -307,6 +313,8 @@ struct MYNTEYE_API Extrinsics {
double translation[3]; /**< translation vector */
};
std::ostream &operator<<(std::ostream &os, const Extrinsics &ex);
/**
* @ingroup calibration
* Image extrinsics.
@ -315,6 +323,8 @@ struct MYNTEYE_API ImgExtrinsics {
Extrinsics left_to_right;
};
std::ostream &operator<<(std::ostream &os, const ImgExtrinsics &ex);
/**
* @ingroup calibration
* IMU extrinsics.
@ -323,6 +333,8 @@ struct MYNTEYE_API ImuExtrinsics {
Extrinsics left_to_imu;
};
std::ostream &operator<<(std::ostream &os, const ImuExtrinsics &ex);
/**
* @defgroup datatypes Datatypes
* @brief Public data types.

44
src/device/device.cc
View File

@ -424,21 +424,54 @@ void Device::ReadAllInfos() {
Channels::img_params_t img_params;
Channels::imu_params_t imu_params;
if (!channels_->GetFiles(device_info_.get(), &img_params, &imu_params)) {
// LOG(FATAL) << "Read device infos failed :(";
LOG(FATAL) << "Read device infos failed :(";
}
VLOG(2) << "Device name: " << device_info_->name
<< ", serial_number: " << device_info_->serial_number
<< ", firmware_version: "
<< device_info_->firmware_version.to_string()
<< ", hardware_version: "
<< device_info_->hardware_version.to_string()
<< ", spec_version: " << device_info_->spec_version.to_string()
<< ", lens_type: " << device_info_->lens_type.to_string()
<< ", imu_type: " << device_info_->imu_type.to_string()
<< ", nominal_baseline: " << device_info_->nominal_baseline;
device_info_->name = uvc::get_name(*device_);
if (img_params.ok) {
img_intrinsics_ = img_params.in;
img_extrinsics_ = img_params.ex;
VLOG(2) << "ImgIntrinsics " << img_intrinsics_;
VLOG(2) << "ImgExtrinsics " << img_extrinsics_;
} else {
LOG(WARNING) << "Img intrinsics & extrinsics not exist";
}
if (imu_params.ok) {
imu_intrinsics_ = imu_params.in;
imu_extrinsics_ = imu_params.ex;
VLOG(2) << "ImuIntrinsics " << imu_intrinsics_;
VLOG(2) << "ImuExtrinsics " << imu_extrinsics_;
} else {
LOG(WARNING) << "Imu intrinsics & extrinsics not exist";
}
}
void Device::WriteDeviceInfo(const DeviceInfo &device_info) {
CHECK_NOTNULL(channels_);
DeviceInfo info = device_info;
if (channels_->SetFiles(&info, nullptr, nullptr)) {
device_info_->lens_type = info.lens_type;
device_info_->imu_type = info.imu_type;
device_info_->nominal_baseline = info.nominal_baseline;
}
}
void Device::WriteImgParams(
const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics) {
CHECK_NOTNULL(channels_);
Channels::img_params_t img_params{intrinsics, extrinsics};
if (channels_->SetFiles(nullptr, &img_params, nullptr)) {
Channels::img_params_t img_params{false, intrinsics, extrinsics};
if (channels_->SetFiles(
nullptr, &img_params, nullptr, &device_info_->spec_version)) {
img_intrinsics_ = intrinsics;
img_extrinsics_ = extrinsics;
}
@ -447,8 +480,9 @@ void Device::WriteImgParams(
void Device::WriteImuParams(
const ImuIntrinsics &intrinsics, const ImuExtrinsics &extrinsics) {
CHECK_NOTNULL(channels_);
Channels::imu_params_t imu_params{intrinsics, extrinsics};
if (channels_->SetFiles(nullptr, nullptr, &imu_params)) {
Channels::imu_params_t imu_params{false, intrinsics, extrinsics};
if (channels_->SetFiles(
nullptr, nullptr, &imu_params, &device_info_->spec_version)) {
imu_intrinsics_ = intrinsics;
imu_extrinsics_ = extrinsics;
}

1
src/device/device.h
View File

@ -140,6 +140,7 @@ class Device {
void ReadAllInfos();
void WriteDeviceInfo(const DeviceInfo &device_info);
void WriteImgParams(
const ImgIntrinsics &intrinsics, const ImgExtrinsics &extrinsics);
void WriteImuParams(

157
src/internal/channels.cc
View File

@ -9,6 +9,8 @@
#include <sstream>
#include <stdexcept>
#include "internal/strings.h"
MYNTEYE_BEGIN_NAMESPACE
namespace {
@ -57,6 +59,26 @@ 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"};
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 be {" << ss.str() << "}";
}
} // namespace
Channels::Channels(std::shared_ptr<uvc::device> device)
@ -316,27 +338,24 @@ T _from_data(const std::uint8_t *data) {
return value;
}
template <>
std::string _from_data(const std::uint8_t *data) {
return std::string(reinterpret_cast<const char *>(data));
}
template <>
double _from_data(const std::uint8_t *data) {
return *(reinterpret_cast<const double *>(data));
}
// std::string _from_data(const std::uint8_t *data, std::size_t n) {
// return std::string(reinterpret_cast<const char *>(data), n);
// }
std::string _from_data(const std::uint8_t *data, std::size_t count) {
std::string s(reinterpret_cast<const char *>(data), count);
strings::trim(s);
return s;
}
std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) {
std::size_t i = 0;
std::size_t i = 4; // skip vid, pid
// name, 16
info->name = _from_data<std::string>(data + i);
info->name = _from_data(data + i, 16);
i += 16;
// serial_number, 16
info->serial_number = _from_data<std::string>(data + i);
info->serial_number = _from_data(data + i, 16);
i += 16;
// firmware_version, 2
info->firmware_version.set_major(data[i]);
@ -362,11 +381,13 @@ std::size_t from_data(Channels::device_info_t *info, const std::uint8_t *data) {
// nominal_baseline, 2
info->nominal_baseline = _from_data<std::uint16_t>(data + i);
i += 2;
return i;
}
std::size_t from_data(
Channels::img_params_t *img_params, const std::uint8_t *data) {
Channels::img_params_t *img_params, const std::uint8_t *data,
const Version *spec_version) {
std::size_t i = 0;
auto &&in = img_params->in;
@ -401,7 +422,7 @@ std::size_t from_data(
// rotation
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
ex.rotation[j][k] = _from_data<double>(data + i + j * 3 + k);
ex.rotation[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -411,25 +432,27 @@ std::size_t from_data(
}
i += 24;
UNUSED(spec_version)
return i;
}
std::size_t from_data(
Channels::imu_params_t *imu_params, const std::uint8_t *data) {
Channels::imu_params_t *imu_params, const std::uint8_t *data,
const Version *spec_version) {
std::size_t i = 0;
auto &&in = imu_params->in;
// acc_scale
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
in.accel.scale[j][k] = _from_data<double>(data + i + j * 3 + k);
in.accel.scale[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
// gyro_scale
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
in.gyro.scale[j][k] = _from_data<double>(data + i + j * 3 + k);
in.gyro.scale[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -468,7 +491,7 @@ std::size_t from_data(
// rotation
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
ex.rotation[j][k] = _from_data<double>(data + i + j * 3 + k);
ex.rotation[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -478,14 +501,15 @@ std::size_t from_data(
}
i += 24;
UNUSED(spec_version)
return i;
}
} // namespace
bool Channels::GetFiles(
device_info_t *info, img_params_t *img_params,
imu_params_t *imu_params) const {
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version) const {
if (info == nullptr && img_params == nullptr && imu_params == nullptr) {
LOG(WARNING) << "Files are not provided to get";
return false;
@ -501,21 +525,22 @@ bool Channels::GetFiles(
header[2] = (imu_params != nullptr);
data[0] = static_cast<std::uint8_t>(header.to_ulong());
// VLOG(2) << "GetFiles header: 0x" << std::hex << std::uppercase
// << std::setw(2) << std::setfill('0') << static_cast<int>(data[0]);
VLOG(2) << "GetFiles header: 0x" << std::hex << std::uppercase << std::setw(2)
<< std::setfill('0') << static_cast<int>(data[0]);
if (!XuFileQuery(uvc::XU_QUERY_SET, 2000, data)) {
LOG(WARNING) << "GetFiles failed";
return false;
}
if (XuFileQuery(uvc::XU_QUERY_GET, 2000, data)) {
VLOG(2) << "GetFiles success";
// header = std::bitset<8>(data[0]);
std::uint16_t size = _from_data<std::uint16_t>(data + 1);
std::uint8_t checksum = data[3 + size];
VLOG(2) << "GetFiles data size: " << size << ", checksum: 0x" << std::hex
<< std::setw(2) << std::setfill('0') << static_cast<int>(checksum);
std::uint8_t checksum_now = 0;
for (std::size_t i = 4, n = 4 + size; i < n; i++) {
for (std::size_t i = 3, n = 3 + size; i < n; i++) {
checksum_now = (checksum_now ^ data[i]);
}
if (checksum != checksum_now) {
@ -527,21 +552,34 @@ bool Channels::GetFiles(
return false;
}
Version *spec_ver = spec_version;
std::size_t i = 3;
std::size_t end = 3 + size;
while (i < end) {
std::uint8_t file_id = *(data + i);
std::uint16_t file_size = _from_data<std::uint16_t>(data + i + 1);
VLOG(2) << "GetFiles id: " << static_cast<int>(file_id)
<< ", size: " << file_size;
i += 3;
switch (file_id) {
case FID_DEVICE_INFO: {
CHECK_EQ(from_data(info, data + i), file_size);
spec_ver = &info->spec_version;
CheckSpecVersion(spec_ver);
} break;
case FID_IMG_PARAMS: {
CHECK_EQ(from_data(img_params, data + i), file_size);
img_params->ok = file_size > 0;
if (img_params->ok) {
CheckSpecVersion(spec_ver);
CHECK_EQ(from_data(img_params, data + i, spec_ver), file_size);
}
} break;
case FID_IMU_PARAMS: {
CHECK_EQ(from_data(imu_params, data + i), file_size);
imu_params->ok = file_size > 0;
if (imu_params->ok) {
CheckSpecVersion(spec_ver);
CHECK_EQ(from_data(imu_params, data + i, spec_ver), file_size);
}
} break;
default:
LOG(FATAL) << "Unsupported file id: " << file_id;
@ -549,6 +587,7 @@ bool Channels::GetFiles(
i += file_size;
}
VLOG(2) << "GetFiles success";
return true;
} else {
LOG(WARNING) << "GetFiles failed";
@ -562,32 +601,36 @@ template <typename T>
std::size_t _to_data(T value, std::uint8_t *data) {
std::size_t size = sizeof(T) / sizeof(std::uint8_t);
for (std::size_t i = 0; i < size; i++) {
data[i] =
static_cast<std::uint8_t>((value >> (8 * (size - i - 1))) && 0xFF);
data[i] = static_cast<std::uint8_t>((value >> (8 * (size - i - 1))) & 0xFF);
}
return size;
}
template <>
std::size_t _to_data(double value, std::uint8_t *data) {
std::copy(data, data + 8, reinterpret_cast<std::uint8_t *>(&value));
std::uint8_t *val = reinterpret_cast<std::uint8_t *>(&value);
std::copy(val, val + 8, data);
return 8;
}
template <>
std::size_t _to_data(std::string value, std::uint8_t *data) {
std::size_t _to_data(std::string value, std::uint8_t *data, std::size_t count) {
std::copy(value.begin(), value.end(), data);
data[value.size()] = '\0';
return value.size() + 1;
for (std::size_t i = value.size(); i < count; i++) {
data[i] = ' ';
}
return count;
}
std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) {
std::size_t i = 3;
std::size_t to_data(
const Channels::device_info_t *info, std::uint8_t *data,
const Version *spec_version) {
std::size_t i = 3; // skip id, size
i += 4; // skip vid, pid
// name, 16
_to_data(info->name, data + i);
_to_data(info->name, data + i, 16);
i += 16;
// serial_number, 16
_to_data(info->serial_number, data + i);
_to_data(info->serial_number, data + i, 16);
i += 16;
// firmware_version, 2
data[i] = info->firmware_version.major();
@ -614,6 +657,9 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) {
// nominal_baseline, 2
_to_data(info->nominal_baseline, data + i);
i += 2;
UNUSED(spec_version)
// others
std::size_t size = i - 3;
data[0] = Channels::FID_DEVICE_INFO;
@ -623,8 +669,9 @@ std::size_t to_data(const Channels::device_info_t *info, std::uint8_t *data) {
}
std::size_t to_data(
const Channels::img_params_t *img_params, std::uint8_t *data) {
std::size_t i = 3;
const Channels::img_params_t *img_params, std::uint8_t *data,
const Version *spec_version) {
std::size_t i = 3; // skip id, size
auto &&in = img_params->in;
// width, 2
@ -658,7 +705,7 @@ std::size_t to_data(
// rotation
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
_to_data(ex.rotation[j][k], data + i + j * 3 + k);
_to_data(ex.rotation[j][k], data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -668,6 +715,8 @@ std::size_t to_data(
}
i += 24;
UNUSED(spec_version)
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMG_PARAMS;
@ -677,21 +726,22 @@ std::size_t to_data(
}
std::size_t to_data(
const Channels::imu_params_t *imu_params, std::uint8_t *data) {
std::size_t i = 3;
const Channels::imu_params_t *imu_params, std::uint8_t *data,
const Version *spec_version) {
std::size_t i = 3; // skip id, size
auto &&in = imu_params->in;
// acc_scale
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
_to_data(in.accel.scale[j][k], data + i + j * 3 + k);
_to_data(in.accel.scale[j][k], data + i + (j * 3 + k) * 8);
}
}
i += 72;
// gyro_scale
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
_to_data(in.gyro.scale[j][k], data + i + j * 3 + k);
_to_data(in.gyro.scale[j][k], data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -730,7 +780,7 @@ std::size_t to_data(
// rotation
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
_to_data(ex.rotation[j][k], data + i + j * 3 + k);
_to_data(ex.rotation[j][k], data + i + (j * 3 + k) * 8);
}
}
i += 72;
@ -740,6 +790,8 @@ std::size_t to_data(
}
i += 24;
UNUSED(spec_version)
// others
std::size_t size = i - 3;
data[0] = Channels::FID_IMU_PARAMS;
@ -751,11 +803,18 @@ std::size_t to_data(
} // namespace
bool Channels::SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params) {
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version) {
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]{};
std::bitset<8> header;
@ -764,21 +823,23 @@ bool Channels::SetFiles(
std::uint16_t size = 0;
if (info != nullptr) {
header[0] = true;
size += to_data(info, data + 3 + size);
size += to_data(info, data + 3 + size, spec_ver);
}
if (img_params != nullptr) {
header[1] = true;
size += to_data(img_params, data + 3 + size);
size += to_data(img_params, data + 3 + size, spec_ver);
}
if (imu_params != nullptr) {
header[2] = true;
size += to_data(imu_params, data + 3 + size);
size += to_data(imu_params, data + 3 + size, spec_ver);
}
data[0] = static_cast<std::uint8_t>(header.to_ulong());
data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
data[2] = static_cast<std::uint8_t>(size & 0xFF);
VLOG(2) << "SetFiles header: 0x" << std::hex << std::uppercase << std::setw(2)
<< std::setfill('0') << static_cast<int>(data[0]);
if (XuFileQuery(uvc::XU_QUERY_SET, 2000, data)) {
VLOG(2) << "SetFiles success";
return true;

15
src/internal/channels.h
View File

@ -45,9 +45,9 @@ class Channels {
} xu_cmd_t;
typedef enum FileId {
FID_DEVICE_INFO = 0, // device info
FID_IMG_PARAMS = 1, // image intrinsics & extrinsics
FID_IMU_PARAMS = 2, // imu intrinsics & extrinsics
FID_DEVICE_INFO = 1, // device info
FID_IMG_PARAMS = 2, // image intrinsics & extrinsics
FID_IMU_PARAMS = 4, // imu intrinsics & extrinsics
FID_LAST,
} file_id_t;
@ -56,11 +56,13 @@ class Channels {
using device_info_t = DeviceInfo;
typedef struct ImgParams {
bool ok;
ImgIntrinsics in;
ImgExtrinsics ex;
} img_params_t;
typedef struct ImuParams {
bool ok;
ImuIntrinsics in;
ImuExtrinsics ex;
} imu_params_t;
@ -82,10 +84,11 @@ class Channels {
void StopImuTracking();
bool GetFiles(
device_info_t *info, img_params_t *img_params,
imu_params_t *imu_params) const;
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version = nullptr) const;
bool SetFiles(
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params);
device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
Version *spec_version = nullptr);
private:
bool PuControlRange(

67
src/public/types.cc
View File

@ -145,4 +145,71 @@ std::ostream &operator<<(std::ostream &os, const StreamRequest &request) {
<< ", format: " << request.format << ", fps: " << request.fps;
}
std::ostream &operator<<(std::ostream &os, const ImgIntrinsics &in) {
os << "width: " << in.width << ", height: " << in.height << ", fx: " << in.fx
<< ", fy: " << in.fy << ", cx: " << in.cx << ", cy: " << in.cy
<< ", model: " << static_cast<int>(in.model) << ", coeffs: [";
for (int i = 0; i <= 3; i++)
os << in.coeffs[i] << ", ";
return os << in.coeffs[4] << "]";
}
std::ostream &operator<<(std::ostream &os, const ImuSensorIntrinsics &in) {
os << "scale: [";
for (int i = 0; i <= 2; i++)
os << in.scale[0][i] << ", ";
for (int i = 0; i <= 2; i++)
os << in.scale[1][i] << ", ";
for (int i = 0; i <= 1; i++)
os << in.scale[2][i] << ", ";
os << in.scale[2][2] << "]";
os << ", drift: [";
for (int i = 0; i <= 1; i++)
os << in.drift[i] << ", ";
os << in.drift[2] << "]";
os << ", noise: [";
for (int i = 0; i <= 1; i++)
os << in.noise[i] << ", ";
os << in.noise[2] << "]";
os << ", bias: [";
for (int i = 0; i <= 1; i++)
os << in.bias[i] << ", ";
os << in.bias[2] << "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
return os << "accel: {" << in.accel << "}, gyro: {" << in.gyro << "}";
}
std::ostream &operator<<(std::ostream &os, const Extrinsics &ex) {
os << "rotation: [";
for (int i = 0; i <= 2; i++)
os << ex.rotation[0][i] << ", ";
for (int i = 0; i <= 2; i++)
os << ex.rotation[1][i] << ", ";
for (int i = 0; i <= 1; i++)
os << ex.rotation[2][i] << ", ";
os << ex.rotation[2][2] << "]";
os << ", translation: [";
for (int i = 0; i <= 1; i++)
os << ex.translation[i] << ", ";
os << ex.translation[2] << "]";
return os;
}
std::ostream &operator<<(std::ostream &os, const ImgExtrinsics &ex) {
return os << "left_to_right: {" << ex.left_to_right << "}";
}
std::ostream &operator<<(std::ostream &os, const ImuExtrinsics &ex) {
return os << "left_to_imu: {" << ex.left_to_imu << "}";
}
MYNTEYE_END_NAMESPACE