Skip to content

Commit

Permalink
Debug OCC outliers/ stability
Browse files Browse the repository at this point in the history
  • Loading branch information
Evgeni Raikhel committed Nov 5, 2022
1 parent 06fe68c commit 74a4fc4
Show file tree
Hide file tree
Showing 3 changed files with 111 additions and 9 deletions.
16 changes: 14 additions & 2 deletions common/on-chip-calib.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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<int>(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));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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?

Expand Down Expand Up @@ -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))) });
Expand Down Expand Up @@ -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"));
}
}

Expand Down Expand Up @@ -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<char*>(s.c_str()),
s.size() + 1, { 490,100 }, ImGuiInputTextFlags_AutoSelectAll | ImGuiInputTextFlags_ReadOnly);
ImGui::PopStyleColor();
Expand Down
103 changes: 96 additions & 7 deletions src/ds5/ds5-auto-calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -463,7 +466,7 @@ namespace librealsense
std::shared_ptr<ds5_advanced_mode_base> 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;
Expand All @@ -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<uint8_t*>(&step_count_v3);
cmd.data.push_back(p[0]);
cmd.data.push_back(p[1]);
Expand Down Expand Up @@ -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<uint8_t*>(&step_count_v3);
cmd.data.push_back(p[0]);
cmd.data.push_back(p[1]);
Expand Down Expand Up @@ -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<uint8_t*>(&step_count_v3);
cmd.data.push_back(p[0]);
cmd.data.push_back(p[1]);
Expand Down Expand Up @@ -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!");
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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<float>(80 + count++));
else if (depth == 0)
progress_callback->on_update_progress(count++ * (2.f * speed)); //curently this number does not reflect the actual progress
Expand Down Expand Up @@ -1031,6 +1050,10 @@ namespace librealsense
}
#endif
double tmp = static_cast<double>(counter) / static_cast<double>(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<uint16_t>(tmp + 0.5f);

}
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -1159,10 +1219,12 @@ namespace librealsense
{
std::vector<uint8_t> 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)
Expand All @@ -1181,20 +1243,27 @@ 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)
{
if (progress_callback)
{
progress_callback->on_update_progress(static_cast<float>(15));
}
//LOG_WARNING(std::string(to_string() << __LINE__ << " return on still waiting"));
return res;
}
if (progress_callback)
Expand All @@ -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
Expand Down Expand Up @@ -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<float>(20 + static_cast<int>(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;
Expand All @@ -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)
Expand Down Expand Up @@ -1330,17 +1415,20 @@ 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;
}
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<float>(80));
Expand All @@ -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 <<
Expand Down
Loading

0 comments on commit 74a4fc4

Please sign in to comment.