diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 6e852e1eae..d51c0c0539 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -16,16 +16,20 @@ #include "../src/algo.h" #include "../tools/depth-quality/depth-metrics.h" + +static constexpr float off_value = 0.0f; +static constexpr float on_value = 1.0f; + namespace rs2 { on_chip_calib_manager::on_chip_calib_manager(viewer_model& viewer, std::shared_ptr sub, device_model& model, device dev, std::shared_ptr sub_color, bool uvmapping_calib_full) : process_manager("On-Chip Calibration"), _model(model), _dev(dev), _sub(sub), _viewer(viewer), _sub_color(sub_color), py_px_only(!uvmapping_calib_full) { - device_id_string = "Unknown"; + device_name_string = "Unknown"; if (dev.supports(RS2_CAMERA_INFO_PRODUCT_ID)) { - device_id_string = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); - if (val_in_range(device_id_string, { std::string("0AD3") })) + device_name_string = _dev.get_info( RS2_CAMERA_INFO_NAME ); + if( val_in_range( device_name_string, { std::string( "Intel RealSense D415" ) } ) ) speed = 4; } if (dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION)) @@ -69,6 +73,56 @@ namespace rs2 _sub_color->algo_roi = { 0, 0, 0, 0 }; } } + + void on_chip_calib_manager::save_laser_emitter_state() + { + auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); + if ( it != _sub->options_metadata.end() ) //Option supported + { + laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED ); + } + } + + void on_chip_calib_manager::save_thermal_loop_state() + { + auto it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION ); + if( it != _sub->options_metadata.end() ) // Option supported + { + thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION ); + } + } + + void on_chip_calib_manager::restore_laser_emitter_state() + { + set_laser_emitter_state( laser_status_prev ); + } + + void on_chip_calib_manager::restore_thermal_loop_state() + { + set_thermal_loop_state( thermal_loop_prev ); + } + + void on_chip_calib_manager::set_laser_emitter_state( float value ) + { + // Use options_model::set_option to update GUI after change + std::string ignored_error_message{ "" }; + auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); + if( it != _sub->options_metadata.end() ) // Option supported + { + it->second.set_option( RS2_OPTION_EMITTER_ENABLED, value, ignored_error_message ); + } + } + + void on_chip_calib_manager::set_thermal_loop_state( float value ) + { + // Use options_model::set_option to update GUI after change + std::string ignored_error_message{ "" }; + auto it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION ); + if( it != _sub->options_metadata.end() ) // Option supported + { + it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, value, ignored_error_message ); + } + } void on_chip_calib_manager::stop_viewer(invoker invoke) { @@ -406,12 +460,6 @@ namespace rs2 bool frame_arrived = false; try { - if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - { - thermal_loop_prev = _sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION); - _sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f); - } - bool run_fl_calib = ( (action == RS2_CALIB_ACTION_FL_CALIB) && (w == 1280) && (h == 720)); if (action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH) { @@ -468,10 +516,9 @@ namespace rs2 break; } - if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - _sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f); - if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - _sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f); + // TODO - When implementing UV mapping calibration - should remove from here and handle in process_flow() + set_laser_emitter_state( off_value ); + set_thermal_loop_state( off_value ); } else if (action == RS2_CALIB_ACTION_UVMAPPING_CALIB) { @@ -516,8 +563,8 @@ namespace rs2 break; } - if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - _sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f); + // TODO - When implementing UV mapping calibration - should remove from here and handle in process_flow() + set_laser_emitter_state( off_value ); } else if (action == RS2_CALIB_ACTION_FL_PLUS_CALIB) { @@ -553,8 +600,8 @@ namespace rs2 break; } - if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - _sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f); + // TODO - When implementing FL plus calibration - should remove from here and handle in process_flow() + set_laser_emitter_state( off_value ); } else if (run_fl_calib) { @@ -577,11 +624,6 @@ namespace rs2 } } } - - if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - _sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f); - if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - _sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f); } else { @@ -1423,43 +1465,30 @@ namespace rs2 if ( action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH ) { - //Laser should be turned off during ground truth calculation - //Use options_model::set_option to update GUI after change - std::string ignored_error_message { "" }; - auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); - if ( it != _sub->options_metadata.end() ) //Option supported - { - laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED ); - it->second.set_option( RS2_OPTION_EMITTER_ENABLED, 0.0f, ignored_error_message ); - } + // Laser should be turned off during ground truth calculation + save_laser_emitter_state(); + set_laser_emitter_state( off_value ); get_ground_truth(); - //Restore laser - if ( it != _sub->options_metadata.end() ) //Option supported - { - it->second.set_option( RS2_OPTION_EMITTER_ENABLED, laser_status_prev, ignored_error_message ); - } + restore_laser_emitter_state(); } else { try { - //Save options that are going to change during the calibration - //Use options_model::set_option to update GUI after change - std::string ignored_error_message { "" }; - auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); - if ( it != _sub->options_metadata.end() ) //Option supported - { - laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED ); - it->second.set_option( RS2_OPTION_EMITTER_ENABLED, 1.0f, ignored_error_message ); - } - it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION ); - if ( it != _sub->options_metadata.end() ) //Option supported - { - thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION ); - it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, 0.0f, ignored_error_message ); - } + // Save options that are going to change during the calibration + save_laser_emitter_state(); + save_thermal_loop_state(); + + // Emitter on by default, off for FL calib and for D415 model + float emitter_value = on_value; + if( action == RS2_CALIB_ACTION_FL_CALIB || device_name_string == std::string( "Intel RealSense D415" ) ) + emitter_value = off_value; + set_laser_emitter_state( emitter_value ); + + // Thermal loop should be off during calibration as to not change calibration tables during calibration + set_thermal_loop_state( off_value ); if (action == RS2_CALIB_ACTION_FL_CALIB) calibrate_fl(); @@ -1467,6 +1496,9 @@ namespace rs2 calibrate_uv_mapping(); else calibrate(); + + restore_laser_emitter_state(); + restore_thermal_loop_state(); } catch (...) { @@ -1485,18 +1517,8 @@ namespace rs2 //Restore options that were changed during the calibration. //When calibration is successful options are restored in autocalib_notification_model::draw_content() - //Use options_model::set_option to update GUI after change - std::string ignored_error_message { "" }; - auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); - if ( it != _sub->options_metadata.end() ) //Option supported - { - it->second.set_option( RS2_OPTION_EMITTER_ENABLED, laser_status_prev, ignored_error_message ); - } - it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION ); - if ( it != _sub->options_metadata.end() ) //Option supported - { - it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, thermal_loop_prev, ignored_error_message ); - } + restore_laser_emitter_state(); + restore_thermal_loop_state(); if (_was_streaming) start_viewer(0, 0, 0, invoke); @@ -1851,10 +1873,6 @@ namespace rs2 { get_manager().action = on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB; update_state = update_state_prev; - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev); get_manager().stop_viewer(); } @@ -1891,10 +1909,6 @@ namespace rs2 { get_manager().action = on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB; update_state = update_state_prev; - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev); } else if (update_state == RS2_CALIB_STATE_GET_TARE_GROUND_TRUTH_FAILED) { @@ -2061,11 +2075,6 @@ namespace rs2 if (ImGui::Button(get_button_name.c_str(), { 42.0f, 20.f })) { - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION); - update_state_prev = update_state; update_state = RS2_CALIB_STATE_GET_TARE_GROUND_TRUTH; get_manager().start_gt_viewer(); @@ -2074,7 +2083,7 @@ namespace rs2 ImGui::SetTooltip("%s", "Calculate ground truth for the specific target"); ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 30) }); - get_manager().host_assistance = (get_manager().device_id_string == std::string("ABCD") ); // To be used for MIPI SKU only + get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only bool assistance = (get_manager().host_assistance != 0); if (ImGui::Checkbox("Host Assistance", &assistance)) get_manager().host_assistance = (assistance ? 1 : 0); @@ -2170,7 +2179,7 @@ namespace rs2 // ImGui::SetTooltip("%s", "On-Chip Calibration Extended"); ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 31) }); - get_manager().host_assistance = (get_manager().device_id_string == std::string("ABCD") ); // To be used for MIPI SKU only + get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only bool assistance = (get_manager().host_assistance != 0); ImGui::Checkbox("Host Assistance", &assistance); if (ImGui::IsItemHovered()) @@ -2269,11 +2278,6 @@ namespace rs2 ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) }); if (ImGui::Button(button_name.c_str(), { float(bar_width), 20.f })) { - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION); - get_manager().restore_workspace([this](std::function a) { a(); }); get_manager().reset(); get_manager().retry_times = 0; @@ -2345,30 +2349,8 @@ namespace rs2 } else if (update_state == RS2_CALIB_STATE_CALIB_COMPLETE) { - if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB || - get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB ) - { - //Restore options that were changed during the calibration. - //Use options_model::set_option to update GUI after change - std::string ignored_error_message { "" }; - auto it = get_manager()._sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED ); - if ( it != get_manager()._sub->options_metadata.end() ) //Option supported - { - it->second.set_option( RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev, ignored_error_message ); - } - it = get_manager()._sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION ); - if ( it != get_manager()._sub->options_metadata.end() ) //Option supported - { - it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev, ignored_error_message ); - } - } if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_UVMAPPING_CALIB) { - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev); - ImGui::SetCursorScreenPos({ float(x + 20), float(y + 33) }); ImGui::Text("%s", "Health-Check Number for PX: "); @@ -2453,14 +2435,6 @@ namespace rs2 } else { - if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_FL_CALIB) - { - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev); - } - auto health = get_manager().get_health(); auto recommend_keep = fabs(health) < 0.25f; @@ -2851,11 +2825,6 @@ namespace rs2 ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) }); if (ImGui::Button(button_name.c_str(), { scale * 3, 20.f })) { - if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED)) - get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED); - if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION)) - get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION); - get_manager().restore_workspace([this](std::function a) { a(); }); get_manager().reset(); get_manager().retry_times = 0; diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 5f31f95ceb..efbda26c88 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -72,8 +72,6 @@ namespace rs2 auto_calib_action action = RS2_CALIB_ACTION_ON_CHIP_CALIB; int host_assistance = 0; int step_count_v3 = 256; - float laser_status_prev = 0.0f; - float thermal_loop_prev = 0.f; int fl_step_count = 51; int fy_scan_range = 40; @@ -103,8 +101,7 @@ namespace rs2 const std::string Y8_FORMAT = "Y8"; const std::string Z16_FORMAT = "Z16"; const std::string RGB8_FORMAT = "RGB8"; - std::string device_id_string; - + std::string device_name_string; void calibrate(); void calibrate_fl(); @@ -115,6 +112,13 @@ namespace rs2 void turn_roi_on(); void turn_roi_off(); + void save_laser_emitter_state(); + void save_thermal_loop_state(); + void restore_laser_emitter_state(); + void restore_thermal_loop_state(); + void set_laser_emitter_state( float value ); + void set_thermal_loop_state( float value ); + void start_gt_viewer(); void start_fl_viewer(); void start_uvmapping_viewer(bool b3D = false); @@ -137,6 +141,10 @@ namespace rs2 device _dev; + + float laser_status_prev = 0.0f; + float thermal_loop_prev = 0.0f; + bool _was_streaming = false; bool _synchronized = false; bool _post_processing = false;