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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/mynteye/types.h
View File

@ -500,12 +500,24 @@ struct MYNTEYE_API ImuIntrinsics {
* \endcode
*/
double scale[3][3];
/** Assembly error [3][3] */
double assembly[3][3];
/* Zero-drift: X, Y, Z */
double drift[3];
/** Noise density variances */
double noise[3];
/** Random walk variances */
double bias[3];
/** Temperature drift
* \code
* 0 - Constant value
* 1 - Slope
* \endcode
*/
double x[2];
double y[2];
double z[2];
};
MYNTEYE_API

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

8
tools/writer/config/S1030/imu.params
View File

@ -3,18 +3,26 @@
version: "1.2"
in_accel:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
drift: [ 0., 0., 0. ]
noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
1.7452487504590969e-02 ]
bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
5.4490537096493644e-04 ]
x: [ 0.0, 0.0 ]
y: [ 0.0, 0.0 ]
z: [ 0.0, 0.0 ]
in_gyro:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
drift: [ 0., 0., 0. ]
noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
1.1003229919806443e-03 ]
bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
1.4970418056326829e-05 ]
x: [ 0.0, 0.0 ]
y: [ 0.0, 0.0 ]
z: [ 0.0, 0.0 ]
ex_left_to_imu:
rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
-7.6356499999999999e-03, 9.9997908999999996e-01,

8
tools/writer/config/S210A/imu.params
View File

@ -3,18 +3,26 @@
version: "1.2"
in_accel:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
drift: [ 0., 0., 0. ]
noise: [ 1.6925432397973516e-02, 1.6735310195561025e-02,
1.7452487504590969e-02 ]
bias: [ 1.9031356589714596e-04, 1.6996777864587261e-04,
5.4490537096493644e-04 ]
x: [ 0.0, 0.0 ]
y: [ 0.0, 0.0 ]
z: [ 0.0, 0.0 ]
in_gyro:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
assembly: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
drift: [ 0., 0., 0. ]
noise: [ 1.0848026158819934e-03, 1.2466367883501759e-03,
1.1003229919806443e-03 ]
bias: [ 2.3404834136742844e-05, 2.3596771567764949e-05,
1.4970418056326829e-05 ]
x: [ 0.0, 0.0 ]
y: [ 0.0, 0.0 ]
z: [ 0.0, 0.0 ]
ex_left_to_imu:
rotation: [ -6.4662000000000001e-03, -9.9994994000000004e-01,
-7.6356499999999999e-03, 9.9997908999999996e-01,

27
tools/writer/device_writer.cc
View File

@ -157,11 +157,20 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) {
scales.push_back(in.scale[i][j]);
}
}
std::vector<double> assembly;
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
assembly.push_back(in.assembly[i][j]);
}
}
fs << "{"
<< "scale" << scales << "drift"
<< "scale" << scales << "assembly" << assembly << "drift"
<< std::vector<double>(in.drift, in.drift + 3) << "noise"
<< std::vector<double>(in.noise, in.noise + 3) << "bias"
<< std::vector<double>(in.bias, in.bias + 3) << "}";
<< std::vector<double>(in.bias, in.bias + 3) << "x"
<< std::vector<double>(in.x, in.x + 2) << "y"
<< std::vector<double>(in.y, in.y + 2) << "z"
<< std::vector<double>(in.z, in.z + 2) << "}";
return fs;
}
@ -343,6 +352,11 @@ void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
in.scale[i][j] = n["scale"][3 * i + j];
}
}
for (std::size_t i = 0; i < 3; i++) {
for (std::size_t j = 0; j < 3; j++) {
in.assembly[i][j] = n["assembly"][3 * i + j];
}
}
for (std::size_t i = 0; i < 3; i++) {
in.drift[i] = n["drift"][i];
}
@ -352,6 +366,15 @@ void operator>>(const cv::FileNode &n, ImuIntrinsics &in) {
for (std::size_t i = 0; i < 3; i++) {
in.bias[i] = n["bias"][i];
}
for (std::size_t i = 0; i < 2; i++) {
in.x[i] = n["x"][i];
}
for (std::size_t i = 0; i < 2; i++) {
in.y[i] = n["y"][i];
}
for (std::size_t i = 0; i < 2; i++) {
in.z[i] = n["z"][i];
}
}
void operator>>(const cv::FileNode &n, Extrinsics &ex) {