From f98fe1b4eca9131a22a2c7445dfd31b6da20d099 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Tue, 22 Jan 2019 18:26:21 +0800 Subject: [PATCH 1/5] 2c92a2b0703ef9a37c653797b133de2a5371a5b7 --- src/mynteye/api/processor.h | 28 +++- src/mynteye/api/synthetic.cc | 290 +++++------------------------------ 2 files changed, 59 insertions(+), 259 deletions(-) diff --git a/src/mynteye/api/processor.h b/src/mynteye/api/processor.h index 09a9cf3..bf36792 100644 --- a/src/mynteye/api/processor.h +++ b/src/mynteye/api/processor.h @@ -133,18 +133,36 @@ void iterate_processors_PtoC_before( } template -void iterate_processors_CtoP_before( +void iterate_processor_PtoC_after( T processor, std::function)> fn) { - if (processor->GetParent() != nullptr) - iterate_processors_CtoP_before(processor->GetParent(), fn); + fn(processor); + auto chids = processor->GetChilds(); + for (auto it : chids) { + iterate_processor_PtoC_after(it, fn); + } +} +template +void iterate_processor_PtoC_before( + T processor, std::function)> fn) { + auto chids = processor->GetChilds(); + for (auto it : chids) { + iterate_processor_PtoC_before(it, fn); + } fn(processor); } template -void iterate_processors_CtoP_after( +void iterate_processor_CtoP_before( + T processor, std::function)> fn) { + if (processor->GetParent() != nullptr) + iterate_processor_CtoP_before(processor->GetParent(), fn); + fn(processor); +} +template +void iterate_processor_CtoP_after( T processor, std::function)> fn) { fn(processor); if (processor->GetParent() != nullptr) - iterate_processors_CtoP_after(processor->GetParent(), fn); + iterate_processor_CtoP_after(processor->GetParent(), fn); } MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 3f97ca6..54935f6 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -42,6 +42,7 @@ #define DISPARITY_NORM_PROC_PERIOD 0 #define POINTS_PROC_PERIOD 0 #define DEPTH_PROC_PERIOD 0 +#define ROOT_PROC_PERIOD 0 MYNTEYE_BEGIN_NAMESPACE @@ -183,21 +184,6 @@ void Synthetic::setControlDateCallbackWithStream( LOG(ERROR) << "ERROR: no suited processor for stream "<< ctr_data.stream; } -// void Synthetic::setControlDateModeWithStream( -// const struct stream_control_t& ctr_data) { -// for (auto &&it : processors_) { -// int i = 0; -// for (auto it_s : it->getTargetStreams()) { -// if (it_s.stream == ctr_data.stream) { -// it->target_streams_[i].mode = ctr_data.mode; -// return; -// } -// i++; -// } -// } -// LOG(ERROR) << "ERROR: no suited processor for stream "<< ctr_data.stream; -// } - bool Synthetic::checkControlDateWithStream(const Stream& stream) const { for (auto &&it : processors_) { for (auto it_s : it->getTargetStreams()) { @@ -209,40 +195,25 @@ bool Synthetic::checkControlDateWithStream(const Stream& stream) const { return false; } -// bool Synthetic::Supports(const Stream &stream) const { -// return checkControlDateWithStream(stream); -// } - -// Synthetic::status_mode_t Synthetic::GetStreamStatusMode( -// const Stream &stream) const { -// if (checkControlDateWithStream(stream)) { -// auto ctrData = getControlDateWithStream(stream); -// return ctrData.mode; -// } else { -// return MODE_STATUS_LAST; -// } -// } - -// void Synthetic::EnableStreamData(const Stream &stream) { -// // Activate processors of synthetic stream -// auto processor = getProcessorWithStream(stream); -// iterate_processors_CtoP_before(processor, -// [](std::shared_ptr proce){ -// auto streams = proce->getTargetStreams(); -// int act_tag = 0; -// for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { -// if (proce->target_streams_[i].mode == MODE_STATUS_DISABLE) { -// act_tag++; -// proce->target_streams_[i].mode = MODE_STATUS_ENABLE; -// } -// } -// if (act_tag > 0) { -// std::cout << proce->Name() << "active" << std::endl; -// proce->Activate(); -// } -// // std::cout < proce){ + auto streams = proce->getTargetStreams(); + int act_tag = 0; + for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { + if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) { + act_tag++; + proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC; + } + } + if (act_tag > 0 && !proce->IsActivated()) { + // std::cout << proce->Name() << " Active now" << std::endl; + proce->Activate(); + } + }); +} bool Synthetic::Supports(const Stream &stream) const { @@ -257,12 +228,23 @@ Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const { return MODE_LAST; } -void Synthetic::EnableStreamData(const Stream &stream) { - EnableStreamData(stream, 0); -} - void Synthetic::DisableStreamData(const Stream &stream) { - DisableStreamData(stream, 0); + auto processor = getProcessorWithStream(stream); + iterate_processor_PtoC_before(processor, + [](std::shared_ptr proce){ + auto streams = proce->getTargetStreams(); + int act_tag = 0; + for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) { + if (proce->target_streams_[i].enabled_mode_ == MODE_SYNTHETIC) { + act_tag++; + proce->target_streams_[i].enabled_mode_ = MODE_LAST; + } + } + if (act_tag > 0 && proce->IsActivated()) { + // std::cout << proce->Name() << "Deactive now" << std::endl; + proce->Deactivate(); + } + }); } bool Synthetic::IsStreamDataEnabled(const Stream &stream) const { @@ -349,7 +331,6 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) { auto sum = processor->getStreamsSum(); auto &&out = processor->GetOutput(); static std::shared_ptr output = nullptr; - std::cout << processor->Name() << stream <(out); @@ -469,205 +450,6 @@ bool Synthetic::IsStreamEnabledSynthetic(const Stream &stream) const { return GetStreamEnabledMode(stream) == MODE_SYNTHETIC; } -void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) { - if (IsStreamDataEnabled(stream)) - return; - // Activate processors of synthetic stream - - auto processor = getProcessorWithStream(stream); - for (unsigned int i = 0; i< processor->target_streams_.size(); i++) { - if (processor->target_streams_[i].stream == stream) { - processor->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC; - } - } - switch (stream) { - case Stream::LEFT_RECTIFIED: { - if (!IsStreamDataEnabled(Stream::LEFT)) - break; - if (calib_model_ == CalibrationModel::PINHOLE) { - CHECK(ActivateProcessor()); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - CHECK(ActivateProcessor()); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_ << ", use default pinhole model"; - CHECK(ActivateProcessor()); - } - } return; - case Stream::RIGHT_RECTIFIED: { - if (!IsStreamDataEnabled(Stream::RIGHT)) - break; - if (calib_model_ == CalibrationModel::PINHOLE) { - CHECK(ActivateProcessor()); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - CHECK(ActivateProcessor()); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_ << ", use default pinhole model"; - CHECK(ActivateProcessor()); - } - } return; - case Stream::DISPARITY: { - EnableStreamData(Stream::LEFT_RECTIFIED, depth + 1); - EnableStreamData(Stream::RIGHT_RECTIFIED, depth + 1); - CHECK(ActivateProcessor()); - } return; - case Stream::DISPARITY_NORMALIZED: { - EnableStreamData(Stream::DISPARITY, depth + 1); - CHECK(ActivateProcessor()); - } return; - case Stream::POINTS: { - if (calib_model_ == CalibrationModel::PINHOLE) { - EnableStreamData(Stream::DISPARITY, depth + 1); - CHECK(ActivateProcessor()); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - EnableStreamData(Stream::DEPTH, depth + 1); - CHECK(ActivateProcessor()); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - } return; - case Stream::DEPTH: { - if (calib_model_ == CalibrationModel::PINHOLE) { - EnableStreamData(Stream::POINTS, depth + 1); - CHECK(ActivateProcessor()); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - EnableStreamData(Stream::DISPARITY, depth + 1); - CHECK(ActivateProcessor()); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - } return; - default: break; - } - if (depth == 0) { - LOG(WARNING) << "Enable stream data of " << stream << " failed"; - } -} - -void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { - if (!IsStreamDataEnabled(stream)) - return; - // Deactivate processors of synthetic stream - auto data = getControlDateWithStream(stream); - if (data.enabled_mode_ != MODE_NATIVE) { - data.enabled_mode_ = MODE_LAST; - switch (stream) { - case Stream::LEFT_RECTIFIED: { - if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { - DisableStreamData(Stream::DISPARITY, depth + 1); - } - if (IsStreamEnabledSynthetic(Stream::RIGHT_RECTIFIED)) { - DisableStreamData(Stream::RIGHT_RECTIFIED, depth + 1); - } - if (calib_model_ == CalibrationModel::PINHOLE) { - DeactivateProcessor(); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - DeactivateProcessor(); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_ << ", use default pinhole model"; - DeactivateProcessor(); - } - } break; - case Stream::RIGHT_RECTIFIED: { - if (IsStreamEnabledSynthetic(Stream::DISPARITY)) { - DisableStreamData(Stream::DISPARITY, depth + 1); - } - if (IsStreamEnabledSynthetic(Stream::LEFT_RECTIFIED)) { - DisableStreamData(Stream::LEFT_RECTIFIED, depth + 1); - } - if (calib_model_ == CalibrationModel::PINHOLE) { - DeactivateProcessor(); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - DeactivateProcessor(); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_ << ", use default pinhole model"; - DeactivateProcessor(); - } - } break; - case Stream::DISPARITY: { - if (calib_model_ == CalibrationModel::PINHOLE) { - if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { - DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1); - } - if (IsStreamEnabledSynthetic(Stream::POINTS)) { - DisableStreamData(Stream::POINTS, depth + 1); - } - DeactivateProcessor(); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - if (IsStreamEnabledSynthetic(Stream::DISPARITY_NORMALIZED)) { - DisableStreamData(Stream::DISPARITY_NORMALIZED, depth + 1); - } - if (IsStreamEnabledSynthetic(Stream::DEPTH)) { - DisableStreamData(Stream::DEPTH, depth + 1); - } - DeactivateProcessor(); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - } break; - case Stream::DISPARITY_NORMALIZED: { - DeactivateProcessor(); - } break; - case Stream::POINTS: { - if (calib_model_ == CalibrationModel::PINHOLE) { - if (IsStreamEnabledSynthetic(Stream::DEPTH)) { - DisableStreamData(Stream::DEPTH, depth + 1); - } - DeactivateProcessor(); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - DeactivateProcessor(); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - } break; - case Stream::DEPTH: { - if (calib_model_ == CalibrationModel::PINHOLE) { - DeactivateProcessor(); -#ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - if (IsStreamEnabledSynthetic(Stream::POINTS)) { - DisableStreamData(Stream::POINTS, depth + 1); - } - DeactivateProcessor(); -#endif - } else { - LOG(ERROR) << "Unknow calib model type in device: " - << calib_model_; - } - } break; - default: return; - } - if (depth > 0) { - LOG(WARNING) << "Disable synthetic stream data of " << stream << " too"; - } - } else if (depth == 0) { - LOG(WARNING) << "Disable native stream data of " << stream << " failed"; - } -} - void Synthetic::InitProcessors() { std::shared_ptr rectify_processor = nullptr; #ifdef WITH_CAM_MODELS From dacad9483e4892429ccf600cb4d353a1dedb95f4 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 15 Mar 2019 14:55:46 +0800 Subject: [PATCH 2/5] fix(api): samples issues --- samples/tutorials/data/get_imu.cc | 2 +- samples/tutorials/data/get_imu_correspondence.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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); } */ From e91a2663e211b3c60fe14d321bfa05ba3992ebb7 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 15 Mar 2019 15:05:22 +0800 Subject: [PATCH 3/5] fix use disparity_normalized as sample --- samples/tutorials/data/get_disparity.cc | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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; From edb18a90b8bbfdf460b60cf775e79693e6283a23 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Fri, 15 Mar 2019 16:27:29 +0800 Subject: [PATCH 4/5] fix: points quality --- samples/tutorials/data/get_points.cc | 1 - samples/tutorials/intermediate/get_depth_and_points.cc | 1 - 2 files changed, 2 deletions(-) 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); From b3e41de62cbb07b9e779abe22ee6f37b2236931e Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 15 Mar 2019 17:00:52 +0800 Subject: [PATCH 5/5] feat(src): added imu params: assembly error, temperature error --- include/mynteye/types.h | 12 +++++ src/mynteye/device/channel/bytes.cc | 60 +++++++++++++++++++++- src/mynteye/device/channel/bytes.h | 6 ++- src/mynteye/device/channel/file_channel.cc | 23 +++++---- src/mynteye/types.cc | 24 +++++++++ tools/writer/config/S1030/imu.params | 8 +++ tools/writer/config/S210A/imu.params | 8 +++ tools/writer/device_writer.cc | 27 +++++++++- 8 files changed, 152 insertions(+), 16 deletions(-) diff --git a/include/mynteye/types.h b/include/mynteye/types.h index adf52c8..8255b39 100644 --- a/include/mynteye/types.h +++ b/include/mynteye/types.h @@ -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 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) {