diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 1e987e3c86..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)) @@ -405,6 +406,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) { @@ -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)); @@ -2063,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); @@ -2158,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(); diff --git a/include/librealsense2/h/rs_frame.h b/include/librealsense2/h/rs_frame.h index 51c8df2213..07dd2e974a 100644 --- a/include/librealsense2/h/rs_frame.h +++ b/include/librealsense2/h/rs_frame.h @@ -62,17 +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_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/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) { diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 84ebab0825..7b465985bd 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_PROFILE +#ifdef UCAL_PROFILE +#define LOG_OCC_WARN(...) do { CLOG(WARNING ,"librealsense") << __VA_ARGS__; } while(false) +#else +#define LOG_OCC_WARN(...) +#endif //UCAL_PROFILE + namespace librealsense { #pragma pack(push, 1) @@ -79,7 +86,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 +292,7 @@ namespace librealsense { // Check calibration status auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status }); + 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 @@ -418,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 = bool(interactive_scan_v3); + _interactive_scan = false; // Production code must enforce non-interactive runs. Interactive scans for development only switch (speed) { case auto_calib_speed::speed_very_fast: @@ -486,13 +494,18 @@ 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 }); + 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) { if (host_assistance == host_assistance_type::assistance_first_feed) { - command cmd(ds::AUTO_CALIB, get_calibration_result, 0, 0); // Rectify interactive_scan_control + command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0); + 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]); @@ -502,7 +515,24 @@ 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 // 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)); // Sending shorter request may fail with MIPI SKU + auto res = _hw_monitor->send(cmd); + success = true; + } + catch(...) + { + LOG_OCC_WARN("occ Save Statistics result failed"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // For the FW to recuperate + }; + } + while(( ++iter < 3) && (!success)); } DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count) @@ -527,7 +557,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_WARN(std::string(to_string() << "Py: " << result.rightPy)); } } else if (calib_type == 1) @@ -571,6 +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_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]); @@ -677,13 +708,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_plus_fl_calib_begin, speed_fl, 0, p4 }); + 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) { 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_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]); @@ -717,6 +752,7 @@ namespace librealsense try { auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result }); + 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!"); @@ -850,7 +886,7 @@ namespace librealsense if (depth > 0) { - LOG_DEBUG("run_tare_calibration interactive control 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 @@ -926,7 +962,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 @@ -1079,6 +1115,53 @@ namespace librealsense } } + 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; + 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; + } + + 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)) + { + 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)) + { + data[i+1] = data[i] + diff/2; + LOG_OCC_WARN(std::string(to_string() << "Outlier with value " << val2 << " was changed to be " << data[i+1] )); + } + } + } + } + // 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,6 +1242,12 @@ 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); //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); + if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START) { if (frame_counter <= 2) @@ -1182,7 +1271,9 @@ namespace librealsense res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback); } _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_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) @@ -1207,6 +1298,7 @@ namespace librealsense _skipped_frames = 0; _prev_frame_counter = -1; _interactive_state = interactive_calibration_state::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) { @@ -1266,10 +1358,19 @@ namespace librealsense { 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 fill_rate = calc_fill_rate(f); + if (frame_counter < 10) // handle discrepancy on stream/preset activation + fill_rate = 0; + if (frame_counter + fw_host_offset < 256) + { + _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) { - _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_OCC_WARN(std::string(to_string() << __LINE__ << " occ interactive_scan_control 1,")); } _skipped_frames = 0; _prev_frame_counter = frame_counter; @@ -1278,6 +1379,7 @@ namespace librealsense else { _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_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) @@ -1295,6 +1397,9 @@ namespace librealsense progress_callback->on_update_progress(static_cast(20 + static_cast(progress_rate * 60.0))); } } + 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) { if (_skipped_frames < FRAMES_TO_SKIP) @@ -1330,6 +1435,7 @@ namespace librealsense } if (_interactive_state == interactive_calibration_state::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; @@ -1337,10 +1443,12 @@ namespace librealsense else if (_interactive_scan) { _hw_monitor->send(command{ ds::AUTO_CALIB, 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_WARN(std::string(to_string() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL")); if (progress_callback) { progress_callback->on_update_progress(static_cast(80)); @@ -1364,7 +1472,9 @@ namespace librealsense fout.close(); } #endif + // Do not delete - to be used to improve algo robustness 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 << @@ -1677,7 +1787,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; diff --git a/src/ds5/ds5-auto-calibration.h b/src/ds5/ds5-auto-calibration.h index 5278d2da33..ea19c5e12c 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_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); 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 cfc1a0445d..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,19 +1306,23 @@ 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)); - 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_CRC, // added for mipi + 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, make_attribute_parser(&md_mipi_depth_mode::crc, md_mipi_depth_control_attributes::crc_attribute, md_prop_offset)); 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 { 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: diff --git a/wrappers/nodejs/index.js b/wrappers/nodejs/index.js index 3260e4e802..7dc9efcabb 100644 --- a/wrappers/nodejs/index.js +++ b/wrappers/nodejs/index.js @@ -5684,6 +5684,93 @@ 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); diff --git a/wrappers/python/examples/depth_ucal_example.py b/wrappers/python/examples/depth_ucal_example.py new file mode 100644 index 0000000000..dc85764706 --- /dev/null +++ b/wrappers/python/examples/depth_ucal_example.py @@ -0,0 +1,177 @@ +## License: Apache 2.0. See LICENSE file in root directory. +## Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +######################################################################################################################################### +## 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 +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:])