From bcaa22c86ec344e1e1ad020f2c533c44534151f2 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Tue, 16 Apr 2024 10:59:44 +0300 Subject: [PATCH] d500 auto calib added for future capabilities --- common/CMakeLists.txt | 2 + common/d500-on-chip-calib.cpp | 306 +++++++++++ common/d500-on-chip-calib.h | 102 ++++ common/device-model.cpp | 757 +++++++++++++++----------- common/device-model.h | 4 + src/ds/d500/CMakeLists.txt | 2 + src/ds/d500/d500-auto-calibration.cpp | 280 ++++++++++ src/ds/d500/d500-auto-calibration.h | 73 +++ src/ds/d500/d500-private.cpp | 75 +++ src/ds/d500/d500-private.h | 8 + src/ds/ds-private.cpp | 11 + src/ds/ds-private.h | 6 +- 12 files changed, 1302 insertions(+), 324 deletions(-) create mode 100644 common/d500-on-chip-calib.cpp create mode 100644 common/d500-on-chip-calib.h create mode 100644 src/ds/d500/d500-auto-calibration.cpp create mode 100644 src/ds/d500/d500-auto-calibration.h diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 6875b2884e..4178d7c5f2 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -40,6 +40,8 @@ set(COMMON_SRC "${CMAKE_CURRENT_LIST_DIR}/measurement.cpp" "${CMAKE_CURRENT_LIST_DIR}/on-chip-calib.h" "${CMAKE_CURRENT_LIST_DIR}/on-chip-calib.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-on-chip-calib.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-on-chip-calib.cpp" "${CMAKE_CURRENT_LIST_DIR}/updates-model.h" "${CMAKE_CURRENT_LIST_DIR}/updates-model.cpp" "${CMAKE_CURRENT_LIST_DIR}/option-model.h" diff --git a/common/d500-on-chip-calib.cpp b/common/d500-on-chip-calib.cpp new file mode 100644 index 0000000000..837b54b5d6 --- /dev/null +++ b/common/d500-on-chip-calib.cpp @@ -0,0 +1,306 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + + +#include +#include "d500-on-chip-calib.h" + + +namespace rs2 +{ + d500_on_chip_calib_manager::d500_on_chip_calib_manager(viewer_model& viewer, std::shared_ptr sub, + device_model& model, device dev) + : process_manager("D500 On-Chip Calibration"), + _model(model), + _dev(dev), + _sub(sub) + { + if (dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE) && + std::string(dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE)) != "D500") + { + throw std::runtime_error("This Calibration Process cannot be processed with this device"); + } + } + + std::string d500_on_chip_calib_manager::convert_action_to_json_string() + { + std::stringstream ss; + if (action == RS2_CALIB_ACTION_ON_CHIP_CALIB) + { + ss << "{\n calib run }"; + } + else if (action == RS2_CALIB_ACTION_ON_CHIP_CALIB_DRY_RUN) + { + ss << "{\n calib dry run }"; + } + else if (action == RS2_CALIB_ACTION_ON_CHIP_CALIB_ABORT) + { + ss << "{\n calib abort }"; + } + return ss.str(); + } + + void d500_on_chip_calib_manager::process_flow(std::function cleanup, invoker invoke) + { + std::string json = convert_action_to_json_string(); + + auto calib_dev = _dev.as(); + float health = 0.f; + int timeout_ms = 120000; // 2 minutes + auto ans = calib_dev.run_on_chip_calibration(json, &health, + [&](const float progress) {_progress = progress; }, timeout_ms); + + // in order to grep new calibration from answer, use: + // auto new_calib = std::vector(ans.begin() + 3, ans.end()); + + _done = (_progress == 100.0); + } + + bool d500_on_chip_calib_manager::abort() + { + auto calib_dev = _dev.as(); + float health = 0.f; + int timeout_ms = 50000; // 50 seconds + std::string json = convert_action_to_json_string(); + auto ans = calib_dev.run_on_chip_calibration(json, &health, + [&](const float progress) {}, timeout_ms); + + // returns 1 on success, 0 on failure + return (ans[0] == 1); + } + + void d500_on_chip_calib_manager::prepare_for_calibration() + { + // set depth preset as default preset, turn projector ON and depth AE ON + if (_sub->s->is ()) + { + auto depth_sensor = _sub->s->as (); + + // set depth preset as default preset + set_option_if_needed(depth_sensor, RS2_OPTION_VISUAL_PRESET, 1); + + // turn projector ON + set_option_if_needed(depth_sensor, RS2_OPTION_EMITTER_ENABLED, 1); + + // turn depth AE ON + set_option_if_needed(depth_sensor, RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1); + } + } + + std::string d500_on_chip_calib_manager::get_device_pid() const + { + std::string pid_str; + if (_dev.supports(RS2_CAMERA_INFO_PRODUCT_ID)) + pid_str = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + return pid_str; + } + + d500_autocalib_notification_model::d500_autocalib_notification_model(std::string name, + std::shared_ptr manager, bool exp) + : process_notification_model(manager) + { + enable_expand = false; + enable_dismiss = true; + expanded = exp; + if (expanded) visible = false; + + message = name; + this->severity = RS2_LOG_SEVERITY_INFO; + this->category = RS2_NOTIFICATION_CATEGORY_HARDWARE_EVENT; + + pinned = true; + } + + void d500_autocalib_notification_model::draw_content(ux_window& win, int x, int y, float t, std::string& error_message) + { + const auto bar_width = width - 115; + ImGui::SetCursorScreenPos({ float(x + 9), float(y + 4) }); + + ImVec4 shadow{ 1.f, 1.f, 1.f, 0.1f }; + ImGui::GetWindowDrawList()->AddRectFilled({ float(x), float(y) }, + { float(x + width), float(y + 25) }, ImColor(shadow)); + + if (update_state != RS2_CALIB_STATE_COMPLETE) + { + if (get_manager().action == d500_on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB) + ImGui::Text("%s", "On-Chip Calibration"); + else if (get_manager().action == d500_on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB_DRY_RUN) + ImGui::Text("%s", "Dry Run On-Chip Calibration"); + + ImGui::PushStyleColor(ImGuiCol_Text, alpha(light_grey, 1.f - t)); + + if (update_state == RS2_CALIB_STATE_CALIB_IN_PROCESS) + { + enable_dismiss = false; + ImGui::SetCursorScreenPos({ float(x + 9), float(y + 27) }); + ImGui::Text("%s", "Camera is being calibrated...\n"); + draw_abort(win, x, y); + } + else if (update_state == RS2_CALIB_STATE_ABORT) + { + get_manager().action = d500_on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB_ABORT; + auto _this = shared_from_this(); + auto invoke = [_this](std::function action) {_this->invoke(action); }; + try + { + update_state = RS2_CALIB_STATE_ABORT_CALLED; + _has_abort_succeeded = get_manager().abort(); + } + catch (...) + { + throw std::runtime_error("Abort could not be performed!"); + } + } + else if (update_state == RS2_CALIB_STATE_ABORT_CALLED) + { + update_ui_after_abort_called(win, x, y); + } + else if (update_state == RS2_CALIB_STATE_INIT_CALIB || + update_state == RS2_CALIB_STATE_INIT_DRY_RUN) + { + calibration_button(win, x, y, bar_width); + } + + ImGui::PopStyleColor(); + } + else + { + update_ui_on_calibration_complete(win, x, y); + } + + ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) }); + + if (update_manager) + { + if (update_state == RS2_CALIB_STATE_CALIB_IN_PROCESS) + { + if (update_manager->done()) + { + update_state = RS2_CALIB_STATE_COMPLETE; + enable_dismiss = true; + } + + if (!expanded) + { + if (update_manager->failed()) + { + update_manager->check_error(_error_message); + update_state = RS2_CALIB_STATE_FAILED; + enable_dismiss = true; + } + + draw_progress_bar(win, bar_width); + ImGui::SetCursorScreenPos({ float(x + width - 105), float(y + height - 25) }); + ImGui::PushStyleColor(ImGuiCol_Text, light_grey); + ImGui::PopStyleColor(); + } + } + } + } + + int d500_autocalib_notification_model::calc_height() + { + // adjusting the height of the notification window + if (update_state == RS2_CALIB_STATE_CALIB_IN_PROCESS || + update_state == RS2_CALIB_STATE_COMPLETE || + update_state == RS2_CALIB_STATE_ABORT_CALLED) + return 90; + return 60; + } + + + void d500_autocalib_notification_model::calibration_button(ux_window& win, int x, int y, int bar_width) + { + using namespace std; + using namespace chrono; + + ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 31) }); + + auto sat = 1.f + sin(duration_cast(system_clock::now() - created_time).count() / 700.f) * 0.1f; + ImGui::PushStyleColor(ImGuiCol_Button, saturate(sensor_header_light_blue, sat)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, saturate(sensor_header_light_blue, 1.5f)); + + std::string activation_cal_str = "Calibrate"; + if (update_state == RS2_CALIB_STATE_INIT_DRY_RUN) + activation_cal_str = "Calibrate Dry Run"; + + std::string calibrate_button_name = rsutils::string::from() << activation_cal_str << "##self" << index; + + ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 28) }); + if (ImGui::Button(calibrate_button_name.c_str(), { float(bar_width), 20.f })) + { + //get_manager().restore_workspace([this](std::function a) { a(); }); + get_manager().reset(); + //get_manager().retry_times = 0; + if (update_state == RS2_CALIB_STATE_INIT_DRY_RUN) + { + get_manager().action = d500_on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB_DRY_RUN; + } + + get_manager().prepare_for_calibration(); + + auto _this = shared_from_this(); + auto invoke = [_this](std::function action) {_this->invoke(action); }; + get_manager().start(invoke); + update_state = RS2_CALIB_STATE_CALIB_IN_PROCESS; + enable_dismiss = false; + } + ImGui::PopStyleColor(2); + } + + void d500_autocalib_notification_model::draw_abort(ux_window& win, int x, int y) + { + ImGui::SetCursorScreenPos({ float(x + width - 105), float(y + height - 25) }); + + std::string id = rsutils::string::from() << "Abort" << "##" << index; + + + ImGui::SetNextWindowPos({ float(x + width - 125), float(y + height - 25) }); + ImGui::SetNextWindowSize({ 120, 70 }); + + if (ImGui::Button(id.c_str(), { 100, 20 })) + { + update_state = RS2_CALIB_STATE_ABORT; + } + if (ImGui::IsItemHovered()) + { + ImGui::SetTooltip("Abort Calibration Process"); + } + } + + void d500_autocalib_notification_model::update_ui_after_abort_called(ux_window& win, int x, int y) + { + ImGui::Text("%s", "Calibration Aborting"); + ImGui::SetCursorScreenPos({ float(x + 10), float(y + 50) }); + ImGui::PushFont(win.get_large_font()); + std::string txt = rsutils::string::from() << textual_icons::stop; + ImGui::Text("%s", txt.c_str()); + ImGui::PopFont(); + + ImGui::SetCursorScreenPos({ float(x + 40), float(y + 50) }); + if (_has_abort_succeeded) + { + ImGui::Text("%s", "Camera Calibration Aborted Successfully"); + } + else + { + ImGui::Text("%s", "Camera Calibration Could not be Aborted"); + } + enable_dismiss = true; + } + + void d500_autocalib_notification_model::update_ui_on_calibration_complete(ux_window& win, int x, int y) + { + ImGui::Text("%s", "Calibration Complete"); + + ImGui::SetCursorScreenPos({ float(x + 10), float(y + 35) }); + ImGui::PushFont(win.get_large_font()); + std::string txt = rsutils::string::from() << textual_icons::throphy; + ImGui::Text("%s", txt.c_str()); + ImGui::PopFont(); + + ImGui::SetCursorScreenPos({ float(x + 40), float(y + 35) }); + + ImGui::Text("%s", "Camera Calibration Applied Successfully"); + } +} diff --git a/common/d500-on-chip-calib.h b/common/d500-on-chip-calib.h new file mode 100644 index 0000000000..c86c3afd88 --- /dev/null +++ b/common/d500-on-chip-calib.h @@ -0,0 +1,102 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "notifications.h" +#include +#include "../src/algo.h" + +#include +#include + +namespace rs2 +{ + class viewer_model; + class subdevice_model; + struct subdevice_ui_selection; + + // On-chip Calibration manager owns the background thread + // leading the calibration process + // It is controlled by autocalib_notification_model UI object + // that invokes the process when needed + class d500_on_chip_calib_manager : public process_manager + { + public: + d500_on_chip_calib_manager(viewer_model& viewer, std::shared_ptr sub, device_model& model, device dev); + + enum calib_action + { + RS2_CALIB_ACTION_ON_CHIP_CALIB, // On-Chip calibration + RS2_CALIB_ACTION_ON_CHIP_CALIB_DRY_RUN, // Dry Run + RS2_CALIB_ACTION_ON_CHIP_CALIB_ABORT // Abort + }; + + calib_action action = RS2_CALIB_ACTION_ON_CHIP_CALIB; + + std::shared_ptr _sub; + + void reset_device() { _dev.hardware_reset(); } + bool abort(); + void prepare_for_calibration(); + std::string get_device_pid() const; + + private: + void process_flow(std::function cleanup, invoker invoke) override; + std::string convert_action_to_json_string(); + + template + void set_option_if_needed(T& sensor, rs2_option opt, float required_value); + device _dev; + device_model& _model; + }; + + template + void d500_on_chip_calib_manager::set_option_if_needed(T& sensor, + rs2_option opt, float required_value) + { + auto curr_value = sensor.get_option(opt); + if (curr_value != required_value) + { + sensor.set_option(opt, required_value); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + curr_value = sensor.get_option(opt); + if (curr_value != required_value) + { + std::stringstream s; + s << "Could not set " << rs2_option_to_string(opt) << " to " << required_value; + throw std::runtime_error(s.str().c_str()); + } + } + } + + // Auto-calib notification model is managing the UI state-machine + // controling auto-calibration + struct d500_autocalib_notification_model : public process_notification_model + { + enum auto_calib_ui_state + { + RS2_CALIB_STATE_INIT_CALIB, // First screen + RS2_CALIB_STATE_FAILED, // Failed, show _error_message + RS2_CALIB_STATE_COMPLETE, // After write, quick blue notification + RS2_CALIB_STATE_CALIB_IN_PROCESS,// Calibration in process... Shows progressbar + RS2_CALIB_STATE_INIT_DRY_RUN, + RS2_CALIB_STATE_ABORT, + RS2_CALIB_STATE_ABORT_CALLED + }; + + d500_autocalib_notification_model(std::string name, std::shared_ptr manager, bool expaned); + + d500_on_chip_calib_manager& get_manager() { return *std::dynamic_pointer_cast(update_manager); } + void draw_content(ux_window& win, int x, int y, float t, std::string& error_message) override; + int calc_height() override; + void calibration_button(ux_window& win, int x, int y, int bar_width); + void draw_abort(ux_window& win, int x, int y); + void update_ui_after_abort_called(ux_window& win, int x, int y); + void update_ui_on_calibration_complete(ux_window& win, int x, int y); + std::string _error_message = ""; + bool reset_called = false; + bool _has_abort_succeeded = false; + }; + +} diff --git a/common/device-model.cpp b/common/device-model.cpp index 2bd3444e46..036376b33b 100644 --- a/common/device-model.cpp +++ b/common/device-model.cpp @@ -18,6 +18,7 @@ #include #include "viewer.h" #include "on-chip-calib.h" +#include "d500-on-chip-calib.h" #include "subdevice-model.h" #include "device-model.h" @@ -1408,329 +1409,7 @@ namespace rs2 } } - bool has_autocalib = false; - for (auto&& sub : subdevices) - { - if (sub->supports_on_chip_calib() && !has_autocalib) - { - something_to_show = true; - - std::string device_pid = sub->s->supports(RS2_CAMERA_INFO_PRODUCT_ID) ? sub->s->get_info(RS2_CAMERA_INFO_PRODUCT_ID) : "unknown"; - std::string device_usb_type = sub->s->supports(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR) ? sub->s->get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR) : "unknown"; - - bool show_disclaimer = val_in_range(device_pid, { std::string("0AD2"), std::string("0AD3") }); // Specific for D410/5 - bool disable_fl_cal = (((device_pid == "0B5C") || show_disclaimer) && - (!starts_with(device_usb_type, "3."))); // D410/D15/D455@USB2 - - if (ImGui::Selectable("On-Chip Calibration")) - { - try - { - if (show_disclaimer) - { - auto disclaimer_notice = std::make_shared(); - viewer.not_model->add_notification(disclaimer_notice); - } - - auto manager = std::make_shared(viewer, sub, *this, dev); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); - n->forced = true; - n->update_state = autocalib_notification_model::RS2_CALIB_STATE_SELF_INPUT; - - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); - - related_notifications.push_back(n); - } - catch (const error& e) - { - error_message = error_to_string(e); - } - catch (const std::exception& e) - { - error_message = e.what(); - } - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("This will improve the depth noise.\n" - "Point at a scene that normally would have > 50 %% valid depth pixels,\n" - "then press calibrate." - "The health-check will be calculated.\n" - "If >0.25 we recommend applying the new calibration.\n" - "\"White wall\" mode should only be used when pointing at a flat white wall with projector on"); - - if (ImGui::Selectable("Focal Length Calibration")) - { - try - { - if (disable_fl_cal) - { - auto disable_fl_notice = std::make_shared(); - viewer.not_model->add_notification(disable_fl_notice); - } - else - { - std::shared_ptr< subdevice_model> sub_color; - for (auto&& sub2 : subdevices) - { - if (sub2->s->is()) - { - sub_color = sub2; - break; - } - } - - if (show_disclaimer) - { - auto disclaimer_notice = std::make_shared(); - viewer.not_model->add_notification(disclaimer_notice); - } - auto manager = std::make_shared(viewer, sub, *this, dev, sub_color); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); - n->forced = true; - n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_INPUT; - - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); - - related_notifications.push_back(n); - manager->start_fl_viewer(); - } - } - catch (const error& e) - { - error_message = error_to_string(e); - } - catch (const std::exception& e) - { - error_message = e.what(); - } - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Focal length calibration is used to adjust camera focal length with specific target."); - - if (ImGui::Selectable("Tare Calibration")) - { - try - { - if (show_disclaimer) - { - auto disclaimer_notice = std::make_shared(); - viewer.not_model->add_notification(disclaimer_notice); - } - auto manager = std::make_shared(viewer, sub, *this, dev); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); - n->forced = true; - n->update_state = autocalib_notification_model::RS2_CALIB_STATE_TARE_INPUT; - - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); - - related_notifications.push_back(n); - } - catch (const error& e) - { - error_message = error_to_string(e); - } - catch (const std::exception& e) - { - error_message = e.what(); - } - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Tare calibration is used to adjust camera absolute distance to flat target.\n" - "User needs either to enter the known ground truth or use the get button\n" - "with specific target to get the ground truth."); - -//#define UVMAP_CAL -#ifdef UVMAP_CAL // Disabled due to stability and maturity levels - try - { - for (auto&& sub2 : subdevices) - { - if (sub2->s->is()) - { - if (ImGui::Selectable("UV-Mapping Calibration")) - { - if (show_disclaimer) - { - auto disclaimer_notice = std::make_shared(); - viewer.not_model->add_notification(disclaimer_notice); - } - auto manager = std::make_shared(viewer, sub, *this, dev, sub2, sub2->uvmapping_calib_full); - auto n = std::make_shared("", manager, false); - viewer.not_model->add_notification(n); - n->forced = true; - n->update_state = autocalib_notification_model::RS2_CALIB_STATE_UVMAPPING_INPUT; - - for (auto&& n : related_notifications) - if (dynamic_cast(n.get())) - n->dismiss(false); - - related_notifications.push_back(n); - manager->start_uvmapping_viewer(); - } - - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("UV-Mapping calibration is used to improve UV-Mapping with specific target."); - } - } - } - catch (const error& e) - { - error_message = error_to_string(e); - } - catch (const std::exception& e) - { - error_message = e.what(); - } -#endif //UVMAP_CAL - - //if (ImGui::Selectable("Focal Length Plus Calibration")) - //{ - // try - // { - // std::shared_ptr< subdevice_model> sub_color; - // for (auto&& sub2 : subdevices) - // { - // if (sub2->s->is()) - // { - // sub_color = sub2; - // break; - // } - // } - - // auto manager = std::make_shared(viewer, sub, *this, dev, sub_color); - // auto n = std::make_shared("", manager, false); - - // viewer.not_model->add_notification(n); - // n->forced = true; - // n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_PLUS_INPUT; - - // for (auto&& n : related_notifications) - // if (dynamic_cast(n.get())) - // n->dismiss(false); - - // related_notifications.push_back(n); - // manager->start_fl_plus_viewer(); - // } - // catch (const error& e) - // { - // error_message = error_to_string(e); - // } - // catch (const std::exception& e) - // { - // error_message = e.what(); - // } - //} - //if (ImGui::IsItemHovered()) - // ImGui::SetTooltip("Focal length plus calibration is used to adjust camera focal length and principal points with specific target."); - - if (_calib_model.supports()) - { - if (ImGui::Selectable("Calibration Data")) - { - _calib_model.open(); - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Access low level camera calibration parameters"); - } - - if (auto fwlogger = dev.as()) - { - if (ImGui::Selectable("Recover Logs from Flash")) - { - try - { - bool has_parser = false; - std::string hwlogger_xml = config_file::instance().get(configurations::viewer::hwlogger_xml); - std::ifstream f(hwlogger_xml.c_str()); - if (f.good()) - { - try - { - std::string str((std::istreambuf_iterator(f)), - std::istreambuf_iterator()); - fwlogger.init_parser(str); - has_parser = true; - } - catch (const std::exception& ex) - { - viewer.not_model->output.add_log( - RS2_LOG_SEVERITY_WARN, - __FILE__, - __LINE__, - rsutils::string::from() - << "Invalid Hardware Logger XML at '" << hwlogger_xml - << "': " << ex.what() << "\nEither configure valid XML or remove it" ); - } - } - - auto message = fwlogger.create_message(); - - while (fwlogger.get_flash_log(message)) - { - auto parsed = fwlogger.create_parsed_message(); - auto parsed_ok = false; - - if (has_parser) - { - if (fwlogger.parse_log(message, parsed)) - { - parsed_ok = true; - - viewer.not_model->output.add_log( message.get_severity(), - parsed.file_name(), - parsed.line(), - rsutils::string::from() - << "FW-LOG [" << parsed.thread_name() - << "] " << parsed.message() ); - } - } - - if (!parsed_ok) - { - std::stringstream ss; - for (auto& elem : message.data()) - ss << std::setfill('0') << std::setw(2) << std::hex << static_cast(elem) << " "; - viewer.not_model->output.add_log(message.get_severity(), __FILE__, 0, ss.str()); - } - } - } - catch(const std::exception& ex) - { - viewer.not_model->output.add_log( - RS2_LOG_SEVERITY_WARN, - __FILE__, - __LINE__, - rsutils::string::from() << "Failed to fetch firmware logs: " << ex.what() ); - } - } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Recovers last set of firmware logs prior to camera shutdown / disconnect"); - } - - has_autocalib = true; - } - } - if (!has_autocalib) - { - bool selected = false; - something_to_show = true; - ImGui::Selectable("On-Chip Calibration", &selected, ImGuiSelectableFlags_Disabled); - ImGui::Selectable("Tare Calibration", &selected, ImGuiSelectableFlags_Disabled); - } - - if (!something_to_show) - { - ImGui::Text("This device has no additional options"); - } + draw_device_panel_auto_calib(viewer, something_to_show, error_message); ImGui::PopStyleColor(); @@ -3306,4 +2985,436 @@ namespace rs2 _changes.pop(); return true; } + + void device_model::draw_device_panel_auto_calib(viewer_model& viewer, bool& something_to_show, + std::string& error_message) + { + bool has_autocalib = false; + bool is_d500 = dev.supports(RS2_CAMERA_INFO_PRODUCT_LINE) && (std::string(dev.get_info(RS2_CAMERA_INFO_PRODUCT_LINE)) == "D500"); + if (is_d500) + { + has_autocalib = draw_device_panel_auto_calib_d500(viewer, something_to_show, error_message); + } + else + { + has_autocalib = draw_device_panel_auto_calib_d400(viewer, something_to_show, error_message); + } + + if (!has_autocalib) + { + bool selected = false; + something_to_show = true; + ImGui::Selectable("On-Chip Calibration", &selected, ImGuiSelectableFlags_Disabled); + ImGui::Selectable("Tare Calibration", &selected, ImGuiSelectableFlags_Disabled); + } + + if (!something_to_show) + { + ImGui::Text("This device has no additional options"); + } + } + + bool device_model::draw_device_panel_auto_calib_d400(viewer_model& viewer, bool& something_to_show, + std::string& error_message) + { + bool has_autocalib = false; + for (auto&& sub : subdevices) + { + if (sub->supports_on_chip_calib() && !has_autocalib) + { + something_to_show = true; + + std::string device_pid = sub->s->supports(RS2_CAMERA_INFO_PRODUCT_ID) ? sub->s->get_info(RS2_CAMERA_INFO_PRODUCT_ID) : "unknown"; + std::string device_usb_type = sub->s->supports(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR) ? sub->s->get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR) : "unknown"; + + bool show_disclaimer = val_in_range(device_pid, { std::string("0AD2"), std::string("0AD3") }); // Specific for D410/5 + bool disable_fl_cal = (((device_pid == "0B5C") || show_disclaimer) && + (!starts_with(device_usb_type, "3."))); // D410/D15/D455@USB2 + + if (ImGui::Selectable("On-Chip Calibration")) + { + try + { + if (show_disclaimer) + { + auto disclaimer_notice = std::make_shared(); + viewer.not_model->add_notification(disclaimer_notice); + } + + + auto manager = std::make_shared(viewer, sub, *this, dev); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_SELF_INPUT; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("This will improve the depth noise.\n" + "Point at a scene that normally would have > 50 %% valid depth pixels,\n" + "then press calibrate." + "The health-check will be calculated.\n" + "If >0.25 we recommend applying the new calibration.\n" + "\"White wall\" mode should only be used when pointing at a flat white wall with projector on"); + + if (ImGui::Selectable("Focal Length Calibration")) + { + try + { + if (disable_fl_cal) + { + auto disable_fl_notice = std::make_shared(); + viewer.not_model->add_notification(disable_fl_notice); + } + else + { + std::shared_ptr< subdevice_model> sub_color; + for (auto&& sub2 : subdevices) + { + if (sub2->s->is()) + { + sub_color = sub2; + break; + } + } + + if (show_disclaimer) + { + auto disclaimer_notice = std::make_shared(); + viewer.not_model->add_notification(disclaimer_notice); + } + auto manager = std::make_shared(viewer, sub, *this, dev, sub_color); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_INPUT; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + manager->start_fl_viewer(); + } + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Focal length calibration is used to adjust camera focal length with specific target."); + + if (ImGui::Selectable("Tare Calibration")) + { + try + { + if (show_disclaimer) + { + auto disclaimer_notice = std::make_shared(); + viewer.not_model->add_notification(disclaimer_notice); + } + auto manager = std::make_shared(viewer, sub, *this, dev); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_TARE_INPUT; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Tare calibration is used to adjust camera absolute distance to flat target.\n" + "User needs either to enter the known ground truth or use the get button\n" + "with specific target to get the ground truth."); + + //#define UVMAP_CAL +#ifdef UVMAP_CAL // Disabled due to stability and maturity levels + try + { + for (auto&& sub2 : subdevices) + { + if (sub2->s->is()) + { + if (ImGui::Selectable("UV-Mapping Calibration")) + { + if (show_disclaimer) + { + auto disclaimer_notice = std::make_shared(); + viewer.not_model->add_notification(disclaimer_notice); + } + auto manager = std::make_shared(viewer, sub, *this, dev, sub2, sub2->uvmapping_calib_full); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = autocalib_notification_model::RS2_CALIB_STATE_UVMAPPING_INPUT; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + manager->start_uvmapping_viewer(); + } + + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("UV-Mapping calibration is used to improve UV-Mapping with specific target."); + } + } + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } +#endif //UVMAP_CAL + + //if (ImGui::Selectable("Focal Length Plus Calibration")) + //{ + // try + // { + // std::shared_ptr< subdevice_model> sub_color; + // for (auto&& sub2 : subdevices) + // { + // if (sub2->s->is()) + // { + // sub_color = sub2; + // break; + // } + // } + + // auto manager = std::make_shared(viewer, sub, *this, dev, sub_color); + // auto n = std::make_shared("", manager, false); + + // viewer.not_model->add_notification(n); + // n->forced = true; + // n->update_state = autocalib_notification_model::RS2_CALIB_STATE_FL_PLUS_INPUT; + + // for (auto&& n : related_notifications) + // if (dynamic_cast(n.get())) + // n->dismiss(false); + + // related_notifications.push_back(n); + // manager->start_fl_plus_viewer(); + // } + // catch (const error& e) + // { + // error_message = error_to_string(e); + // } + // catch (const std::exception& e) + // { + // error_message = e.what(); + // } + //} + //if (ImGui::IsItemHovered()) + // ImGui::SetTooltip("Focal length plus calibration is used to adjust camera focal length and principal points with specific target."); + + if (_calib_model.supports()) + { + if (ImGui::Selectable("Calibration Data")) + { + _calib_model.open(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Access low level camera calibration parameters"); + } + + if (auto fwlogger = dev.as()) + { + if (ImGui::Selectable("Recover Logs from Flash")) + { + try + { + bool has_parser = false; + std::string hwlogger_xml = config_file::instance().get(configurations::viewer::hwlogger_xml); + std::ifstream f(hwlogger_xml.c_str()); + if (f.good()) + { + try + { + std::string str((std::istreambuf_iterator(f)), + std::istreambuf_iterator()); + fwlogger.init_parser(str); + has_parser = true; + } + catch (const std::exception& ex) + { + viewer.not_model->output.add_log( + RS2_LOG_SEVERITY_WARN, + __FILE__, + __LINE__, + rsutils::string::from() + << "Invalid Hardware Logger XML at '" << hwlogger_xml + << "': " << ex.what() << "\nEither configure valid XML or remove it"); + } + } + + auto message = fwlogger.create_message(); + + while (fwlogger.get_flash_log(message)) + { + auto parsed = fwlogger.create_parsed_message(); + auto parsed_ok = false; + + if (has_parser) + { + if (fwlogger.parse_log(message, parsed)) + { + parsed_ok = true; + + viewer.not_model->output.add_log(message.get_severity(), + parsed.file_name(), + parsed.line(), + rsutils::string::from() + << "FW-LOG [" << parsed.thread_name() + << "] " << parsed.message()); + } + } + + if (!parsed_ok) + { + std::stringstream ss; + for (auto& elem : message.data()) + ss << std::setfill('0') << std::setw(2) << std::hex << static_cast(elem) << " "; + viewer.not_model->output.add_log(message.get_severity(), __FILE__, 0, ss.str()); + } + } + } + catch (const std::exception& ex) + { + viewer.not_model->output.add_log( + RS2_LOG_SEVERITY_WARN, + __FILE__, + __LINE__, + rsutils::string::from() << "Failed to fetch firmware logs: " << ex.what()); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Recovers last set of firmware logs prior to camera shutdown / disconnect"); + } + + has_autocalib = true; + } + } + return has_autocalib; + } + + bool device_model::draw_device_panel_auto_calib_d500(viewer_model& viewer, bool& something_to_show, + std::string& error_message) + { + bool has_autocalib = false; + + bool streaming = is_streaming(); + ImGuiSelectableFlags avoid_selection_flag = (streaming) ? ImGuiSelectableFlags_Disabled : 0; + + for (auto&& sub : subdevices) + { + if (sub->supports_on_chip_calib() && !has_autocalib) + { + if (ImGui::Selectable("On-Chip Calibration", false, avoid_selection_flag)) + { + try + { + auto manager = std::make_shared(viewer, sub, *this, dev); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = d500_autocalib_notification_model::RS2_CALIB_STATE_INIT_CALIB; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } + } + if (ImGui::IsItemHovered()) + { + std::string tooltip = rsutils::string::from() + << "On-Chip Calibration" + << (streaming ? " (Disabled while streaming)" : ""); + ImGui::SetTooltip("%s", tooltip.c_str()); + } + + if (ImGui::Selectable("Dry Run On-Chip Calibration", false, avoid_selection_flag)) + { + try + { + auto manager = std::make_shared(viewer, sub, *this, dev); + auto n = std::make_shared("", manager, false); + viewer.not_model->add_notification(n); + n->forced = true; + n->update_state = d500_autocalib_notification_model::RS2_CALIB_STATE_INIT_DRY_RUN; + + for (auto&& n : related_notifications) + if (dynamic_cast(n.get())) + n->dismiss(false); + + related_notifications.push_back(n); + } + catch (const error& e) + { + error_message = error_to_string(e); + } + catch (const std::exception& e) + { + error_message = e.what(); + } + } + if (ImGui::IsItemHovered()) + { + std::string tooltip = rsutils::string::from() + << "Dry Run On-Chip Calibration" + << (streaming ? " (Disabled while streaming)" : ""); + ImGui::SetTooltip("%s", tooltip.c_str()); + } + + has_autocalib = true; + continue; + } + } + return has_autocalib; + } + } diff --git a/common/device-model.h b/common/device-model.h index 6c637747e0..11401f8267 100644 --- a/common/device-model.h +++ b/common/device-model.h @@ -439,6 +439,10 @@ namespace rs2 std::shared_ptr< sw_update::dev_updates_profile::update_profile > update_profile, bool reset_delay = false ); + void draw_device_panel_auto_calib(viewer_model& viewer, bool& something_to_show, std::string& error_message); + bool draw_device_panel_auto_calib_d400(viewer_model& viewer, bool& something_to_show, std::string& error_message); + bool draw_device_panel_auto_calib_d500(viewer_model& viewer, bool& something_to_show, std::string& error_message); + std::shared_ptr _recorder; std::vector> live_subdevices; rsutils::time::periodic_timer _update_readonly_options_timer; diff --git a/src/ds/d500/CMakeLists.txt b/src/ds/d500/CMakeLists.txt index 253217d7ae..b36adf1b7d 100644 --- a/src/ds/d500/CMakeLists.txt +++ b/src/ds/d500/CMakeLists.txt @@ -11,6 +11,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/hw_monitor_extended_buffers.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-options.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-fw-update-device.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-auto-calibration.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-device.h" "${CMAKE_CURRENT_LIST_DIR}/d500-color.h" "${CMAKE_CURRENT_LIST_DIR}/d500-motion.h" @@ -20,4 +21,5 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/d500-options.h" "${CMAKE_CURRENT_LIST_DIR}/d500-info.h" "${CMAKE_CURRENT_LIST_DIR}/d500-fw-update-device.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-auto-calibration.h" ) diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp new file mode 100644 index 0000000000..905d5a5a0d --- /dev/null +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -0,0 +1,280 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + + +#include "d500-auto-calibration.h" +#include "d500-private.h" +#include + + +namespace librealsense +{ + static const std::string d500_calibration_state_strings[] = { + "Idle", + "In Process", + "Done Success", + "Done Failure", + "Flash Update", + "Complete" + }; + + static const std::string d500_calibration_result_strings[] = { + "Unkown", + "Success", + "Failed to Converge", + "Failed to Run" + }; + +#pragma pack(push, 1) + struct d500_calibration_answer + { + uint8_t calibration_state; + int8_t calibration_progress; + uint8_t calibration_result; + ds::d500_coefficients_table depth_calibration; + }; +#pragma pack(pop) + + + d500_auto_calibrated::d500_auto_calibrated() : + _mode (d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RESERVED), + _state (d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE), + _result(d500_calibration_result::RS2_D500_CALIBRATION_RESULT_UNKNOWN) + {} + + bool d500_auto_calibrated::check_buffer_size_from_get_calib_status(std::vector res) const + { + // the GET_CALIB_STATUS command will return: + // - 3 bytes during the whole process + // - 515 bytes (3 bytes + 512 bytes of the depth calibration) when the state is Complete + + bool is_size_ok = false; + if (res.size() > 1) + { + if (res[0] < static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE) && + res.size() == (sizeof(d500_calibration_answer) - sizeof(ds::d500_coefficients_table))) + is_size_ok = true; + + if (res[0] == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE) && + res.size() == sizeof(d500_calibration_answer)) + is_size_ok = true; + } + return is_size_ok; + } + + void d500_auto_calibrated::check_preconditions_and_set_state() + { + // moreover, relevant only for d585s, not for d555e + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN || + _mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + { + // calibration state to be IDLE or COMPLETE + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + + // checking size of received buffer + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_result = *reinterpret_cast(res.data()); + _state = static_cast(calib_result.calibration_state); + if (!(_state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE || + _state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE)) + { + LOG_ERROR("Calibration State is not Idle nor Complete - pleare restart the device"); + throw std::runtime_error("OCC triggerred when Calibration State is not Idle not Complete"); + } + } + + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + { + // calibration state to be IN_PROCESS + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_result = *reinterpret_cast(res.data()); + _state = static_cast(calib_result.calibration_state); + if (!(_state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_PROCESS)) + { + LOG_ERROR("Calibration State is not In Process - so it could not be aborted"); + throw std::runtime_error("OCC aborted when Calibration State is not In Process"); + } + } + } + + void d500_auto_calibrated::get_mode_from_json(const std::string& json) + { + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN; + if (json.find("dry run") != std::string::npos) + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN; + else if (json.find("abort") != std::string::npos) + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT; + } + + std::vector d500_auto_calibrated::run_on_chip_calibration(int timeout_ms, std::string json, + float* const health, rs2_update_progress_callback_sptr progress_callback) + { + std::vector res; + try + { + get_mode_from_json(json); + + // checking preconditions + check_preconditions_and_set_state(); + + // sending command to start calibration + res = _hw_monitor->send(command{ ds::SET_CALIB_MODE, static_cast(_mode), 1 /*always*/}); + + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN || + _mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + { + res = update_calibration_status(timeout_ms, progress_callback); + } + else if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + { + res = update_abort_status(); + } + } + catch(...) + { + std::string error_message_prefix = "\nRUN OCC "; + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + error_message_prefix = "\nDRY RUN OCC "; + else if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + error_message_prefix = "\nABORT OCC "; + + throw std::runtime_error(rsutils::string::from() << error_message_prefix + "Could not be triggered"); + } + return res; + } + + std::vector d500_auto_calibrated::update_calibration_status(int timeout_ms, + rs2_update_progress_callback_sptr progress_callback) + { + auto start_time = std::chrono::high_resolution_clock::now(); + std::vector res; + d500_calibration_answer calib_answer; + do + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + calib_answer = *reinterpret_cast(res.data()); + _state = static_cast(calib_answer.calibration_state); + _result = static_cast(calib_answer.calibration_result); + LOG_INFO("Calibration in progress - State = " << d500_calibration_state_strings[static_cast(_state)] + << ", progress = " << static_cast(calib_answer.calibration_progress) + << ", result = " << d500_calibration_result_strings[static_cast(_result)]); + if (progress_callback) + { + progress_callback->on_update_progress(calib_answer.calibration_progress); + } + bool is_timed_out(std::chrono::high_resolution_clock::now() - start_time > std::chrono::milliseconds(timeout_ms)); + if (is_timed_out) + { + throw std::runtime_error("OCC Calibration Timeout"); + } + } while (_state != d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE); + + // printing new calibration to log + if (_result == d500_calibration_result::RS2_D500_CALIBRATION_RESULT_SUCCESS) + { + auto depth_calib = *reinterpret_cast(&calib_answer.depth_calibration); + LOG_INFO("Depth new Calibration = \n" + depth_calib.to_string()); + } + else + { + LOG_ERROR("Calibration completed but algorithm failed"); + throw std::runtime_error("Calibration completed but algorithm failed"); + } + + return res; + } + + std::vector d500_auto_calibrated::update_abort_status() + { + std::vector ans; + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_answer = *reinterpret_cast(res.data()); + if (calib_answer.calibration_state == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_PROCESS)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + calib_answer = *reinterpret_cast(res.data()); + } + if (calib_answer.calibration_state == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE)) + { + LOG_INFO("Depth Calibration Successfully Aborted"); + // returning success + ans.push_back(1); + } + else + { + LOG_INFO("Depth Calibration Could not be Aborted"); + // returning failure + ans.push_back(0); + } + return ans; + } + + std::vector d500_auto_calibrated::run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Tare Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Process Calibration Frame not applicable for this device"); + } + + std::vector d500_auto_calibrated::get_calibration_table() const + { + throw std::runtime_error(rsutils::string::from() << "Get Calibration Table not applicable for this device"); + } + + void d500_auto_calibrated::write_calibration() const + { + throw std::runtime_error(rsutils::string::from() << "Write Calibration not applicable for this device"); + } + + void d500_auto_calibrated::set_calibration_table(const std::vector& calibration) + { + throw std::runtime_error(rsutils::string::from() << "Set Calibration Table not applicable for this device"); + } + + void d500_auto_calibrated::reset_to_factory_calibration() const + { + throw std::runtime_error(rsutils::string::from() << "Tare Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::run_focal_length_calibration(rs2_frame_queue* left, rs2_frame_queue* right, float target_w, float target_h, + int adjust_both_sides, float *ratio, float * angle, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Focal Length Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only, + float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "UV Map Calibration not applicable for this device"); + } + + float d500_auto_calibrated::calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, + float target_w, float target_h, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Calculate T not applicable for this device"); + } + + void d500_auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr hwm) + { + _hw_monitor = hwm; + } +} diff --git a/src/ds/d500/d500-auto-calibration.h b/src/ds/d500/d500-auto-calibration.h new file mode 100644 index 0000000000..93fd016a09 --- /dev/null +++ b/src/ds/d500/d500-auto-calibration.h @@ -0,0 +1,73 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "auto-calibrated-device.h" +#include "../../core/advanced_mode.h" + + +namespace librealsense +{ +#pragma pack(push, 1) +#pragma pack(1) + +#pragma pack(pop) + + class d500_auto_calibrated : public auto_calibrated_interface + { + public: + d500_auto_calibrated(); + void write_calibration() const override; + std::vector run_on_chip_calibration(int timeout_ms, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector get_calibration_table() const override; + void set_calibration_table(const std::vector& calibration) override; + void reset_to_factory_calibration() const override; + std::vector run_focal_length_calibration(rs2_frame_queue* left, rs2_frame_queue* right, float target_w, float target_h, + int adjust_both_sides, float* ratio, float* angle, rs2_update_progress_callback_sptr progress_callback) override; + std::vector run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only, + float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) override; + float calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, + float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override; + void set_hw_monitor_for_auto_calib(std::shared_ptr hwm); + + enum class d500_calibration_state + { + RS2_D500_CALIBRATION_STATE_IDLE = 0, + RS2_D500_CALIBRATION_STATE_PROCESS, + RS2_D500_CALIBRATION_STATE_DONE_SUCCESS, + RS2_D500_CALIBRATION_STATE_DONE_FAILURE, + RS2_D500_CALIBRATION_STATE_FLASH_UPDATE, + RS2_D500_CALIBRATION_STATE_COMPLETE + }; + + enum class d500_calibration_result + { + RS2_D500_CALIBRATION_RESULT_UNKNOWN = 0, + RS2_D500_CALIBRATION_RESULT_SUCCESS, + RS2_D500_CALIBRATION_RESULT_FAILED_TO_CONVERGE, + RS2_D500_CALIBRATION_RESULT_FAILED_TO_RUN + }; + + private: + enum class d500_calibration_mode + { + RS2_D500_CALIBRATION_MODE_RESERVED = 0, + RS2_D500_CALIBRATION_MODE_RUN, + RS2_D500_CALIBRATION_MODE_ABORT, + RS2_D500_CALIBRATION_MODE_DRY_RUN + }; + + void check_preconditions_and_set_state(); + bool check_buffer_size_from_get_calib_status(std::vector res) const; + void get_mode_from_json(const std::string& json); + std::vector update_calibration_status(int timeout_ms, rs2_update_progress_callback_sptr progress_callback); + std::vector update_abort_status(); + std::shared_ptr _hw_monitor; + d500_calibration_mode _mode; + d500_calibration_state _state; + d500_calibration_result _result; + }; +} diff --git a/src/ds/d500/d500-private.cpp b/src/ds/d500/d500-private.cpp index 3626ff52b1..45744d70b5 100644 --- a/src/ds/d500/d500-private.cpp +++ b/src/ds/d500/d500-private.cpp @@ -272,5 +272,80 @@ namespace librealsense return{ rect_rot_mat,trans_vector }; } + std::string d500_coefficients_table::to_string() const + { + std::string res; + res += "\n--- header ---\n"; + res += header.to_string(); + res += "--- left coeffs ---\n"; + res += left_coefficients_table.to_string(); + res += "--- right coeffs ---\n"; + res += right_coefficients_table.to_string(); + res += "--- --- ---\n"; + res += "baseline:\t" + std::to_string(baseline) + "\n"; + res += "translation_dir:\t" + std::to_string(translation_dir) + "\n"; + res += "realignement_essential:\t" + std::to_string(realignement_essential) + "\n"; + res += "vertical_shift:\t" + std::to_string(vertical_shift) + "\n"; + res += "--- rectified intrinsics ---\n"; + res += rectified_intrinsics.to_string(); + return res; + } + + std::string single_sensor_coef_table::to_string() const + { + std::string res; + res += "-- base_instrinsics --\n"; + res += base_instrinsics.to_string(); + res += "distortion_non_parametric:" + std::to_string(distortion_non_parametric) + "\n"; + res += "distortion_model:\t\t" + std::to_string(int(distortion_model)) + "\n"; + for (int i = 0; i < 13; ++i) + { + res += "distortion_coeffs[" + std::to_string(i) + "]:\t" + std::to_string(distortion_coeffs[i]) + "\n"; + } + res += "radial_distortion_lut_range_degs:\t" + std::to_string(radial_distortion_lut_range_degs) + "\n"; + res += "radial_distortion_lut_focal_length:\t" + std::to_string(radial_distortion_lut_focal_length) + "\n"; + res += "-- undist config --\n"; + res += undist_config.to_string(); + res += "rotation_matrix[0]:\t" + std::to_string(rotation_matrix.x.x) + "\n"; + res += "rotation_matrix[1]:\t" + std::to_string(rotation_matrix.x.y) + "\n"; + res += "rotation_matrix[2]:\t" + std::to_string(rotation_matrix.x.z) + "\n"; + res += "rotation_matrix[3]:\t" + std::to_string(rotation_matrix.y.x) + "\n"; + res += "rotation_matrix[4]:\t" + std::to_string(rotation_matrix.y.y) + "\n"; + res += "rotation_matrix[5]:\t" + std::to_string(rotation_matrix.y.z) + "\n"; + res += "rotation_matrix[6]:\t" + std::to_string(rotation_matrix.z.x) + "\n"; + res += "rotation_matrix[7]:\t" + std::to_string(rotation_matrix.z.y) + "\n"; + res += "rotation_matrix[8]:\t" + std::to_string(rotation_matrix.z.z) + "\n"; + return res; + } + + std::string mini_intrinsics::to_string() const + { + std::string res; + res += "image_width:\t" + std::to_string(image_width) + "\n"; + res += "image_height:\t" + std::to_string(image_height) + "\n"; + res += "ppx:\t" + std::to_string(ppx) + "\n"; + res += "ppy:\t" + std::to_string(ppy) + "\n"; + res += "fx:\t" + std::to_string(fx) + "\n"; + res += "fy:\t" + std::to_string(fy) + "\n"; + + return res; + + } + + std::string d500_undist_configuration::to_string() const + { + std::string res; + res += "fx:\t" + std::to_string(fx) + "\n"; + res += "fy:\t" + std::to_string(fy) + "\n"; + res += "x0:\t" + std::to_string(x0) + "\n"; + res += "y0:\t" + std::to_string(y0) + "\n"; + res += "x_shift_in:\t" + std::to_string(x_shift_in) + "\n"; + res += "y_shift_in:\t" + std::to_string(y_shift_in) + "\n"; + res += "x_scale_in:\t" + std::to_string(x_scale_in) + "\n"; + res += "y_scale_in:\t" + std::to_string(y_scale_in) + "\n"; + + return res; + } + } // librealsense::ds } // namespace librealsense diff --git a/src/ds/d500/d500-private.h b/src/ds/d500/d500-private.h index d38a8e784b..f060e94838 100644 --- a/src/ds/d500/d500-private.h +++ b/src/ds/d500/d500-private.h @@ -95,6 +95,8 @@ namespace librealsense uint32_t y_shift_in; uint32_t x_scale_in; uint32_t y_scale_in; + + std::string to_string() const; }; // Calibration implemented according to version 3.1 @@ -106,6 +108,8 @@ namespace librealsense float ppy; /**< Vertical coordinate of the principal point of the image, as a pixel offset from the top edge */ float fx; /**< Focal length of the image plane, as a multiple of pixel width */ float fy; /**< Focal length of the image plane, as a multiple of pixel height */ + + std::string to_string() const; }; // These are the possible values for the calibration table 'distortion_model' field @@ -127,6 +131,8 @@ namespace librealsense float radial_distortion_lut_focal_length; d500_undist_configuration undist_config; float3x3 rotation_matrix; + + std::string to_string() const; }; struct d500_coefficients_table @@ -141,6 +147,8 @@ namespace librealsense int16_t vertical_shift; // in pixels mini_intrinsics rectified_intrinsics; uint8_t reserved[148]; + + std::string to_string() const; }; struct d500_rgb_calibration_table diff --git a/src/ds/ds-private.cpp b/src/ds/ds-private.cpp index e2beb52c39..3618b32d0b 100644 --- a/src/ds/ds-private.cpp +++ b/src/ds/ds-private.cpp @@ -106,5 +106,16 @@ namespace librealsense return rv; } + + std::string table_header::to_string() const + { + std::string res; + res += "version:\t" + std::to_string(version) + "\n"; + res += "table_type:\t" + std::to_string(table_type) + "\n"; + res += "table_size:\t" + std::to_string(table_size) + "\n"; + res += "param:\t" + std::to_string(param) + "\n"; + res += "crc32:\t" + std::to_string(crc32) + "\n"; + return res; + } } // librealsense::ds } // namespace librealsense \ No newline at end of file diff --git a/src/ds/ds-private.h b/src/ds/ds-private.h index 2cde1a23d6..c4862c0c92 100644 --- a/src/ds/ds-private.h +++ b/src/ds/ds-private.h @@ -132,7 +132,9 @@ namespace librealsense SET_HKR_CONFIG_TABLE = 0xA6, // HKR Set Internal sub calibration table GET_HKR_CONFIG_TABLE = 0xA7, // HKR Get Internal sub calibration table CALIBRESTOREEPROM = 0xA8, // HKR Store EEPROM Calibration - GET_FW_LOGS = 0xB4 // Get FW logs extended format + GET_FW_LOGS = 0xB4, // Get FW logs extended format + SET_CALIB_MODE = 0xB8, // Set Calibration Mode + GET_CALIB_STATUS = 0xB9 // Get Calibration Status }; #define TOSTRING(arg) #arg @@ -265,6 +267,8 @@ namespace librealsense uint32_t table_size; // full size including: TOC header + TOC + actual tables uint32_t param; // This field content is defined ny table type uint32_t crc32; // crc of all the actual table data excluding header/CRC + + std::string to_string() const; }; enum ds_rect_resolutions : unsigned short