diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 174660e..d5666db 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -518,12 +518,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 diff --git a/samples/tutorials/data/get_disparity.cc b/samples/tutorials/data/get_disparity.cc index ff9ae4a..b5f1fd5 100644 --- a/samples/tutorials/data/get_disparity.cc +++ b/samples/tutorials/data/get_disparity.cc @@ -34,8 +34,8 @@ int main(int argc, char *argv[]) { api->Start(Source::VIDEO_STREAMING); cv::namedWindow("frame"); - cv::namedWindow("disparity"); - // cv::namedWindow("disparity_normalized"); + // cv::namedWindow("disparity"); + cv::namedWindow("disparity_normalized"); while (true) { api->WaitForStreams(); @@ -49,16 +49,16 @@ int main(int argc, char *argv[]) { cv::imshow("frame", img); } - auto &&disp_data = api->GetStreamData(Stream::DISPARITY); - if (!disp_data.frame.empty()) { - cv::imshow("disparity", disp_data.frame); - } - - // auto &&disp_norm_data = api->GetStreamData(Stream::DISPARITY_NORMALIZED); - // if (!disp_norm_data.frame.empty()) { - // cv::imshow("disparity_normalized", disp_norm_data.frame); // CV_8UC1 + // auto &&disp_data = api->GetStreamData(Stream::DISPARITY); + // if (!disp_data.frame.empty()) { + // cv::imshow("disparity", disp_data.frame); // } + auto &&disp_norm_data = api->GetStreamData(Stream::DISPARITY_NORMALIZED); + if (!disp_norm_data.frame.empty()) { + cv::imshow("disparity_normalized", disp_norm_data.frame); // CV_8UC1 + } + char key = static_cast(cv::waitKey(1)); if (key == 27 || key == 'q' || key == 'Q') { // ESC/Q break; diff --git a/samples/tutorials/data/get_imu.cc b/samples/tutorials/data/get_imu.cc index eca5b3d..99ea685 100644 --- a/samples/tutorials/data/get_imu.cc +++ b/samples/tutorials/data/get_imu.cc @@ -64,7 +64,7 @@ int main(int argc, char *argv[]) { */ painter.DrawImgData(img, *left_data.img); - if (!motion_datas.empty()) { + if (!motion_datas.empty() && motion_datas.size() > 0) { painter.DrawImuData(img, *motion_datas[0].imu); } diff --git a/samples/tutorials/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc index 7faef67..3a2e184 100644 --- a/samples/tutorials/data/get_imu_correspondence.cc +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -119,7 +119,7 @@ int main(int argc, char *argv[]) { /* painter.DrawImgData(img, *left_data.img); - if (!motion_datas.empty()) { + if (!motion_datas.empty() && motion_datas.size() > 0) { painter.DrawImuData(img, *motion_datas[0].imu); } */ diff --git a/samples/tutorials/data/get_points.cc b/samples/tutorials/data/get_points.cc index b51c966..0ac59d7 100644 --- a/samples/tutorials/data/get_points.cc +++ b/samples/tutorials/data/get_points.cc @@ -27,7 +27,6 @@ int main(int argc, char *argv[]) { auto &&request = api->SelectStreamRequest(&ok); if (!ok) return 1; api->ConfigStreamRequest(request); - api->SetDisparityComputingMethodType(DisparityComputingMethod::BM); api->EnableStreamData(Stream::POINTS); diff --git a/samples/tutorials/intermediate/get_depth_and_points.cc b/samples/tutorials/intermediate/get_depth_and_points.cc index 63692d1..349a767 100644 --- a/samples/tutorials/intermediate/get_depth_and_points.cc +++ b/samples/tutorials/intermediate/get_depth_and_points.cc @@ -154,7 +154,6 @@ int main(int argc, char *argv[]) { auto &&request = api->SelectStreamRequest(&ok); if (!ok) return 1; api->ConfigStreamRequest(request); - api->SetDisparityComputingMethodType(DisparityComputingMethod::BM); api->SetOptionValue(Option::IR_CONTROL, 80); diff --git a/src/mynteye/device/channel/bytes.cc b/src/mynteye/device/channel/bytes.cc index 077632f..f04da30 100644 --- a/src/mynteye/device/channel/bytes.cc +++ b/src/mynteye/device/channel/bytes.cc @@ -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(data + i + (j * 3 + k) * 8); + } + } + i += 72; + } // drift for (std::size_t j = 0; j < 3; j++) { in->drift[j] = _from_data(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(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(data + i + j * 8); + } + i += 16; + // y + for (std::size_t j = 0; j < 2; j++) { + in->y[j] = _from_data(data + i + j * 8); + } + i += 16; + // z + for (std::size_t j = 0; j < 2; j++) { + in->z[j] = _from_data(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(in->x[j], data + i + j * 8); + } + i += 16; + // y + for (std::size_t j = 0; j < 2; j++) { + _to_data(in->y[j], data + i + j * 8); + } + i += 16; + // z + for (std::size_t j = 0; j < 2; j++) { + _to_data(in->z[j], data + i + j * 8); + } + i += 16; + } return i; } diff --git a/src/mynteye/device/channel/bytes.h b/src/mynteye/device/channel/bytes.h index 01bafdc..b22515e 100644 --- a/src/mynteye/device/channel/bytes.h +++ b/src/mynteye/device/channel/bytes.h @@ -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); diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc index 66d674b..6f31d0d 100644 --- a/src/mynteye/device/channel/file_channel.cc +++ b/src/mynteye/device/channel/file_channel.cc @@ -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 " diff --git a/src/mynteye/types.cc b/src/mynteye/types.cc index 4bc83f1..77d4eba 100644 --- a/src/mynteye/types.cc +++ b/src/mynteye/types.cc @@ -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; } diff --git a/tools/writer/config/S1030/imu.params b/tools/writer/config/S1030/imu.params index 00993e2..93ad7e8 100644 --- a/tools/writer/config/S1030/imu.params +++ b/tools/writer/config/S1030/imu.params @@ -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, diff --git a/tools/writer/config/S210A/imu.params b/tools/writer/config/S210A/imu.params index 00993e2..93ad7e8 100644 --- a/tools/writer/config/S210A/imu.params +++ b/tools/writer/config/S210A/imu.params @@ -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, diff --git a/tools/writer/device_writer.cc b/tools/writer/device_writer.cc index 96d9f77..ddd9207 100644 --- a/tools/writer/device_writer.cc +++ b/tools/writer/device_writer.cc @@ -157,11 +157,20 @@ cv::FileStorage &operator<<(cv::FileStorage &fs, const ImuIntrinsics &in) { scales.push_back(in.scale[i][j]); } } + std::vector 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(in.drift, in.drift + 3) << "noise" << std::vector(in.noise, in.noise + 3) << "bias" - << std::vector(in.bias, in.bias + 3) << "}"; + << std::vector(in.bias, in.bias + 3) << "x" + << std::vector(in.x, in.x + 2) << "y" + << std::vector(in.y, in.y + 2) << "z" + << std::vector(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) {