From 74a4fc41058b8320ea1efee11c6b4467e5ace314 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Tue, 16 Aug 2022 07:37:56 +0300 Subject: [PATCH 01/17] Debug OCC outliers/ stability --- common/on-chip-calib.cpp | 16 ++++- src/ds5/ds5-auto-calibration.cpp | 103 ++++++++++++++++++++++++++++--- src/ds5/ds5-auto-calibration.h | 1 + 3 files changed, 111 insertions(+), 9 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 1e987e3c86..96f91ce8cd 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -405,6 +405,12 @@ 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) { @@ -859,6 +865,7 @@ namespace rs2 void on_chip_calib_manager::calibrate() { + std::cout << __FUNCTION__ << " was called" << std::endl; int occ_timeout_ms = 9000; if (action == RS2_CALIB_ACTION_ON_CHIP_OB_CALIB || action == RS2_CALIB_ACTION_ON_CHIP_FL_CALIB) { @@ -908,7 +915,7 @@ namespace rs2 ",\n \"apply preset\":" << (apply_preset ? 1 : 0) << ",\n \"accuracy\":" << accuracy << ",\n \"scan only\":" << (host_assistance ? 1 : 0) << - ",\n \"interactive scan\":" << 1 << "}"; + ",\n \"interactive scan\":" << 0 << "}"; } else if (action == RS2_CALIB_ACTION_ON_CHIP_FL_CALIB) { @@ -961,7 +968,7 @@ namespace rs2 bool calib_done(!_new_calib.empty()); - int timeout_sec(30); + int timeout_sec(10); timeout_sec *= (1 + static_cast(action == RS2_CALIB_ACTION_ON_CHIP_CALIB)); // when RS2_CALIB_ACTION_ON_CHIP_CALIB is in interactive-mode the process takes longer. auto start = std::chrono::high_resolution_clock::now(); bool is_timed_out(std::chrono::high_resolution_clock::now() - start > std::chrono::seconds(timeout_sec)); @@ -1527,6 +1534,7 @@ namespace rs2 // Make new calibration active apply_calib(true); + LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); // Capture metrics after auto metrics_after = get_depth_metrics(invoke); @@ -2424,6 +2432,7 @@ namespace rs2 { get_manager().apply_calib(true); // Store the new calibration internally get_manager().keep(); // Flash the new calibration + LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied and flashed")); if (RS2_CALIB_STATE_UVMAPPING_INPUT == update_state) get_manager().reset_device(); // Workaround for reloading color calibration table. Other approach? @@ -2810,6 +2819,7 @@ namespace rs2 { use_new_calib = true; get_manager().apply_calib(true); + LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); } ImGui::SetCursorScreenPos({ float(x + 150), (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_OB_CALIB || get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_FL_CALIB ? float(y + 70) + ImGui::GetTextLineHeightWithSpacing() : (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB ? (get_manager().tare_health ? float(y + 70) : float(y + 15)) + ImGui::GetTextLineHeightWithSpacing() : float(y + 70))) }); @@ -2966,6 +2976,7 @@ namespace rs2 { get_manager().apply_calib(true); use_new_calib = true; + LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); } } @@ -3055,6 +3066,7 @@ namespace rs2 ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, regular_blue); auto s = update_manager->get_log(); + rs2::log(RS2_LOG_SEVERITY_INFO, s.c_str()); ImGui::InputTextMultiline("##autocalib_log", const_cast(s.c_str()), s.size() + 1, { 490,100 }, ImGuiInputTextFlags_AutoSelectAll | ImGuiInputTextFlags_ReadOnly); ImGui::PopStyleColor(); diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 84ebab0825..2dbad885ed 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -79,7 +79,7 @@ namespace librealsense enum auto_calib_sub_cmd : uint8_t { py_rx_calib_check_status = 0x03, - interactive_scan_control = 0x06, + interactive_scan_control = 0x05, py_rx_calib_begin = 0x08, tare_calib_begin = 0x0b, tare_calib_check_status = 0x0c, @@ -285,6 +285,8 @@ namespace librealsense { // Check calibration status auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status }); + std::cout << __LINE__ << " check occ status, res size = " << res.size() << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << " check occ status, res size = " << res.size())); if (res.size() < sizeof(DirectSearchCalibrationResult)) { if (!((retries++) % 5)) // Add log debug once in a sec @@ -355,6 +357,7 @@ namespace librealsense //Enforce Thermal Compensation off during OCC volatile thermal_compensation_guard grd(this); + std::cout << __FUNCTION__ << " json size = " << json.size() << std::endl; if (json.size() > 0) { int tmp_speed(DEFAULT_SPEED); @@ -463,7 +466,7 @@ namespace librealsense std::shared_ptr preset_recover; if (calib_type == 0) { - LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); + LOG_WARNING("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); check_params(speed, scan_parameter, data_sampling); int p4 = 0; @@ -486,13 +489,17 @@ namespace librealsense // Begin auto-calibration if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) - _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); + { + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); + std::cout << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size() << std::endl; + } if (host_assistance != host_assistance_type::assistance_start) { if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, get_calibration_result, 0, 0); // Rectify interactive_scan_control + std::cout << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size() << std::endl; uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -571,6 +578,8 @@ namespace librealsense if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + std::cout << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size() << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -677,13 +686,19 @@ namespace librealsense // Begin auto-calibration if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) + { _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_plus_fl_calib_begin, speed_fl, 0, p4 }); + std::cout << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size() << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size())); + } if (host_assistance != host_assistance_type::assistance_start) { if ((host_assistance == host_assistance_type::assistance_first_feed) || (host_assistance == host_assistance_type::assistance_second_feed)) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + std::cout << __LINE__ << "occ interactive_scan_control 0 0" << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0")); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -717,6 +732,8 @@ namespace librealsense try { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); + std::cout << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); if (res.size() < sizeof(DscPyRxFLCalibrationTableResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); @@ -852,6 +869,8 @@ namespace librealsense { LOG_DEBUG("run_tare_calibration interactive control with parameters: depth = " << depth); _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 2, depth }); + std::cout << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() << std::endl; + LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() )); } else { @@ -926,7 +945,7 @@ namespace librealsense if (progress_callback) { - if (depth < 0 && count <= 20) + if (depth < 0 && count < 20) progress_callback->on_update_progress(static_cast(80 + count++)); else if (depth == 0) progress_callback->on_update_progress(count++ * (2.f * speed)); //curently this number does not reflect the actual progress @@ -1031,6 +1050,10 @@ namespace librealsense } #endif double tmp = static_cast(counter) / static_cast(data_size) * 10000.0; +// LOG_WARNING(std::string(to_string() << __LINE__ << " roi_w = " << roi_w << " roi_h = " << roi_h +// << " _min_valid_depth = " << _min_valid_depth +// << " _max_valid_depth = " << _max_valid_depth +// << " counter = " << counter << " data_size = " << data_size << " fillrate = " << tmp)); // Evgeni -Debug return static_cast(tmp + 0.5f); } @@ -1079,6 +1102,43 @@ namespace librealsense } } + void auto_calibrated::remove_leading_outliers(uint16_t data[256], int size) + { + //Due to the async between the preset activation and the flow, the initial frames of the sample may include unrelated data. + // the purpose of the function is to eliminate those by replacing them with a single value. + // This assumes that the fill_rate is contiguous during scan (i.e. grows or shrinks monotonically) + uint16_t base_fr = 0; + bool gap_identified = false; + for (int i = 255; i >= 0; i--) + { + // Initialize reference value + if (!base_fr) + { + if (data[i]) + base_fr = data[i]; + continue; + } + + // Rectify missing values + if (!data[i]) + data[i] = base_fr; + else + { + if (gap_identified) + { + data[i] = base_fr; + continue; + } + float change_scale = (float)base_fr / data[i]; + if (change_scale > 1.5f || change_scale < 0.67f) // use 50% increment/decrement from previous sample + { + gap_identified = true; + std::cout << "Start rectifying data from index " << i << " due to jump from " << base_fr << " to " << data[i] << std::endl; + } + } + } + } + // get_depth_frame_sum: // Function sums the pixels in the image ROI - Add the collected avarage parameters to _collected_counter, _collected_sum. void auto_calibrated::collect_depth_frame_sum(const rs2_frame* f) @@ -1159,10 +1219,12 @@ namespace librealsense { std::vector res; rs2_metadata_type frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); + //LOG_WARNING(std::string(to_string() << __FUNCTION__ << " fc = " << frame_counter)); if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START) { if (frame_counter <= 2) { + LOG_WARNING("return on FC <=2"); return res; } if (progress_callback) @@ -1181,13 +1243,19 @@ namespace librealsense { res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback); } - _prev_frame_counter = frame_counter; + _prev_frame_counter = frame_counter+10;//*2; // Evgeni - bypass the async (+10 addition to trigger transition to collect stage) _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START; + LOG_WARNING(std::string(to_string() << "swith INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); return res; } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START) { bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); + if (_prev_frame_counter !=frame_counter) + { + LOG_WARNING(std::string(to_string() << __FUNCTION__ << " _prev_frame_counter = " << _prev_frame_counter + << ", frames_counter = " << frame_counter << ", tf = " << _total_frames)); + } _prev_frame_counter = frame_counter; if (still_waiting) { @@ -1195,6 +1263,7 @@ namespace librealsense { progress_callback->on_update_progress(static_cast(15)); } + //LOG_WARNING(std::string(to_string() << __LINE__ << " return on still waiting")); return res; } if (progress_callback) @@ -1207,9 +1276,11 @@ namespace librealsense _skipped_frames = 0; _prev_frame_counter = -1; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT; + LOG_WARNING(std::string(to_string() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT")); } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT) { + LOG_DEBUG(std::string(to_string() << "in RS2_OCC_STATE_DATA_COLLECT")); if (_action == auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB) { #ifdef SAVE_RAW_IMAGE @@ -1262,14 +1333,27 @@ namespace librealsense } else { + std::cout << "Process frame id " << frame_counter << std::endl; if (progress_callback) { progress_callback->on_update_progress(static_cast(20 + static_cast(frame_counter * 60.0 / _total_frames))); } - _fill_factor[frame_counter + fw_host_offset] = calc_fill_rate(f); + auto fr = calc_fill_rate(f); + if (frame_counter < 40) // Evgeni - handle discrepancy on stream/preset activation + fr = 0; + if (frame_counter + fw_host_offset < 256) + { + _fill_factor[frame_counter + fw_host_offset] = fr; + std::cout << "fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr << std::endl; + } + else + std::cout << " trying to fill invalid fill index " << frame_counter + fw_host_offset << std::endl; + if (_interactive_scan) { - _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1}); + auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1}); + LOG_WARNING(std::string(to_string() << __LINE__ << "occ interactive_scan_control 1, res size = " << res.size() )); + } _skipped_frames = 0; _prev_frame_counter = frame_counter; @@ -1278,6 +1362,7 @@ namespace librealsense else { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL; + LOG_WARNING(std::string(to_string() << __LINE__ << " go to final FW call")); } } else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB) @@ -1330,6 +1415,7 @@ namespace librealsense } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL) { + LOG_WARNING(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL")); if (frame_counter > _total_frames) { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL; @@ -1337,10 +1423,12 @@ namespace librealsense else if (_interactive_scan) { _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1 }); + LOG_WARNING(std::string(to_string() << __LINE__ << "Call for interactive_scan_control, 1")); } } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL) { + LOG_WARNING(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); if (progress_callback) { progress_callback->on_update_progress(static_cast(80)); @@ -1364,6 +1452,7 @@ namespace librealsense fout.close(); } #endif + remove_leading_outliers(_fill_factor, _total_frames); fill_missing_data(_fill_factor, _total_frames); std::stringstream ss; ss << "{\n \"calib type\":" << 0 << diff --git a/src/ds5/ds5-auto-calibration.h b/src/ds5/ds5-auto-calibration.h index 5278d2da33..57433219ed 100644 --- a/src/ds5/ds5-auto-calibration.h +++ b/src/ds5/ds5-auto-calibration.h @@ -79,6 +79,7 @@ namespace librealsense void get_target_dots_info(rs2_frame_queue* frames, float dots_x[4], float dots_y[4], rs2::stream_profile & profile, rs2_intrinsics & fy, int progress, update_progress_callback_ptr progress_callback); uint16_t calc_fill_rate(const rs2_frame* f); void fill_missing_data(uint16_t data[256], int size); + void remove_leading_outliers(uint16_t data[256], int size); void collect_depth_frame_sum(const rs2_frame* f); DirectSearchCalibrationResult get_calibration_status(int timeout_ms, std::function progress_func, bool wait_for_final_results = true); From bc47b4854788a0326834d3017aec9838527bf42d Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Wed, 28 Sep 2022 23:43:56 +0300 Subject: [PATCH 02/17] Minor adjustment for flow --- src/ds5/ds5-auto-calibration.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 2dbad885ed..b07a1dee04 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -185,7 +185,7 @@ namespace librealsense _collected_counter(-1), _collected_frame_num(-1), _collected_sum(-1.0), - _min_valid_depth(0), + _min_valid_depth(1), _max_valid_depth(uint16_t(-1)), _resize_factor(5), _skipped_frames(0) @@ -421,7 +421,7 @@ namespace librealsense _json = json; _action = auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START; - _interactive_scan = bool(interactive_scan_v3); + _interactive_scan = false;// Evgeni bool(interactive_scan_v3); switch (speed) { case auto_calib_speed::speed_very_fast: @@ -491,15 +491,15 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); - std::cout << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size() << std::endl; + LOG_WARNING(std::string(to_string() << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); } if (host_assistance != host_assistance_type::assistance_start) { if (host_assistance == host_assistance_type::assistance_first_feed) { - command cmd(ds::AUTO_CALIB, get_calibration_result, 0, 0); // Rectify interactive_scan_control - std::cout << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size() << std::endl; + command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + LOG_WARNING(" occ interactive_scan_control 0,0 - save statistics "); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -1333,18 +1333,17 @@ namespace librealsense } else { - std::cout << "Process frame id " << frame_counter << std::endl; if (progress_callback) { progress_callback->on_update_progress(static_cast(20 + static_cast(frame_counter * 60.0 / _total_frames))); } auto fr = calc_fill_rate(f); - if (frame_counter < 40) // Evgeni - handle discrepancy on stream/preset activation + if (frame_counter < 20) // Evgeni - handle discrepancy on stream/preset activation fr = 0; if (frame_counter + fw_host_offset < 256) { _fill_factor[frame_counter + fw_host_offset] = fr; - std::cout << "fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr << std::endl; + LOG_WARNING(std::string(to_string() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr)); } else std::cout << " trying to fill invalid fill index " << frame_counter + fw_host_offset << std::endl; @@ -1452,7 +1451,7 @@ namespace librealsense fout.close(); } #endif - remove_leading_outliers(_fill_factor, _total_frames); + //remove_leading_outliers(_fill_factor, _total_frames); fill_missing_data(_fill_factor, _total_frames); std::stringstream ss; ss << "{\n \"calib type\":" << 0 << From f41fa72a6601ac099c634d117808a2a3ba635b97 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Wed, 28 Sep 2022 23:44:10 +0300 Subject: [PATCH 03/17] Handle HWM failures Debug on apply_calib --- common/on-chip-calib.cpp | 11 +++++++++++ src/ds5/ds5-auto-calibration.cpp | 29 +++++++++++++++++++++++++++-- 2 files changed, 38 insertions(+), 2 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 96f91ce8cd..24b662fb21 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -1343,6 +1343,12 @@ namespace rs2 void on_chip_calib_manager::process_flow(std::function cleanup, invoker invoke) { + static auto_calib_action prev_action = RS2_CALIB_ACTION_ON_CHIP_OB_CALIB; + if (prev_action != action) + { + prev_action = action; + LOG_WARNING(std::string(to_string() << __LINE__ << " action = " << action )); + } if (action == RS2_CALIB_ACTION_FL_CALIB || action == RS2_CALIB_ACTION_UVMAPPING_CALIB || action == RS2_CALIB_ACTION_FL_PLUS_CALIB) stop_viewer(invoke); @@ -1533,8 +1539,13 @@ namespace rs2 try_start_viewer(0, 0, 0, invoke); // Start with default settings // Make new calibration active + LOG_WARNING(std::string(to_string() << __LINE__ << " wait 4")); + std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // Evgeni - debug apply_calib(true); LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); + LOG_WARNING(std::string(to_string() << __LINE__ << " wait 5")); + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // Evgeni - debug + //LOG_WARNING(std::string(to_string() << __LINE__ << " wait done")); // Capture metrics after auto metrics_after = get_depth_metrics(invoke); diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index b07a1dee04..8ac7464bfb 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -509,7 +509,25 @@ namespace librealsense cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); } - _hw_monitor->send(cmd); + bool success = false; + int iter =0; + do + { + try + { + if (iter==0) // apply only in the first iteration + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Debug sendng request below this threshold + auto res = _hw_monitor->send(cmd); // most likely will fail with MIPI SKU and require retry + LOG_WARNING("occ Save Statistics transfer FR buffer succeeded"); + success = true; + } + catch(...) + { + LOG_WARNING("occ Save Statistics result failed"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + }; + } + while(( ++iter < 3) && (!success)); } DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) @@ -1765,7 +1783,14 @@ namespace librealsense uint8_t* table = (uint8_t*)(calibration.data() + sizeof(table_header)); command write_calib(ds::CALIBRECALC, 0, 0, 0, 0xcafecafe); write_calib.data.insert(write_calib.data.end(), (uint8_t*)table, ((uint8_t*)table) + hd->table_size); - _hw_monitor->send(write_calib); + try + { + _hw_monitor->send(write_calib); + } + catch(...) + { + LOG_ERROR("Flashing coefficients_table_id failed"); + } } case rgb_calibration_id: // case fall-through by design. For RGB skip loading to RAM (not supported) _curr_calibration = calibration; From 58c29d31c48b1efb490b5d1f12e982ffe20fb802 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Sun, 2 Oct 2022 17:15:31 +0300 Subject: [PATCH 04/17] OCC converge, Tare streamline --- common/on-chip-calib.cpp | 6 ------ src/ds5/ds5-auto-calibration.cpp | 13 ++++++++----- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 24b662fb21..dcd7d34838 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -1539,13 +1539,7 @@ namespace rs2 try_start_viewer(0, 0, 0, invoke); // Start with default settings // Make new calibration active - LOG_WARNING(std::string(to_string() << __LINE__ << " wait 4")); - std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // Evgeni - debug apply_calib(true); - LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); - LOG_WARNING(std::string(to_string() << __LINE__ << " wait 5")); - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // Evgeni - debug - //LOG_WARNING(std::string(to_string() << __LINE__ << " wait done")); // Capture metrics after auto metrics_after = get_depth_metrics(invoke); diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 8ac7464bfb..195f3a6eac 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -185,7 +185,7 @@ namespace librealsense _collected_counter(-1), _collected_frame_num(-1), _collected_sum(-1.0), - _min_valid_depth(1), + _min_valid_depth(0), _max_valid_depth(uint16_t(-1)), _resize_factor(5), _skipped_frames(0) @@ -885,10 +885,10 @@ namespace librealsense if (depth > 0) { - LOG_DEBUG("run_tare_calibration interactive control with parameters: depth = " << depth); + LOG_WARNING("run_tare_calibration interactive control (2) with parameters: depth = " << depth); _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 2, depth }); - std::cout << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() )); + //std::cout << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() << std::endl; + //LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() )); } else { @@ -1237,6 +1237,7 @@ namespace librealsense { std::vector res; rs2_metadata_type frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); + rs2_metadata_type frame_ts = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP); //LOG_WARNING(std::string(to_string() << __FUNCTION__ << " fc = " << frame_counter)); if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START) { @@ -1356,7 +1357,7 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(frame_counter * 60.0 / _total_frames))); } auto fr = calc_fill_rate(f); - if (frame_counter < 20) // Evgeni - handle discrepancy on stream/preset activation + if (frame_counter < 10) // Evgeni - handle discrepancy on stream/preset activation fr = 0; if (frame_counter + fw_host_offset < 256) { @@ -1397,6 +1398,8 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(progress_rate * 60.0))); } } + LOG_WARNING(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter + << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); if (frame_counter < _total_frames) { if (_skipped_frames < FRAMES_TO_SKIP) From e715c91ebf0baf1c59d2ece9580555b3661fa2b0 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 3 Oct 2022 07:32:57 +0300 Subject: [PATCH 05/17] W/A for Tare using rolling frame counter --- src/ds5/ds5-auto-calibration.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 195f3a6eac..b60cc2deec 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -1262,20 +1262,17 @@ namespace librealsense { res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback); } - _prev_frame_counter = frame_counter+10;//*2; // Evgeni - bypass the async (+10 addition to trigger transition to collect stage) + _prev_frame_counter = frame_counter;//+10;//*2; // Evgeni - bypass the async (+10 addition to trigger transition to collect stage) _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START; - LOG_WARNING(std::string(to_string() << "swith INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); + LOG_WARNING(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); return res; } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START) { - bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); - if (_prev_frame_counter !=frame_counter) - { - LOG_WARNING(std::string(to_string() << __FUNCTION__ << " _prev_frame_counter = " << _prev_frame_counter - << ", frames_counter = " << frame_counter << ", tf = " << _total_frames)); - } - _prev_frame_counter = frame_counter; + LOG_WARNING(std::string(to_string() << "fc = " << frame_counter)); + //Evgeni bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); + bool still_waiting(frame_counter <= _prev_frame_counter+10); // Evgeni - bypass FW bug + //_prev_frame_counter = frame_counter; // Evgeni - to be removed if (still_waiting) { if (progress_callback) @@ -1386,7 +1383,7 @@ namespace librealsense else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB) { static const int FRAMES_TO_SKIP(1); - if (frame_counter != _prev_frame_counter) + if ((frame_counter/25) != _prev_frame_counter) // Evgeni - W/A for FW not handling Frame counters correctly { _collected_counter = 0; _collected_sum = 0.0; @@ -1400,7 +1397,7 @@ namespace librealsense } LOG_WARNING(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); - if (frame_counter < _total_frames) + if (frame_counter < 200) //(frame_counter < _total_frames) // Evgeni - see above { if (_skipped_frames < FRAMES_TO_SKIP) { @@ -1425,7 +1422,7 @@ namespace librealsense } ++_collected_frame_num; } - _prev_frame_counter = frame_counter; + _prev_frame_counter = (frame_counter/25); // Evgeni } else { From 03e120c1a88bbf29876b0cbc60f4755e0e2ebb46 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 3 Oct 2022 09:32:14 +0300 Subject: [PATCH 06/17] Allow for Tare/OCC run side by side remove unnecessary messages --- src/ds5/ds5-auto-calibration.cpp | 36 +++++++++++++++++++------------- src/linux/backend-v4l2.h | 1 - 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index b60cc2deec..85d5d4e6fe 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -421,7 +421,7 @@ namespace librealsense _json = json; _action = auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START; - _interactive_scan = false;// Evgeni bool(interactive_scan_v3); + _interactive_scan = false;//Ev bool(interactive_scan_v3); switch (speed) { case auto_calib_speed::speed_very_fast: @@ -1068,10 +1068,6 @@ namespace librealsense } #endif double tmp = static_cast(counter) / static_cast(data_size) * 10000.0; -// LOG_WARNING(std::string(to_string() << __LINE__ << " roi_w = " << roi_w << " roi_h = " << roi_h -// << " _min_valid_depth = " << _min_valid_depth -// << " _max_valid_depth = " << _max_valid_depth -// << " counter = " << counter << " data_size = " << data_size << " fillrate = " << tmp)); // Evgeni -Debug return static_cast(tmp + 0.5f); } @@ -1238,7 +1234,8 @@ namespace librealsense std::vector res; rs2_metadata_type frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); rs2_metadata_type frame_ts = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP); - //LOG_WARNING(std::string(to_string() << __FUNCTION__ << " fc = " << frame_counter)); + bool tare_fc_workaround = (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB); //Ev - work-around tare implementation using rolling frame counter + if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START) { if (frame_counter <= 2) @@ -1262,7 +1259,8 @@ namespace librealsense { res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback); } - _prev_frame_counter = frame_counter;//+10;//*2; // Evgeni - bypass the async (+10 addition to trigger transition to collect stage) + _prev_frame_counter = frame_counter; //Ev - bypass the async (+10 addition to trigger transition to collect stage) + if (!tare_fc_workaround) _prev_frame_counter +=10; // for OCC calib Evgeni _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START; LOG_WARNING(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); return res; @@ -1270,16 +1268,17 @@ namespace librealsense if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START) { LOG_WARNING(std::string(to_string() << "fc = " << frame_counter)); - //Evgeni bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); - bool still_waiting(frame_counter <= _prev_frame_counter+10); // Evgeni - bypass FW bug - //_prev_frame_counter = frame_counter; // Evgeni - to be removed + bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); + if (tare_fc_workaround) + still_waiting = (frame_counter <= _prev_frame_counter+10); //Ev - bypass for Tare + else + _prev_frame_counter = frame_counter; if (still_waiting) { if (progress_callback) { progress_callback->on_update_progress(static_cast(15)); } - //LOG_WARNING(std::string(to_string() << __LINE__ << " return on still waiting")); return res; } if (progress_callback) @@ -1383,7 +1382,10 @@ namespace librealsense else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB) { static const int FRAMES_TO_SKIP(1); - if ((frame_counter/25) != _prev_frame_counter) // Evgeni - W/A for FW not handling Frame counters correctly + bool cond = (frame_counter != _prev_frame_counter); + if (tare_fc_workaround) + cond = ((frame_counter/25) != _prev_frame_counter); // Evgeni - W/A for FW not handling Frame counters correctly + if (cond) { _collected_counter = 0; _collected_sum = 0.0; @@ -1397,7 +1399,11 @@ namespace librealsense } LOG_WARNING(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); - if (frame_counter < 200) //(frame_counter < _total_frames) // Evgeni - see above + cond = (frame_counter < _total_frames); + if (tare_fc_workaround) + cond = (frame_counter < 200); + + if (cond) //(frame_counter < _total_frames) // Evgeni - see above { if (_skipped_frames < FRAMES_TO_SKIP) { @@ -1422,7 +1428,9 @@ namespace librealsense } ++_collected_frame_num; } - _prev_frame_counter = (frame_counter/25); // Evgeni + _prev_frame_counter = frame_counter; + if (tare_fc_workaround) + _prev_frame_counter = (frame_counter/25); // Evgeni. W/A for Tare with rolling frame counter } else { diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 898e374847..ed7d380573 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -159,7 +159,6 @@ namespace librealsense }; - //Debug Evgeni // RAII for buffer exchange with kernel struct kernel_buf_guard { From 03bee19a8e92d238c09b23c0f7a65ae60cb9b166 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 3 Oct 2022 15:29:57 +0300 Subject: [PATCH 07/17] Add calib_info frame counter --- include/librealsense2/h/rs_frame.h | 1 + src/ds5/ds5-device.cpp | 6 +++++- src/metadata.h | 7 +++++-- src/to-string.cpp | 1 + 4 files changed, 12 insertions(+), 3 deletions(-) diff --git a/include/librealsense2/h/rs_frame.h b/include/librealsense2/h/rs_frame.h index 51c8df2213..d8edfe6078 100644 --- a/include/librealsense2/h/rs_frame.h +++ b/include/librealsense2/h/rs_frame.h @@ -72,6 +72,7 @@ typedef enum rs2_frame_metadata_value RS2_FRAME_METADATA_INPUT_WIDTH, RS2_FRAME_METADATA_INPUT_HEIGHT, RS2_FRAME_METADATA_SUB_PRESET_INFO, + RS2_FRAME_METADATA_CALIB_INFO, RS2_FRAME_METADATA_CRC, RS2_FRAME_METADATA_COUNT diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index cfc1a0445d..af3f2eaea9 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -1314,10 +1314,14 @@ namespace librealsense make_attribute_parser(&md_mipi_depth_mode::input_height, md_mipi_depth_control_attributes::input_height_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_SUB_PRESET_INFO, // added for mipi + depth_sensor.register_metadata(RS2_FRAME_METADATA_SUB_PRESET_INFO, make_attribute_parser(&md_mipi_depth_mode::sub_preset_info, md_mipi_depth_control_attributes::sub_preset_info_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_CALIB_INFO, + make_attribute_parser(&md_mipi_depth_mode::calib_info, + md_mipi_depth_control_attributes::calibration_info_attribute, + md_prop_offset)); depth_sensor.register_metadata(RS2_FRAME_METADATA_CRC, // added for mipi make_attribute_parser(&md_mipi_depth_mode::crc, md_mipi_depth_control_attributes::crc_attribute, diff --git a/src/metadata.h b/src/metadata.h index 91e17dabea..f28d17a661 100644 --- a/src/metadata.h +++ b/src/metadata.h @@ -136,7 +136,8 @@ namespace librealsense input_width_attribute = (1u << 10), input_height_attribute = (1u << 11), sub_preset_info_attribute = (1u << 12), - crc_attribute = (1u << 13) + calibration_info_attribute = (1u << 13), // shall be used to stire the frame counter in calibration routines + crc_attribute = (1u << 14) }; /**\brief md_mipi_rgb_control_attributes - bit mask to find active attributes, @@ -368,7 +369,9 @@ namespace librealsense struct md_mipi_depth_mode { md_header header; - uint32_t version; // maybe need to replace by uint8_t for version and 3 others for reserved + uint8_t version; // maybe need to replace by uint8_t for version and 3 others for reserved + uint16_t calib_info; + uint8_t reserved; uint32_t flags; // Bit array to specify attributes that are valid uint32_t hw_timestamp; uint32_t optical_timestamp; //In microsecond unit diff --git a/src/to-string.cpp b/src/to-string.cpp index 1740b7bc8c..b28ded5b9b 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -530,6 +530,7 @@ const char * get_string( rs2_frame_metadata_value value ) CASE( INPUT_WIDTH ) CASE( INPUT_HEIGHT ) CASE( SUB_PRESET_INFO ) + CASE( CALIB_INFO ) CASE( CRC ) default: From 619ad0340435d007aaeef8d34c4e08996a4a94bf Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 3 Oct 2022 16:30:40 +0300 Subject: [PATCH 08/17] Read calib frame counter via dedicated MD field --- src/ds5/ds5-auto-calibration.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 85d5d4e6fe..ee20688198 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -1235,6 +1235,9 @@ namespace librealsense rs2_metadata_type frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); rs2_metadata_type frame_ts = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP); bool tare_fc_workaround = (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB); //Ev - work-around tare implementation using rolling frame counter + bool mipi_sku = ((frame_interface*)f)->supports_frame_metadata(RS2_FRAME_METADATA_CALIB_INFO); + if (mipi_sku) + frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_CALIB_INFO); if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START) { @@ -1269,9 +1272,9 @@ namespace librealsense { LOG_WARNING(std::string(to_string() << "fc = " << frame_counter)); bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); - if (tare_fc_workaround) - still_waiting = (frame_counter <= _prev_frame_counter+10); //Ev - bypass for Tare - else +// if (tare_fc_workaround) +// still_waiting = (frame_counter <= _prev_frame_counter+10); //Ev - bypass for Tare +// else _prev_frame_counter = frame_counter; if (still_waiting) { @@ -1383,8 +1386,8 @@ namespace librealsense { static const int FRAMES_TO_SKIP(1); bool cond = (frame_counter != _prev_frame_counter); - if (tare_fc_workaround) - cond = ((frame_counter/25) != _prev_frame_counter); // Evgeni - W/A for FW not handling Frame counters correctly +// if (tare_fc_workaround) +// cond = ((frame_counter/25) != _prev_frame_counter); // Evgeni - W/A for FW not handling Frame counters correctly if (cond) { _collected_counter = 0; @@ -1400,8 +1403,8 @@ namespace librealsense LOG_WARNING(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); cond = (frame_counter < _total_frames); - if (tare_fc_workaround) - cond = (frame_counter < 200); +// if (tare_fc_workaround) +// cond = (frame_counter < 200); if (cond) //(frame_counter < _total_frames) // Evgeni - see above { @@ -1429,8 +1432,8 @@ namespace librealsense ++_collected_frame_num; } _prev_frame_counter = frame_counter; - if (tare_fc_workaround) - _prev_frame_counter = (frame_counter/25); // Evgeni. W/A for Tare with rolling frame counter +// if (tare_fc_workaround) +// _prev_frame_counter = (frame_counter/25); // Evgeni. W/A for Tare with rolling frame counter } else { From 491d1d05b91518a861ebcf0d6dfd6953c8fdc402 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Thu, 6 Oct 2022 09:22:23 +0300 Subject: [PATCH 09/17] Clean up and reorg debug messages --- common/on-chip-calib.cpp | 11 ---- src/ds5/ds5-auto-calibration.cpp | 96 +++++++++++++------------------- 2 files changed, 39 insertions(+), 68 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index dcd7d34838..4f6891e05b 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -865,7 +865,6 @@ namespace rs2 void on_chip_calib_manager::calibrate() { - std::cout << __FUNCTION__ << " was called" << std::endl; int occ_timeout_ms = 9000; if (action == RS2_CALIB_ACTION_ON_CHIP_OB_CALIB || action == RS2_CALIB_ACTION_ON_CHIP_FL_CALIB) { @@ -1343,12 +1342,6 @@ namespace rs2 void on_chip_calib_manager::process_flow(std::function cleanup, invoker invoke) { - static auto_calib_action prev_action = RS2_CALIB_ACTION_ON_CHIP_OB_CALIB; - if (prev_action != action) - { - prev_action = action; - LOG_WARNING(std::string(to_string() << __LINE__ << " action = " << action )); - } if (action == RS2_CALIB_ACTION_FL_CALIB || action == RS2_CALIB_ACTION_UVMAPPING_CALIB || action == RS2_CALIB_ACTION_FL_PLUS_CALIB) stop_viewer(invoke); @@ -2437,7 +2430,6 @@ namespace rs2 { get_manager().apply_calib(true); // Store the new calibration internally get_manager().keep(); // Flash the new calibration - LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied and flashed")); if (RS2_CALIB_STATE_UVMAPPING_INPUT == update_state) get_manager().reset_device(); // Workaround for reloading color calibration table. Other approach? @@ -2824,7 +2816,6 @@ namespace rs2 { use_new_calib = true; get_manager().apply_calib(true); - LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); } ImGui::SetCursorScreenPos({ float(x + 150), (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_OB_CALIB || get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_FL_CALIB ? float(y + 70) + ImGui::GetTextLineHeightWithSpacing() : (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB ? (get_manager().tare_health ? float(y + 70) : float(y + 15)) + ImGui::GetTextLineHeightWithSpacing() : float(y + 70))) }); @@ -2981,7 +2972,6 @@ namespace rs2 { get_manager().apply_calib(true); use_new_calib = true; - LOG_WARNING(std::string(to_string() << __LINE__ << " new calib applied")); } } @@ -3071,7 +3061,6 @@ namespace rs2 ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, regular_blue); auto s = update_manager->get_log(); - rs2::log(RS2_LOG_SEVERITY_INFO, s.c_str()); ImGui::InputTextMultiline("##autocalib_log", const_cast(s.c_str()), s.size() + 1, { 490,100 }, ImGuiInputTextFlags_AutoSelectAll | ImGuiInputTextFlags_ReadOnly); ImGui::PopStyleColor(); diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index ee20688198..7e30e848fe 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -10,6 +10,13 @@ #include "librealsense2/rsutil.h" #include "../algo.h" +#undef UCAL_DEBUG +#ifdef UCAL_DEBUG +#define LOG_OCC_DEBUG(...) do { CLOG(WARNING ,"librealsense") << __VA_ARGS__; } while(false) +#else +#define LOG_OCC_DEBUG(...) +#endif //UCAL_DEBUG + namespace librealsense { #pragma pack(push, 1) @@ -285,8 +292,7 @@ namespace librealsense { // Check calibration status auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status }); - std::cout << __LINE__ << " check occ status, res size = " << res.size() << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << " check occ status, res size = " << res.size())); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " check occ status, res size = " << res.size())); if (res.size() < sizeof(DirectSearchCalibrationResult)) { if (!((retries++) % 5)) // Add log debug once in a sec @@ -357,7 +363,6 @@ namespace librealsense //Enforce Thermal Compensation off during OCC volatile thermal_compensation_guard grd(this); - std::cout << __FUNCTION__ << " json size = " << json.size() << std::endl; if (json.size() > 0) { int tmp_speed(DEFAULT_SPEED); @@ -421,7 +426,7 @@ namespace librealsense _json = json; _action = auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START; - _interactive_scan = false;//Ev bool(interactive_scan_v3); + _interactive_scan = false;//bool(interactive_scan_v3); - enforce non-interactive run switch (speed) { case auto_calib_speed::speed_very_fast: @@ -466,7 +471,7 @@ namespace librealsense std::shared_ptr preset_recover; if (calib_type == 0) { - LOG_WARNING("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); + LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling); check_params(speed, scan_parameter, data_sampling); int p4 = 0; @@ -491,7 +496,7 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); - LOG_WARNING(std::string(to_string() << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); } if (host_assistance != host_assistance_type::assistance_start) @@ -499,7 +504,7 @@ namespace librealsense if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - LOG_WARNING(" occ interactive_scan_control 0,0 - save statistics "); + LOG_OCC_DEBUG(" occ interactive_scan_control 0,0 - save statistics "); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -516,14 +521,13 @@ namespace librealsense try { if (iter==0) // apply only in the first iteration - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Debug sendng request below this threshold - auto res = _hw_monitor->send(cmd); // most likely will fail with MIPI SKU and require retry - LOG_WARNING("occ Save Statistics transfer FR buffer succeeded"); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // TODO: sending shorter request + auto res = _hw_monitor->send(cmd); // is likely to fail with MIPI SKU success = true; } catch(...) { - LOG_WARNING("occ Save Statistics result failed"); + LOG_OCC_DEBUG("occ Save Statistics result failed"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); }; } @@ -552,7 +556,7 @@ namespace librealsense if (progress_callback) progress_callback->on_update_progress(static_cast(100)); res = get_calibration_results(health); - std::cout << "Py: " << result.rightPy << std::endl; + LOG_OCC_DEBUG(std::string(to_string() << "Py: " << result.rightPy)); } } else if (calib_type == 1) @@ -596,8 +600,7 @@ namespace librealsense if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - std::cout << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size() << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -706,8 +709,7 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_plus_fl_calib_begin, speed_fl, 0, p4 }); - std::cout << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size() << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size())); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size())); } if (host_assistance != host_assistance_type::assistance_start) @@ -715,8 +717,7 @@ namespace librealsense if ((host_assistance == host_assistance_type::assistance_first_feed) || (host_assistance == host_assistance_type::assistance_second_feed)) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - std::cout << __LINE__ << "occ interactive_scan_control 0 0" << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0")); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -750,8 +751,7 @@ namespace librealsense try { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); - std::cout << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() << std::endl; - LOG_DEBUG(std::string(to_string() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); if (res.size() < sizeof(DscPyRxFLCalibrationTableResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); @@ -885,10 +885,8 @@ namespace librealsense if (depth > 0) { - LOG_WARNING("run_tare_calibration interactive control (2) with parameters: depth = " << depth); + LOG_OCC_DEBUG("run_tare_calibration interactive control (2) with parameters: depth = " << depth); _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 2, depth }); - //std::cout << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() << std::endl; - //LOG_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 2, res size = " << res.size() )); } else { @@ -1147,7 +1145,7 @@ namespace librealsense if (change_scale > 1.5f || change_scale < 0.67f) // use 50% increment/decrement from previous sample { gap_identified = true; - std::cout << "Start rectifying data from index " << i << " due to jump from " << base_fr << " to " << data[i] << std::endl; + LOG_OCC_DEBUG(std::string(to_string() << "Start rectifying data from index " << i << " due to jump from " << base_fr << " to " << data[i])); } } } @@ -1243,7 +1241,6 @@ namespace librealsense { if (frame_counter <= 2) { - LOG_WARNING("return on FC <=2"); return res; } if (progress_callback) @@ -1262,20 +1259,16 @@ namespace librealsense { res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback); } - _prev_frame_counter = frame_counter; //Ev - bypass the async (+10 addition to trigger transition to collect stage) - if (!tare_fc_workaround) _prev_frame_counter +=10; // for OCC calib Evgeni + _prev_frame_counter = frame_counter; + if ((!tare_fc_workaround) && mipi_sku) _prev_frame_counter +=10; // Compensate for MIPI OCC calib. invoke delay _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START; - LOG_WARNING(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); + LOG_OCC_DEBUG(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); return res; } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START) { - LOG_WARNING(std::string(to_string() << "fc = " << frame_counter)); bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames); -// if (tare_fc_workaround) -// still_waiting = (frame_counter <= _prev_frame_counter+10); //Ev - bypass for Tare -// else - _prev_frame_counter = frame_counter; + _prev_frame_counter = frame_counter; if (still_waiting) { if (progress_callback) @@ -1294,11 +1287,10 @@ namespace librealsense _skipped_frames = 0; _prev_frame_counter = -1; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT; - LOG_WARNING(std::string(to_string() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT")); } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT) { - LOG_DEBUG(std::string(to_string() << "in RS2_OCC_STATE_DATA_COLLECT")); if (_action == auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB) { #ifdef SAVE_RAW_IMAGE @@ -1356,21 +1348,18 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(frame_counter * 60.0 / _total_frames))); } auto fr = calc_fill_rate(f); - if (frame_counter < 10) // Evgeni - handle discrepancy on stream/preset activation + if (frame_counter < 10) // handle discrepancy on stream/preset activation fr = 0; if (frame_counter + fw_host_offset < 256) { _fill_factor[frame_counter + fw_host_offset] = fr; - LOG_WARNING(std::string(to_string() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr)); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr)); } - else - std::cout << " trying to fill invalid fill index " << frame_counter + fw_host_offset << std::endl; - + if (_interactive_scan) { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1}); - LOG_WARNING(std::string(to_string() << __LINE__ << "occ interactive_scan_control 1, res size = " << res.size() )); - + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " occ interactive_scan_control 1,")); } _skipped_frames = 0; _prev_frame_counter = frame_counter; @@ -1379,16 +1368,13 @@ namespace librealsense else { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL; - LOG_WARNING(std::string(to_string() << __LINE__ << " go to final FW call")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " go to final FW call")); } } else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB) { static const int FRAMES_TO_SKIP(1); - bool cond = (frame_counter != _prev_frame_counter); -// if (tare_fc_workaround) -// cond = ((frame_counter/25) != _prev_frame_counter); // Evgeni - W/A for FW not handling Frame counters correctly - if (cond) + if (frame_counter != _prev_frame_counter) { _collected_counter = 0; _collected_sum = 0.0; @@ -1400,13 +1386,10 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(progress_rate * 60.0))); } } - LOG_WARNING(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); - cond = (frame_counter < _total_frames); -// if (tare_fc_workaround) -// cond = (frame_counter < 200); - if (cond) //(frame_counter < _total_frames) // Evgeni - see above + if (frame_counter < _total_frames) { if (_skipped_frames < FRAMES_TO_SKIP) { @@ -1432,8 +1415,6 @@ namespace librealsense ++_collected_frame_num; } _prev_frame_counter = frame_counter; -// if (tare_fc_workaround) -// _prev_frame_counter = (frame_counter/25); // Evgeni. W/A for Tare with rolling frame counter } else { @@ -1443,7 +1424,7 @@ namespace librealsense } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL) { - LOG_WARNING(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL")); if (frame_counter > _total_frames) { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL; @@ -1451,12 +1432,12 @@ namespace librealsense else if (_interactive_scan) { _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1 }); - LOG_WARNING(std::string(to_string() << __LINE__ << "Call for interactive_scan_control, 1")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "Call for interactive_scan_control, 1")); } } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL) { - LOG_WARNING(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); + LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); if (progress_callback) { progress_callback->on_update_progress(static_cast(80)); @@ -1480,6 +1461,7 @@ namespace librealsense fout.close(); } #endif + // Do not delete - to be used to improve algo robustness //remove_leading_outliers(_fill_factor, _total_frames); fill_missing_data(_fill_factor, _total_frames); std::stringstream ss; From a8d122a1b1dc9e2e774ef8dbe69acc9341cf42c1 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Sun, 6 Nov 2022 22:58:59 +0200 Subject: [PATCH 10/17] Provide OCC/Tare updated python example with host-assistance mode --- .../python/examples/depth_ucal_example.py | 176 ++++++++++++++++++ 1 file changed, 176 insertions(+) create mode 100644 wrappers/python/examples/depth_ucal_example.py diff --git a/wrappers/python/examples/depth_ucal_example.py b/wrappers/python/examples/depth_ucal_example.py new file mode 100644 index 0000000000..8067bbec1d --- /dev/null +++ b/wrappers/python/examples/depth_ucal_example.py @@ -0,0 +1,176 @@ +## License: Apache 2.0. See LICENSE file in root directory. +## Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +##################################################### +## auto calibration ## +##################################################### + +# First import the library +import sys +import os +import time + +import pyrealsense2 as rs + +#rs.log_to_console(rs.log_severity.warn) + +def on_chip_calibration_json(occ_json_file, host_assistance, interactive_mode): + try: + occ_json = open(occ_json_file).read() + except: + if occ_json_file: + print('Error reading occ_json_file: ', occ_json_file) + print ('Using default parameters for on-chip calibration.') + occ_json = '{\n '+\ + '"calib type": 0,\n'+\ + '"host assistance": ' + str(int(host_assistance)) + ',\n'+\ + '"keep new value after sucessful scan": 0,\n'+\ + '"fl data sampling": 1,\n'+\ + '"adjust both sides": 0,\n'+\ + '"fl scan location": 0,\n'+\ + '"fy scan direction": 0,\n'+\ + '"white wall mode": 0,\n'+\ + '"speed": 3,\n'+\ + '"scan parameter": 0,\n'+\ + '"apply preset": 1,\n'+\ + '"scan only": ' + str(int(host_assistance)) + ',\n'+\ + '"interactive scan": ' + str(int(interactive_mode)) + ',\n'+\ + '"resize factor": 1\n'+\ + '}' + return occ_json + + +def tare_calibration_json(tare_json_file, host_assistance): + try: + tare_json = open(tare_json_file).read() + except: + if tare_json_file: + print('Error reading tare_json_file: ', tare_json_file) + print ('Using default parameters for Tare calibration.') + tare_json = '{\n '+\ + '"host assistance": ' + str(int(host_assistance)) + ',\n'+\ + '"speed": 3,\n'+\ + '"scan parameter": 0,\n'+\ + '"step count": 20,\n'+\ + '"apply preset": 1,\n'+\ + '"accuracy": 2,\n'+\ + '"depth": 0,\n'+\ + '"resize factor": 1\n'+\ + '}' + return tare_json + +def on_chip_calib_cb(progress): + pp = int(progress) + sys.stdout.write('\r' + '*'*pp + ' '*(99-pp) + '*') + if (pp == 100): + print() + +def main(argv): + if '--help' in sys.argv or '-h' in sys.argv: + print('USAGE:') + print('depth_auto_calibration_example.py [--occ ] [--tare ]') + print + print('Occ and Tare calibration uses parameters given with --occ and --tare arguments respectfully.') + print('If these are argument are not given, using default values, defined in this example program.') + sys.exit(-1) + + #rs.log_to_console(rs.log_severity.warn) + ctx = rs.context() + + try: + device = ctx.query_devices()[0] + except IndexError: + print('Device is not connected') + sys.exit(1) + # Bring device in start phase + device.hardware_reset() + time.sleep(1) + + params = dict(zip(sys.argv[1::2], sys.argv[2::2])) + occ_json_file = params.get('--occ', None) + tare_json_file = params.get('--tare', None) + + pipeline = rs.pipeline() + config = rs.config() + + # Get device product line for setting a supporting resolution + pipeline_wrapper = rs.pipeline_wrapper(pipeline) + pipeline_profile = config.resolve(pipeline_wrapper) + device = pipeline_profile.get_device() + + auto_calibrated_device = rs.auto_calibrated_device(device) + + if not auto_calibrated_device: + print("The connected device does not support auto calibration") + return + + config.enable_stream(rs.stream.depth, 256, 144, rs.format.z16, 90) + # config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30) + conf = pipeline.start(config) + calib_dev = rs.auto_calibrated_device(conf.get_device()) + interactive_mode = False + + while True: + try: + print ("interactive_mode: ", interactive_mode) + operation_str = "Please select what the operation you want to do\n" + \ + "c - on chip calibration\n" + \ + "C - on chip calibration - host assist\n" + \ + "t - tare calibration\n" + \ + "T - tare calibration - host assist\n" + \ + "g - get the active calibration\n" + \ + "w - write new calibration\n" + \ + "e - exit\n" + operation = input(operation_str) + + if operation.lower() == 'c': + print("Starting on chip calibration") + occ_json = on_chip_calibration_json(occ_json_file, operation == 'C', interactive_mode) + new_calib, health = calib_dev.run_on_chip_calibration(occ_json, on_chip_calib_cb, 5000) + calib_done = len(new_calib) > 0 + while (not calib_done): + frame_set = pipeline.wait_for_frames() + depth_frame = frame_set.get_depth_frame() + new_calib, health = calib_dev.process_calibration_frame(depth_frame, on_chip_calib_cb, 5000) + calib_done = len(new_calib) > 0 + print("Calibration completed") + print("health factor = ", health) + + if operation.lower() == 'i': + interactive_mode = not interactive_mode + + if operation.lower() == 't': + print("Starting tare calibration" + (" - host assistance" if operation == 'T' else "")) + ground_truth = float(input("Please enter ground truth in mm\n")) + tare_json = tare_calibration_json(tare_json_file, operation == 'T') + new_calib, health = calib_dev.run_tare_calibration(ground_truth, tare_json, on_chip_calib_cb, 5000) + calib_done = len(new_calib) > 0 + while (not calib_done): + frame_set = pipeline.wait_for_frames() + depth_frame = frame_set.get_depth_frame() + new_calib, health = calib_dev.process_calibration_frame(depth_frame, on_chip_calib_cb, 5000) + calib_done = len(new_calib) > 0 + print("Calibration completed") + print("health factor = ", health) + + if operation == 'g': + calib = calib_dev.get_calibration_table() + print("Calibration", calib) + + if operation == 'w': + print("Writing the new calibration") + calib_dev.set_calibration_table(new_calib) + calib_dev.write_calibration() + + if operation == 'e': + pipeline.stop() + return + + print("Done\n") + except Exception as e: + print(e) + except: + print("A different Error") + +if __name__ == "__main__": + main(sys.argv[1:]) From 4bd7ae14c8d2ce220ec520dd5fdb5314519babc7 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Sun, 6 Nov 2022 22:59:54 +0200 Subject: [PATCH 11/17] D457 to use default preset specified for D455 --- src/ds5/advanced_mode/advanced_mode.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ds5/advanced_mode/advanced_mode.cpp b/src/ds5/advanced_mode/advanced_mode.cpp index 7abe96d46f..56a4ea0cd1 100644 --- a/src/ds5/advanced_mode/advanced_mode.cpp +++ b/src/ds5/advanced_mode/advanced_mode.cpp @@ -90,6 +90,7 @@ namespace librealsense default_430(p); break; case ds::RS455_PID: + case ds::RS457_PID: default_450_mid_low_res(p); switch (res) { From 18af76b958b4e7530b7818bed21dd3bf29c877d9 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 01:06:35 +0200 Subject: [PATCH 12/17] Enforce HA option for MIPI SKU --- common/on-chip-calib.cpp | 10 ++++++---- common/on-chip-calib.h | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 4f6891e05b..a817ca3f41 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -21,10 +21,11 @@ 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"; if (dev.supports(RS2_CAMERA_INFO_PRODUCT_ID)) { - std::string dev_pid = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); - if (val_in_range(dev_pid, { std::string("0AD3") })) + device_id_string = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + if (val_in_range(device_id_string, { std::string("0AD3") })) speed = 4; } if (dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION)) @@ -2069,6 +2070,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 bool assistance = (get_manager().host_assistance != 0); if (ImGui::Checkbox("Host Assistance", &assistance)) get_manager().host_assistance = (assistance ? 1 : 0); @@ -2164,9 +2166,9 @@ 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 bool assistance = (get_manager().host_assistance != 0); - if (ImGui::Checkbox("Host Assistance", &assistance)) - get_manager().host_assistance = (assistance ? 1 : 0); + ImGui::Checkbox("Host Assistance", &assistance); if (ImGui::IsItemHovered()) ImGui::SetTooltip("%s", "check = host assitance for statistics data, uncheck = no host assistance"); diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 741594df07..9690592c5f 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -103,6 +103,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; void calibrate(); From 5d2861d3909f2157919d70907e214ce3d2b6766c Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 01:45:06 +0200 Subject: [PATCH 13/17] Fix outliers correction --- src/ds5/ds5-auto-calibration.cpp | 36 ++++++++++++++++++++------------ src/ds5/ds5-auto-calibration.h | 2 +- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 7e30e848fe..d2bedf07ba 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -1114,13 +1114,13 @@ namespace librealsense } } - void auto_calibrated::remove_leading_outliers(uint16_t data[256], int size) + void auto_calibrated::remove_outliers(uint16_t data[256], int size) { //Due to the async between the preset activation and the flow, the initial frames of the sample may include unrelated data. // the purpose of the function is to eliminate those by replacing them with a single value. // This assumes that the fill_rate is contiguous during scan (i.e. grows or shrinks monotonically) + // Additionally, this function rectifies singular sporadic outliers which are in the top 5% that may skew the results uint16_t base_fr = 0; - bool gap_identified = false; for (int i = 255; i >= 0; i--) { // Initialize reference value @@ -1134,18 +1134,28 @@ namespace librealsense // Rectify missing values if (!data[i]) data[i] = base_fr; - else + } + + static const int _outlier_percentile = 9500; // The outlier value is expected to be significantly above this value + for (int i = 0; i <= 253; i++) // Check for single outliers by assessing triples + { + auto val1 = data[i]; + auto val2 = data[i+1]; + auto val3 = data[i+2]; + + // Check for rectification candidate + if ((val2 > val1) && (val2 > val3)) { - if (gap_identified) - { - data[i] = base_fr; - continue; - } - float change_scale = (float)base_fr / data[i]; - if (change_scale > 1.5f || change_scale < 0.67f) // use 50% increment/decrement from previous sample + auto diff = val3-val1; + auto delta = std::max(std::abs(val2-val1),std::abs(val2-val3)); + // Actual outlier is a + // - spike 3 times or more than the expected gap + // - in the 5 top percentile + // - with neighbour values being smaller by at least 500 points to avoid clamping around the peak + if ((delta > 500) && (delta > (std::abs(diff) * 3)) && (val2 > _outlier_percentile)) { - gap_identified = true; - LOG_OCC_DEBUG(std::string(to_string() << "Start rectifying data from index " << i << " due to jump from " << base_fr << " to " << data[i])); + data[i+1] = data[i] + diff/2; + LOG_OCC_DEBUG(std::string(to_string() << "Outlier with value " << val2 << " was changed to be " << data[i+1] )); } } } @@ -1462,8 +1472,8 @@ namespace librealsense } #endif // Do not delete - to be used to improve algo robustness - //remove_leading_outliers(_fill_factor, _total_frames); fill_missing_data(_fill_factor, _total_frames); + remove_outliers(_fill_factor, _total_frames); std::stringstream ss; ss << "{\n \"calib type\":" << 0 << ",\n \"host assistance\":" << 2 << diff --git a/src/ds5/ds5-auto-calibration.h b/src/ds5/ds5-auto-calibration.h index 57433219ed..ea19c5e12c 100644 --- a/src/ds5/ds5-auto-calibration.h +++ b/src/ds5/ds5-auto-calibration.h @@ -79,7 +79,7 @@ namespace librealsense void get_target_dots_info(rs2_frame_queue* frames, float dots_x[4], float dots_y[4], rs2::stream_profile & profile, rs2_intrinsics & fy, int progress, update_progress_callback_ptr progress_callback); uint16_t calc_fill_rate(const rs2_frame* f); void fill_missing_data(uint16_t data[256], int size); - void remove_leading_outliers(uint16_t data[256], int size); + void remove_outliers(uint16_t data[256], int size); void collect_depth_frame_sum(const rs2_frame* f); DirectSearchCalibrationResult get_calibration_status(int timeout_ms, std::function progress_func, bool wait_for_final_results = true); From 1b0e06baedaef0de68f14dfb7818c4889e46a420 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 07:11:10 +0200 Subject: [PATCH 14/17] Fix NodeJs bindings with MIPI Metadata. Update attributes documentation --- include/librealsense2/h/rs_frame.h | 20 +++---- wrappers/nodejs/index.js | 84 ++++++++++++++++++++++++++++++ wrappers/nodejs/src/addon.cpp | 1 + 3 files changed, 95 insertions(+), 10 deletions(-) diff --git a/include/librealsense2/h/rs_frame.h b/include/librealsense2/h/rs_frame.h index d8edfe6078..07dd2e974a 100644 --- a/include/librealsense2/h/rs_frame.h +++ b/include/librealsense2/h/rs_frame.h @@ -62,18 +62,18 @@ typedef enum rs2_frame_metadata_value RS2_FRAME_METADATA_FRAME_LED_POWER , /**< Led power value 0-360. */ RS2_FRAME_METADATA_RAW_FRAME_SIZE , /**< The number of transmitted payload bytes, not including metadata */ RS2_FRAME_METADATA_GPIO_INPUT_DATA , /**< GPIO input data */ - RS2_FRAME_METADATA_SEQUENCE_NAME , /**< sub-preset id */ - RS2_FRAME_METADATA_SEQUENCE_ID , /**< sub-preset sequence id */ - RS2_FRAME_METADATA_SEQUENCE_SIZE , /**< sub-preset sequence size */ + RS2_FRAME_METADATA_SEQUENCE_NAME , /**< sub-preset id */ + RS2_FRAME_METADATA_SEQUENCE_ID , /**< sub-preset sequence id */ + RS2_FRAME_METADATA_SEQUENCE_SIZE , /**< sub-preset sequence size */ //mipi metadata_values - RS2_FRAME_METADATA_TRIGGER, - RS2_FRAME_METADATA_PRESET, - RS2_FRAME_METADATA_INPUT_WIDTH, - RS2_FRAME_METADATA_INPUT_HEIGHT, - RS2_FRAME_METADATA_SUB_PRESET_INFO, - RS2_FRAME_METADATA_CALIB_INFO, - RS2_FRAME_METADATA_CRC, + RS2_FRAME_METADATA_TRIGGER , /**< Frame trigger type */ + RS2_FRAME_METADATA_PRESET , /**< Preset id, used in MIPI SKU Metadata */ + RS2_FRAME_METADATA_INPUT_WIDTH , /**< Frame input width in pixels, used as safety attribute */ + RS2_FRAME_METADATA_INPUT_HEIGHT , /**< Frame input height in pixels, used as safety attribute */ + RS2_FRAME_METADATA_SUB_PRESET_INFO , /**< Sub-preset information */ + RS2_FRAME_METADATA_CALIB_INFO , /**< FW-controlled frame counter to be using in Calibration scenarios */ + RS2_FRAME_METADATA_CRC , /**< CRC checksum of the Metadata */ RS2_FRAME_METADATA_COUNT } rs2_frame_metadata_value; diff --git a/wrappers/nodejs/index.js b/wrappers/nodejs/index.js index 3260e4e802..7e74ee83e2 100644 --- a/wrappers/nodejs/index.js +++ b/wrappers/nodejs/index.js @@ -5684,6 +5684,90 @@ const frame_metadata = { * @type {Integer} */ FRAME_METADATA_LOW_LIGHT_COMPENSATION: RS2.RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, + /** + * Emitter mode: 0 - all emitters disabled. 1 - laser enabled. 2 - auto laser enabled (opt). 3 - LED enabled (opt). + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_FRAME_EMITTER_MODE: RS2.RS2_FRAME_METADATA_FRAME_EMITTER_MODE, + /** + * Led power value 0-360. + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_FRAME_LED_POWER: RS2.RS2_FRAME_METADATA_FRAME_LED_POWER, + /** + * The number of transmitted payload bytes, not including metadata + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_RAW_FRAME_SIZE: RS2.RS2_FRAME_METADATA_RAW_FRAME_SIZE, + /** + * GPIO input data + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_GPIO_INPUT_DATA: RS2.RS2_FRAME_METADATA_GPIO_INPUT_DATA, + /** + * Sub-preset name. Used in advanced scenarios, such as HDR + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_SEQUENCE_NAME: RS2.RS2_FRAME_METADATA_SEQUENCE_NAME, + /** + * Sub-preset sequence id + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_SEQUENCE_ID: RS2.RS2_FRAME_METADATA_SEQUENCE_ID, + /** + * Sub-preset sequence size in bytes + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_SEQUENCE_SIZE: RS2.RS2_FRAME_METADATA_SEQUENCE_SIZE, + /** + * Frame trigger type, used in MIPI SKU Metadata + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_TRIGGER: RS2.RS2_FRAME_METADATA_TRIGGER, + /** + * Preset id, used in MIPI SKU Metadata + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_PRESET: RS2.RS2_FRAME_METADATA_PRESET, + /** + * Frame input width in pixels, used in MIPI SKU Metadata as safety attribute + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_INPUT_WIDTH: RS2.RS2_FRAME_METADATA_INPUT_WIDTH, + /** + * Frame input height in pixels, used in MIPI SKU Metadata as safety attribute + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_INPUT_HEIGHT: RS2.RS2_FRAME_METADATA_INPUT_HEIGHT, + /** + * Sub-preset information, used in MIPI SKU + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_SUB_PRESET_INFO: RS2.RS2_FRAME_METADATA_SUB_PRESET_INFO, + /** + * FW-controlled frame counter to be using in Calibration scenarios. Used with MIPI SKU only + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_CALIB_INFO: RS2.RS2_FRAME_METADATA_CALIB_INFO, + /** + * CRC checksum of the Metadata, avalable for MIPI SKU only + *
Equivalent to its lowercase counterpart + * @type {Integer} + */ + RS2_FRAME_METADATA_CRC: RS2.RS2_FRAME_METADATA_CRC, /** * Number of enumeration values. Not a valid input: intended to be used in for-loops. * @type {Integer} diff --git a/wrappers/nodejs/src/addon.cpp b/wrappers/nodejs/src/addon.cpp index 22334ef372..9fe4e3f854 100644 --- a/wrappers/nodejs/src/addon.cpp +++ b/wrappers/nodejs/src/addon.cpp @@ -4749,6 +4749,7 @@ void InitModule(v8::Local exports) { _FORCE_SET_ENUM(RS2_FRAME_METADATA_INPUT_WIDTH); _FORCE_SET_ENUM(RS2_FRAME_METADATA_INPUT_HEIGHT); _FORCE_SET_ENUM(RS2_FRAME_METADATA_SUB_PRESET_INFO); + _FORCE_SET_ENUM(RS2_FRAME_METADATA_CALIB_INFO); _FORCE_SET_ENUM(RS2_FRAME_METADATA_CRC); _FORCE_SET_ENUM(RS2_FRAME_METADATA_COUNT); From e2f1f9b80e93d5de1b3c199a5873a9b730d4e096 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 07:24:07 +0200 Subject: [PATCH 15/17] Adjust line width according to NodeJs formatting --- wrappers/nodejs/index.js | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/wrappers/nodejs/index.js b/wrappers/nodejs/index.js index 7e74ee83e2..7dc9efcabb 100644 --- a/wrappers/nodejs/index.js +++ b/wrappers/nodejs/index.js @@ -5685,7 +5685,10 @@ const frame_metadata = { */ FRAME_METADATA_LOW_LIGHT_COMPENSATION: RS2.RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, /** - * Emitter mode: 0 - all emitters disabled. 1 - laser enabled. 2 - auto laser enabled (opt). 3 - LED enabled (opt). + * Emitter mode: 0 - all emitters disabled. + * 1- laser enabled. + * 2 - auto laser enabled (opt). + * 3 - LED enabled (opt). *
Equivalent to its lowercase counterpart * @type {Integer} */ From bb4f27a7d1a53d2cd7f12bc0811e0a3225b3e005 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 09:52:16 +0200 Subject: [PATCH 16/17] Addressing CR comments --- src/ds5/ds5-auto-calibration.cpp | 67 ++++++++++--------- src/ds5/ds5-color.cpp | 6 +- src/ds5/ds5-device.cpp | 10 +-- .../python/examples/depth_ucal_example.py | 7 +- 4 files changed, 46 insertions(+), 44 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index d2bedf07ba..d385e06040 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -10,12 +10,12 @@ #include "librealsense2/rsutil.h" #include "../algo.h" -#undef UCAL_DEBUG -#ifdef UCAL_DEBUG -#define LOG_OCC_DEBUG(...) do { CLOG(WARNING ,"librealsense") << __VA_ARGS__; } while(false) +#undef UCAL_PROFILE +#ifdef UCAL_PROFILE +#define LOG_OCC_WARN(...) do { CLOG(WARNING ,"librealsense") << __VA_ARGS__; } while(false) #else -#define LOG_OCC_DEBUG(...) -#endif //UCAL_DEBUG +#define LOG_OCC_WARN(...) +#endif //UCAL_PROFILE namespace librealsense { @@ -292,7 +292,7 @@ namespace librealsense { // Check calibration status auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status }); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " check occ status, res size = " << res.size())); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " check occ status, res size = " << res.size())); if (res.size() < sizeof(DirectSearchCalibrationResult)) { if (!((retries++) % 5)) // Add log debug once in a sec @@ -426,7 +426,7 @@ namespace librealsense _json = json; _action = auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START; - _interactive_scan = false;//bool(interactive_scan_v3); - enforce non-interactive run + _interactive_scan = false; // Production code must enforce non-interactive runs. Interactive scans for development only switch (speed) { case auto_calib_speed::speed_very_fast: @@ -496,7 +496,8 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 }); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); + LOG_OCC_WARN(std::string(to_string() << __LINE__ + << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size())); } if (host_assistance != host_assistance_type::assistance_start) @@ -504,7 +505,7 @@ namespace librealsense if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - LOG_OCC_DEBUG(" occ interactive_scan_control 0,0 - save statistics "); + LOG_OCC_WARN(" occ interactive_scan_control 0,0 - save statistics "); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -516,18 +517,18 @@ namespace librealsense } bool success = false; int iter =0; - do + do // Retries are needed to overcome MIPI SKU occasionaly reporting busy state { try { if (iter==0) // apply only in the first iteration - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // TODO: sending shorter request - auto res = _hw_monitor->send(cmd); // is likely to fail with MIPI SKU + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Sending shorter request may fail with MIPI SKU + auto res = _hw_monitor->send(cmd); success = true; } catch(...) { - LOG_OCC_DEBUG("occ Save Statistics result failed"); + LOG_OCC_WARN("occ Save Statistics result failed"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); }; } @@ -556,7 +557,7 @@ namespace librealsense if (progress_callback) progress_callback->on_update_progress(static_cast(100)); res = get_calibration_results(health); - LOG_OCC_DEBUG(std::string(to_string() << "Py: " << result.rightPy)); + LOG_OCC_WARN(std::string(to_string() << "Py: " << result.rightPy)); } } else if (calib_type == 1) @@ -600,7 +601,7 @@ namespace librealsense if (host_assistance == host_assistance_type::assistance_first_feed) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size())); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -709,7 +710,7 @@ namespace librealsense if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start) { _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_plus_fl_calib_begin, speed_fl, 0, p4 }); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size())); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size())); } if (host_assistance != host_assistance_type::assistance_start) @@ -717,7 +718,7 @@ namespace librealsense if ((host_assistance == host_assistance_type::assistance_first_feed) || (host_assistance == host_assistance_type::assistance_second_feed)) { command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << "occ interactive_scan_control 0,0")); uint8_t* p = reinterpret_cast(&step_count_v3); cmd.data.push_back(p[0]); cmd.data.push_back(p[1]); @@ -751,7 +752,7 @@ namespace librealsense try { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() )); if (res.size() < sizeof(DscPyRxFLCalibrationTableResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); @@ -885,7 +886,7 @@ namespace librealsense if (depth > 0) { - LOG_OCC_DEBUG("run_tare_calibration interactive control (2) with parameters: depth = " << depth); + LOG_OCC_WARN("run_tare_calibration interactive control (2) with parameters: depth = " << depth); _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 2, depth }); } else @@ -1155,7 +1156,7 @@ namespace librealsense if ((delta > 500) && (delta > (std::abs(diff) * 3)) && (val2 > _outlier_percentile)) { data[i+1] = data[i] + diff/2; - LOG_OCC_DEBUG(std::string(to_string() << "Outlier with value " << val2 << " was changed to be " << data[i+1] )); + LOG_OCC_WARN(std::string(to_string() << "Outlier with value " << val2 << " was changed to be " << data[i+1] )); } } } @@ -1242,7 +1243,7 @@ namespace librealsense std::vector res; rs2_metadata_type frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER); rs2_metadata_type frame_ts = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP); - bool tare_fc_workaround = (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB); //Ev - work-around tare implementation using rolling frame counter + bool tare_fc_workaround = (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB); //Tare calib shall use rolling frame counter bool mipi_sku = ((frame_interface*)f)->supports_frame_metadata(RS2_FRAME_METADATA_CALIB_INFO); if (mipi_sku) frame_counter = ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_CALIB_INFO); @@ -1272,7 +1273,7 @@ namespace librealsense _prev_frame_counter = frame_counter; if ((!tare_fc_workaround) && mipi_sku) _prev_frame_counter +=10; // Compensate for MIPI OCC calib. invoke delay _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START; - LOG_OCC_DEBUG(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); + LOG_OCC_WARN(std::string(to_string() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter)); return res; } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START) @@ -1297,7 +1298,7 @@ namespace librealsense _skipped_frames = 0; _prev_frame_counter = -1; _interactive_state = interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT; - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT")); } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT) { @@ -1357,19 +1358,19 @@ namespace librealsense { progress_callback->on_update_progress(static_cast(20 + static_cast(frame_counter * 60.0 / _total_frames))); } - auto fr = calc_fill_rate(f); + auto fill_rate = calc_fill_rate(f); if (frame_counter < 10) // handle discrepancy on stream/preset activation - fr = 0; + fill_rate = 0; if (frame_counter + fw_host_offset < 256) { - _fill_factor[frame_counter + fw_host_offset] = fr; - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fr)); + _fill_factor[frame_counter + fw_host_offset] = fill_rate; + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fill_rate)); } if (_interactive_scan) { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1}); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " occ interactive_scan_control 1,")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " occ interactive_scan_control 1,")); } _skipped_frames = 0; _prev_frame_counter = frame_counter; @@ -1378,7 +1379,7 @@ namespace librealsense else { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL; - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " go to final FW call")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " go to final FW call")); } } else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB) @@ -1396,7 +1397,7 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(progress_rate * 60.0))); } } - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " fr_c = " << frame_counter << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num)); if (frame_counter < _total_frames) @@ -1434,7 +1435,7 @@ namespace librealsense } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL) { - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL")); if (frame_counter > _total_frames) { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL; @@ -1442,12 +1443,12 @@ namespace librealsense else if (_interactive_scan) { _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1 }); - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << "Call for interactive_scan_control, 1")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << "Call for interactive_scan_control, 1")); } } if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL) { - LOG_OCC_DEBUG(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); + LOG_OCC_WARN(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); if (progress_callback) { progress_callback->on_update_progress(static_cast(80)); diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index 8aaa835048..a0e1e2bd68 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -392,15 +392,15 @@ namespace librealsense make_attribute_parser(&md_mipi_rgb_mode::low_light_compensation, md_mipi_rgb_control_attributes::low_light_compensation_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_INPUT_WIDTH, // added for mipi - use RS2_FRAME_METADATA_WIDTH (internal only) ?? + color_ep.register_metadata(RS2_FRAME_METADATA_INPUT_WIDTH, make_attribute_parser(&md_mipi_rgb_mode::input_width, md_mipi_rgb_control_attributes::input_width_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_INPUT_HEIGHT, // added for mipi - use RS2_FRAME_METADATA_HEIGHT (internal only) ?? + color_ep.register_metadata(RS2_FRAME_METADATA_INPUT_HEIGHT, make_attribute_parser(&md_mipi_rgb_mode::input_height, md_mipi_rgb_control_attributes::input_height_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_CRC, // added for mipi + color_ep.register_metadata(RS2_FRAME_METADATA_CRC, make_attribute_parser(&md_mipi_rgb_mode::crc, md_mipi_rgb_control_attributes::crc_attribute, md_prop_offset)); diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index af3f2eaea9..536c3d2a89 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -1285,7 +1285,7 @@ namespace librealsense make_attribute_parser(&md_mipi_depth_mode::laser_power, md_mipi_depth_control_attributes::laser_power_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_TRIGGER, // added for mipi + depth_sensor.register_metadata(RS2_FRAME_METADATA_TRIGGER, make_attribute_parser(&md_mipi_depth_mode::trigger, md_mipi_depth_control_attributes::trigger_attribute, md_prop_offset)); @@ -1293,7 +1293,7 @@ namespace librealsense make_attribute_parser(&md_mipi_depth_mode::projector_mode, md_mipi_depth_control_attributes::projector_mode_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_PRESET, // added for mipi + depth_sensor.register_metadata(RS2_FRAME_METADATA_PRESET, make_attribute_parser(&md_mipi_depth_mode::preset, md_mipi_depth_control_attributes::preset_attribute, md_prop_offset)); @@ -1306,11 +1306,11 @@ namespace librealsense make_attribute_parser(&md_mipi_depth_mode::auto_exposure_mode, md_mipi_depth_control_attributes::auto_exposure_mode_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_INPUT_WIDTH, // added for mipi - use RS2_FRAME_METADATA_WIDTH (internal only) ?? + depth_sensor.register_metadata(RS2_FRAME_METADATA_INPUT_WIDTH, make_attribute_parser(&md_mipi_depth_mode::input_width, md_mipi_depth_control_attributes::input_width_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_INPUT_HEIGHT, // added for mipi - use RS2_FRAME_METADATA_HEIGHT (internal only) ?? + depth_sensor.register_metadata(RS2_FRAME_METADATA_INPUT_HEIGHT, make_attribute_parser(&md_mipi_depth_mode::input_height, md_mipi_depth_control_attributes::input_height_attribute, md_prop_offset)); @@ -1322,7 +1322,7 @@ namespace librealsense make_attribute_parser(&md_mipi_depth_mode::calib_info, md_mipi_depth_control_attributes::calibration_info_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_CRC, // added for mipi + depth_sensor.register_metadata(RS2_FRAME_METADATA_CRC, make_attribute_parser(&md_mipi_depth_mode::crc, md_mipi_depth_control_attributes::crc_attribute, md_prop_offset)); diff --git a/wrappers/python/examples/depth_ucal_example.py b/wrappers/python/examples/depth_ucal_example.py index 8067bbec1d..dc85764706 100644 --- a/wrappers/python/examples/depth_ucal_example.py +++ b/wrappers/python/examples/depth_ucal_example.py @@ -1,9 +1,10 @@ ## License: Apache 2.0. See LICENSE file in root directory. ## Copyright(c) 2022 Intel Corporation. All Rights Reserved. -##################################################### -## auto calibration ## -##################################################### +######################################################################################################################################### +## This example exercises the auto-calibration APIs for OCC and Tare calib flows ## +## Shall be used as reference for users who are willing to integrate calibration flows into their respective UC-es ## +######################################################################################################################################### # First import the library import sys From 4035ee73591ba54c61c1a198aff744969d2b0097 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 7 Nov 2022 10:00:37 +0200 Subject: [PATCH 17/17] Dev comment for maintenance --- src/ds5/ds5-auto-calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index d385e06040..7b465985bd 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -529,7 +529,7 @@ namespace librealsense catch(...) { LOG_OCC_WARN("occ Save Statistics result failed"); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // For the FW to recuperate }; } while(( ++iter < 3) && (!success));