Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Close laser emitter on Focal Length calibration #11995

Merged
merged 2 commits into from
Jul 16, 2023
+99 −122
Merged

Close laser emitter on Focal Length calibration #11995

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ab36a88
Close laser emitter on Focal Length calibration
OhadMeir Jul 13, 2023
54363f6
Handle PR#11995 comments
OhadMeir Jul 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
205 changes: 87 additions & 118 deletions common/on-chip-calib.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,20 @@
#include "../src/algo.h"
#include "../tools/depth-quality/depth-metrics.h"


static constexpr float off_value = 0.0f;
static constexpr float on_value = 1.0f;

namespace rs2
{
on_chip_calib_manager::on_chip_calib_manager(viewer_model& viewer, std::shared_ptr<subdevice_model> sub, device_model& model, device dev, std::shared_ptr<subdevice_model> sub_color, bool uvmapping_calib_full)
: process_manager("On-Chip Calibration"), _model(model), _dev(dev), _sub(sub), _viewer(viewer), _sub_color(sub_color), py_px_only(!uvmapping_calib_full)
{
device_id_string = "Unknown";
device_name_string = "Unknown";
if (dev.supports(RS2_CAMERA_INFO_PRODUCT_ID))
{
device_id_string = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
if (val_in_range(device_id_string, { std::string("0AD3") }))
device_name_string = _dev.get_info( RS2_CAMERA_INFO_NAME );
if( val_in_range( device_name_string, { std::string( "Intel RealSense D415" ) } ) )
speed = 4;
}
if (dev.supports(RS2_CAMERA_INFO_FIRMWARE_VERSION))
Expand Down Expand Up @@ -69,6 +73,56 @@ namespace rs2
_sub_color->algo_roi = { 0, 0, 0, 0 };
}
}

void on_chip_calib_manager::save_laser_emitter_state()
{
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != _sub->options_metadata.end() ) //Option supported
{
laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED );
}
}

void on_chip_calib_manager::save_thermal_loop_state()
{
auto it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if( it != _sub->options_metadata.end() ) // Option supported
{
thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION );
}
}

void on_chip_calib_manager::restore_laser_emitter_state()
{
set_laser_emitter_state( laser_status_prev );
}

void on_chip_calib_manager::restore_thermal_loop_state()
{
set_thermal_loop_state( thermal_loop_prev );
}

void on_chip_calib_manager::set_laser_emitter_state( float value )
{
// Use options_model::set_option to update GUI after change
std::string ignored_error_message{ "" };
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if( it != _sub->options_metadata.end() ) // Option supported
{
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, value, ignored_error_message );
}
}

void on_chip_calib_manager::set_thermal_loop_state( float value )
{
// Use options_model::set_option to update GUI after change
std::string ignored_error_message{ "" };
auto it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if( it != _sub->options_metadata.end() ) // Option supported
{
it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, value, ignored_error_message );
}
}

void on_chip_calib_manager::stop_viewer(invoker invoke)
{
Expand Down Expand Up @@ -406,12 +460,6 @@ namespace rs2
bool frame_arrived = false;
try
{
if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
{
thermal_loop_prev = _sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION);
_sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f);
}

bool run_fl_calib = ( (action == RS2_CALIB_ACTION_FL_CALIB) && (w == 1280) && (h == 720));
if (action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH)
{
Expand Down Expand Up @@ -468,10 +516,9 @@ namespace rs2
break;
}

if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
_sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f);
if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
_sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f);
// TODO - When implementing UV mapping calibration - should remove from here and handle in process_flow()
set_laser_emitter_state( off_value );
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see you removed the supports check, why?
Don't needed anymore?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe add it inside the helper funcs?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is checked in the helper functions. More "clean" this way, and less code repetitions

set_thermal_loop_state( off_value );
}
else if (action == RS2_CALIB_ACTION_UVMAPPING_CALIB)
{
Expand Down Expand Up @@ -516,8 +563,8 @@ namespace rs2
break;
}

if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
_sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f);
// TODO - When implementing UV mapping calibration - should remove from here and handle in process_flow()
set_laser_emitter_state( off_value );
}
else if (action == RS2_CALIB_ACTION_FL_PLUS_CALIB)
{
Expand Down Expand Up @@ -553,8 +600,8 @@ namespace rs2
break;
}

if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
_sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f);
// TODO - When implementing FL plus calibration - should remove from here and handle in process_flow()
set_laser_emitter_state( off_value );
}
else if (run_fl_calib)
{
Expand All @@ -577,11 +624,6 @@ namespace rs2
}
}
}

if (_sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
_sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, 0.0f);
if (_sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
_sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, 0.f);
}
else
{
Expand Down Expand Up @@ -1423,50 +1465,40 @@ namespace rs2

if ( action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH )
{
//Laser should be turned off during ground truth calculation
//Use options_model::set_option to update GUI after change
std::string ignored_error_message { "" };
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != _sub->options_metadata.end() ) //Option supported
{
laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED );
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, 0.0f, ignored_error_message );
}
// Laser should be turned off during ground truth calculation
save_laser_emitter_state();
set_laser_emitter_state( off_value );

get_ground_truth();

//Restore laser
if ( it != _sub->options_metadata.end() ) //Option supported
{
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, laser_status_prev, ignored_error_message );
}
restore_laser_emitter_state();
}
else
{
try
{
//Save options that are going to change during the calibration
//Use options_model::set_option to update GUI after change
std::string ignored_error_message { "" };
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != _sub->options_metadata.end() ) //Option supported
{
laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED );
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, 1.0f, ignored_error_message );
}
it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if ( it != _sub->options_metadata.end() ) //Option supported
{
thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION );
it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, 0.0f, ignored_error_message );
}
// Save options that are going to change during the calibration
save_laser_emitter_state();
save_thermal_loop_state();

// Emitter on by default, off for FL calib and for D415 model
float emitter_value = on_value;
if( action == RS2_CALIB_ACTION_FL_CALIB || device_name_string == std::string( "Intel RealSense D415" ) )
emitter_value = off_value;
set_laser_emitter_state( emitter_value );

// Thermal loop should be off during calibration as to not change calibration tables during calibration
set_thermal_loop_state( off_value );

if (action == RS2_CALIB_ACTION_FL_CALIB)
calibrate_fl();
else if (action == RS2_CALIB_ACTION_UVMAPPING_CALIB)
calibrate_uv_mapping();
else
calibrate();

restore_laser_emitter_state();
restore_thermal_loop_state();
}
catch (...)
{
Expand All @@ -1485,18 +1517,8 @@ namespace rs2

//Restore options that were changed during the calibration.
//When calibration is successful options are restored in autocalib_notification_model::draw_content()
//Use options_model::set_option to update GUI after change
std::string ignored_error_message { "" };
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != _sub->options_metadata.end() ) //Option supported
{
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, laser_status_prev, ignored_error_message );
}
it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if ( it != _sub->options_metadata.end() ) //Option supported
{
it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, thermal_loop_prev, ignored_error_message );
}
restore_laser_emitter_state();
restore_thermal_loop_state();

if (_was_streaming)
start_viewer(0, 0, 0, invoke);
Expand Down Expand Up @@ -1851,10 +1873,6 @@ namespace rs2
{
get_manager().action = on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB;
update_state = update_state_prev;
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev);
get_manager().stop_viewer();
}

Expand Down Expand Up @@ -1891,10 +1909,6 @@ namespace rs2
{
get_manager().action = on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB;
update_state = update_state_prev;
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev);
}
else if (update_state == RS2_CALIB_STATE_GET_TARE_GROUND_TRUTH_FAILED)
{
Expand Down Expand Up @@ -2061,11 +2075,6 @@ namespace rs2

if (ImGui::Button(get_button_name.c_str(), { 42.0f, 20.f }))
{
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION);

update_state_prev = update_state;
update_state = RS2_CALIB_STATE_GET_TARE_GROUND_TRUTH;
get_manager().start_gt_viewer();
Expand All @@ -2074,7 +2083,7 @@ namespace rs2
ImGui::SetTooltip("%s", "Calculate ground truth for the specific target");

ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 30) });
get_manager().host_assistance = (get_manager().device_id_string == std::string("ABCD") ); // To be used for MIPI SKU only
get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only
bool assistance = (get_manager().host_assistance != 0);
if (ImGui::Checkbox("Host Assistance", &assistance))
get_manager().host_assistance = (assistance ? 1 : 0);
Expand Down Expand Up @@ -2170,7 +2179,7 @@ namespace rs2
// ImGui::SetTooltip("%s", "On-Chip Calibration Extended");

ImGui::SetCursorScreenPos({ float(x + 9), float(y + height - ImGui::GetTextLineHeightWithSpacing() - 31) });
get_manager().host_assistance = (get_manager().device_id_string == std::string("ABCD") ); // To be used for MIPI SKU only
get_manager().host_assistance = (get_manager().device_name_string == std::string("Intel RealSense D457") ); // To be used for MIPI SKU only
bool assistance = (get_manager().host_assistance != 0);
ImGui::Checkbox("Host Assistance", &assistance);
if (ImGui::IsItemHovered())
Expand Down Expand Up @@ -2269,11 +2278,6 @@ namespace rs2
ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) });
if (ImGui::Button(button_name.c_str(), { float(bar_width), 20.f }))
{
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION);

get_manager().restore_workspace([this](std::function<void()> a) { a(); });
get_manager().reset();
get_manager().retry_times = 0;
Expand Down Expand Up @@ -2345,30 +2349,8 @@ namespace rs2
}
else if (update_state == RS2_CALIB_STATE_CALIB_COMPLETE)
{
if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_ON_CHIP_CALIB ||
get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_TARE_CALIB )
{
//Restore options that were changed during the calibration.
//Use options_model::set_option to update GUI after change
std::string ignored_error_message { "" };
auto it = get_manager()._sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != get_manager()._sub->options_metadata.end() ) //Option supported
{
it->second.set_option( RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev, ignored_error_message );
}
it = get_manager()._sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if ( it != get_manager()._sub->options_metadata.end() ) //Option supported
{
it->second.set_option( RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev, ignored_error_message );
}
}
if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_UVMAPPING_CALIB)
{
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev);

ImGui::SetCursorScreenPos({ float(x + 20), float(y + 33) });
ImGui::Text("%s", "Health-Check Number for PX: ");

Expand Down Expand Up @@ -2453,14 +2435,6 @@ namespace rs2
}
else
{
if (get_manager().action == on_chip_calib_manager::RS2_CALIB_ACTION_FL_CALIB)
{
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager()._sub->s->set_option(RS2_OPTION_EMITTER_ENABLED, get_manager().laser_status_prev);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager()._sub->s->set_option(RS2_OPTION_THERMAL_COMPENSATION, get_manager().thermal_loop_prev);
}

auto health = get_manager().get_health();

auto recommend_keep = fabs(health) < 0.25f;
Expand Down Expand Up @@ -2851,11 +2825,6 @@ namespace rs2
ImGui::SetCursorScreenPos({ float(x + 5), float(y + height - 25) });
if (ImGui::Button(button_name.c_str(), { scale * 3, 20.f }))
{
if (get_manager()._sub->s->supports(RS2_OPTION_EMITTER_ENABLED))
get_manager().laser_status_prev = get_manager()._sub->s->get_option(RS2_OPTION_EMITTER_ENABLED);
if (get_manager()._sub->s->supports(RS2_OPTION_THERMAL_COMPENSATION))
get_manager().thermal_loop_prev = get_manager()._sub->s->get_option(RS2_OPTION_THERMAL_COMPENSATION);

get_manager().restore_workspace([this](std::function<void()> a) { a(); });
get_manager().reset();
get_manager().retry_times = 0;
Expand Down
16 changes: 12 additions & 4 deletions common/on-chip-calib.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,6 @@ namespace rs2
auto_calib_action action = RS2_CALIB_ACTION_ON_CHIP_CALIB;
int host_assistance = 0;
int step_count_v3 = 256;
float laser_status_prev = 0.0f;
float thermal_loop_prev = 0.f;

int fl_step_count = 51;
int fy_scan_range = 40;
Expand Down Expand Up @@ -103,8 +101,7 @@ namespace rs2
const std::string Y8_FORMAT = "Y8";
const std::string Z16_FORMAT = "Z16";
const std::string RGB8_FORMAT = "RGB8";
std::string device_id_string;

std::string device_name_string;

void calibrate();
void calibrate_fl();
Expand All @@ -115,6 +112,13 @@ namespace rs2
void turn_roi_on();
void turn_roi_off();

void save_laser_emitter_state();
void save_thermal_loop_state();
void restore_laser_emitter_state();
void restore_thermal_loop_state();
void set_laser_emitter_state( float value );
void set_thermal_loop_state( float value );

void start_gt_viewer();
void start_fl_viewer();
void start_uvmapping_viewer(bool b3D = false);
Expand All @@ -137,6 +141,10 @@ namespace rs2

device _dev;


float laser_status_prev = 0.0f;
float thermal_loop_prev = 0.0f;

bool _was_streaming = false;
bool _synchronized = false;
bool _post_processing = false;
Expand Down