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

feat(src): added imu params: assembly error, temperature error

Browse Source
This commit is contained in:
Osenberg
2019-03-15 17:00:52 +08:00
parent 3f80c8bbff
commit b3e41de62c
8 changed files with 152 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -103,7 +103,8 @@ std::size_t from_data(IntrinsicsEquidistant *in, const std::uint8_t *data,
return i;
}
std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
bool get_size) {
std::size_t i = 0;
// scale
@@ -113,6 +114,15 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
}
}
i += 72;
if (get_size) {
// assembly
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
in->assembly[j][k] = _from_data<double>(data + i + (j * 3 + k) * 8);
}
}
i += 72;
}
// drift
for (std::size_t j = 0; j < 3; j++) {
in->drift[j] = _from_data<double>(data + i + j * 8);
@@ -128,6 +138,24 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data) {
in->bias[j] = _from_data<double>(data + i + j * 8);
}
i += 24;
if (get_size) {
// temperature drift
// x
for (std::size_t j = 0; j < 2; j++) {
in->x[j] = _from_data<double>(data + i + j * 8);
}
i += 16;
// y
for (std::size_t j = 0; j < 2; j++) {
in->y[j] = _from_data<double>(data + i + j * 8);
}
i += 16;
// z
for (std::size_t j = 0; j < 2; j++) {
in->z[j] = _from_data<double>(data + i + j * 8);
}
i += 16;
}
return i;
}
@@ -236,7 +264,8 @@ std::size_t to_data(const IntrinsicsEquidistant *in, std::uint8_t *data,
return i;
}
std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data,
bool set_size) {
std::size_t i = 0;
// scale
@@ -246,6 +275,15 @@ std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
}
}
i += 72;
if (set_size) {
// assembly
for (std::size_t j = 0; j < 3; j++) {
for (std::size_t k = 0; k < 3; k++) {
_to_data(in->assembly[j][k], data + i + (j * 3 + k) * 8);
}
}
i += 72;
}
// drift
for (std::size_t j = 0; j < 3; j++) {
_to_data(in->drift[j], data + i + j * 8);
@@ -261,6 +299,24 @@ std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data) {
_to_data(in->bias[j], data + i + j * 8);
}
i += 24;
if (set_size) {
// temperature drift
// x
for (std::size_t j = 0; j < 2; j++) {
_to_data<double>(in->x[j], data + i + j * 8);
}
i += 16;
// y
for (std::size_t j = 0; j < 2; j++) {
_to_data<double>(in->y[j], data + i + j * 8);
}
i += 16;
// z
for (std::size_t j = 0; j < 2; j++) {
_to_data<double>(in->z[j], data + i + j * 8);
}
i += 16;
}
return i;
}

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

@@ -55,7 +55,8 @@ std::size_t from_data(IntrinsicsPinhole *in, const std::uint8_t *data,
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);
std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
bool get_size);
std::size_t from_data(Extrinsics *ex, const std::uint8_t *data);
@@ -88,7 +89,8 @@ std::size_t to_data(const IntrinsicsPinhole *in, std::uint8_t *data,
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);
std::size_t to_data(const ImuIntrinsics *in, std::uint8_t *data, bool
set_size);
std::size_t to_data(const Extrinsics *ex, std::uint8_t *data);

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

@@ -442,16 +442,19 @@ std::size_t ImuParamsParser::GetFromData(
std::size_t ImuParamsParser::SetToData(
const imu_params_t *imu_params, std::uint8_t *data) const {
// always set imu params with new version format
return SetToData_new(imu_params, data);
if (spec_version_ >= Version(1, 2)) {
return SetToData_new(imu_params, data);
} else {
return SetToData_old(imu_params, data);
}
}
std::size_t ImuParamsParser::GetFromData_old(
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->in_accel, data + i, false);
i += bytes::from_data(&imu_params->in_gyro, data + i, false);
i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
imu_params->version = spec_version_.to_string();
MYNTEYE_UNUSED(data_size)
@@ -461,8 +464,8 @@ std::size_t ImuParamsParser::GetFromData_old(
std::size_t ImuParamsParser::SetToData_old(
const imu_params_t *imu_params, std::uint8_t *data) const {
std::size_t i = 3; // skip id, size
i += bytes::to_data(&imu_params->in_accel, data + i);
i += bytes::to_data(&imu_params->in_gyro, data + i);
i += bytes::to_data(&imu_params->in_accel, data + i, false);
i += bytes::to_data(&imu_params->in_gyro, data + i, false);
i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
// others
std::size_t size = i - 3;
@@ -482,8 +485,8 @@ std::size_t ImuParamsParser::GetFromData_new(
i += 2;
// get imu params according to version
if (version == Version(1, 2)) { // v1.2
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->in_accel, data + i, true);
i += bytes::from_data(&imu_params->in_gyro, data + i, true);
i += bytes::from_data(&imu_params->ex_left_to_imu, data + i);
} else {
LOG(FATAL) << "Could not get imu params of version "
@@ -506,8 +509,8 @@ std::size_t ImuParamsParser::SetToData_new(
i += 2;
// set imu params with new version format
if (version_raw <= version_new) {
i += bytes::to_data(&imu_params->in_accel, data + i);
i += bytes::to_data(&imu_params->in_gyro, data + i);
i += bytes::to_data(&imu_params->in_accel, data + i, true);
i += bytes::to_data(&imu_params->in_gyro, data + i, true);
i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
} else {
LOG(FATAL) << "Could not set imu params of version "

24
src/mynteye/types.cc
View File

@@ -246,6 +246,15 @@ std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
os << in.scale[2][i] << ", ";
os << in.scale[2][2] << "]";
os << ", assembly: [";
for (int i = 0; i <= 2; i++)
os << in.assembly[0][i] << ", ";
for (int i = 0; i <= 2; i++)
os << in.assembly[1][i] << ", ";
for (int i = 0; i <= 2; i++)
os << in.assembly[2][i] << ", ";
os << in.assembly[2][2] << "]";
os << ", drift: [";
for (int i = 0; i <= 1; i++)
os << in.drift[i] << ", ";
@@ -261,6 +270,21 @@ std::ostream &operator<<(std::ostream &os, const ImuIntrinsics &in) {
os << in.bias[i] << ", ";
os << in.bias[2] << "]";
os << ", x: [";
for (int i = 0; i <= 0; i++)
os << in.x[i] << ", ";
os << in.x[1] << "]";
os << ", y: [";
for (int i = 0; i <= 0; i++)
os << in.y[i] << ", ";
os << in.y[1] << "]";
os << ", z: [";
for (int i = 0; i <= 0; i++)
os << in.z[i] << ", ";
os << in.z[1] << "]";
return os;
}