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

Jump to bottom

On-Chip and Tare Calibration Fixes #5768

Merged
merged 8 commits into from
Feb 5, 2020
Merged

On-Chip and Tare Calibration Fixes #5768

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
fa8ad65
Adding extrinsic/intrinsic scan toggle to on-chip and tare calibratio…
dorodnic Jan 20, 2020
5f9a51e
Fixing on-chip and tare using USB2 in DQT
dorodnic Jan 20, 2020
741d3ec
Give special treatment to crop resolution used in OCC
dorodnic Jan 20, 2020
cc9a189
Revert "Fixing on-chip and tare using USB2 in DQT" and more clean fix
dorodnic Feb 2, 2020
3ca3135
Better depth hover text
dorodnic Feb 2, 2020
21d8e4e
Fix integrity error in tare calib
dorodnic Feb 3, 2020
de606ce
Add apply preset param to OCC and tare
dorodnic Feb 3, 2020
5fb13ec
Moving some code around
dorodnic Feb 3, 2020
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
6 changes: 3 additions & 3 deletions common/model-views.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2744,17 +2744,17 @@ namespace rs2
float val{};
if (texture->try_pick(x, y, &val))
{
ss << ", *p: 0x" << std::hex << static_cast<int>(round(val));
ss << " 0x" << std::hex << static_cast<int>(round(val)) << " =";
}

if (texture->get_last_frame().is<depth_frame>())
{
auto meters = texture->get_last_frame().as<depth_frame>().get_distance(x, y);

if (viewer.metric_system)
ss << std::dec << ", " << std::setprecision(2) << meters << " meters";
ss << std::dec << " " << std::setprecision(3) << meters << " meters";
else
ss << std::dec << ", " << std::setprecision(2) << meters / FEET_TO_METER << " feet";
ss << std::dec << " " << std::setprecision(3) << meters / FEET_TO_METER << " feet";
}

std::string msg(ss.str().c_str());
Expand Down
62 changes: 53 additions & 9 deletions common/on-chip-calib.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,10 @@ namespace rs2
{
for (auto&& stream : _viewer.streams)
{
if (std::find(profiles.begin(), profiles.end(),
stream.second.original_profile) != profiles.end())
if (std::find_if(profiles.begin(), profiles.end(),
[&stream](rs2::stream_profile p) {
return stream.second.original_profile.unique_id() == p.unique_id();
}) != profiles.end())
{
auto now = std::chrono::high_resolution_clock::now();
if (now - stream.second.last_frame < std::chrono::milliseconds(100))
Expand Down Expand Up @@ -309,8 +311,10 @@ namespace rs2
{
std::stringstream ss;
ss << "{\n \"speed\":" << speed <<
",\n \"average_step_count\":" << average_step_count <<
",\n \"step_count\":" << step_count <<
",\n \"average step count\":" << average_step_count <<
",\n \"scan parameter\":" << (intrinsic_scan ? 0 : 1) <<
",\n \"step count\":" << step_count <<
",\n \"apply preset\":" << (apply_preset ? 1 : 0) <<
",\n \"accuracy\":" << accuracy <<"}";

std::string json = ss.str();
Expand Down Expand Up @@ -443,6 +447,38 @@ namespace rs2
notification_model::draw_dismiss(win, x, y);
}

void autocalib_notification_model::draw_intrinsic_extrinsic(int x, int y)
{
bool intrinsic = get_manager().intrinsic_scan;
bool extrinsic = !intrinsic;

ImGui::SetCursorScreenPos({ float(x + 9), float(y + 35 + ImGui::GetTextLineHeightWithSpacing()) });

std::string id = to_string() << "##Intrinsic_" << index;
if (ImGui::Checkbox("Intrinsic", &intrinsic))
{
extrinsic = !intrinsic;
}
if (ImGui::IsItemHovered())
{
ImGui::SetTooltip("%s", "Calibrate intrinsic parameters of the camera");
}
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 35 + ImGui::GetTextLineHeightWithSpacing()) });

id = to_string() << "##Intrinsic_" << index;

if (ImGui::Checkbox("Extrinsic", &extrinsic))
{
intrinsic = !extrinsic;
}
if (ImGui::IsItemHovered())
{
ImGui::SetTooltip("%s", "Calibrate extrinsic parameters between left and right cameras");
}

get_manager().intrinsic_scan = intrinsic;
}

void autocalib_notification_model::draw_content(ux_window& win, int x, int y, float t, std::string& error_message)
{
using namespace std;
Expand Down Expand Up @@ -571,19 +607,25 @@ namespace rs2
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 35 + ImGui::GetTextLineHeightWithSpacing()) });

ImGui::PopItemWidth();

draw_intrinsic_extrinsic(x, y + 3 * ImGui::GetTextLineHeightWithSpacing() - 10);

ImGui::SetCursorScreenPos({ float(x + 9), float(y + 52 + 4 * ImGui::GetTextLineHeightWithSpacing()) });
id = to_string() << "Apply High-Accuracy Preset##apply_preset_" << index;
ImGui::Checkbox(id.c_str(), &get_manager().apply_preset);
}

if (update_state == RS2_CALIB_STATE_TARE_INPUT_ADVANCED)
{
ImGui::SetCursorScreenPos({ float(x + 9), float(y + 48 + 3 * ImGui::GetTextLineHeightWithSpacing()) });
ImGui::SetCursorScreenPos({ float(x + 9), float(y + 60 + 5 * ImGui::GetTextLineHeightWithSpacing()) });
ImGui::Text("%s", "Ground Truth(mm):");
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 45 + 3 * ImGui::GetTextLineHeightWithSpacing()) });
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 58 + 5 * ImGui::GetTextLineHeightWithSpacing()) });
}
else
{
ImGui::SetCursorScreenPos({ float(x + 9), float(y + 33) });
ImGui::Text("%s", "Ground Truth(mm):");
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 28) });
ImGui::SetCursorScreenPos({ float(x + 135), float(y + 30) });
}

if (ImGui::IsItemHovered())
Expand Down Expand Up @@ -652,6 +694,8 @@ namespace rs2
ImGui::Combo(id.c_str(), &get_manager().speed, vals_cstr.data(), vals.size());
ImGui::PopItemWidth();

draw_intrinsic_extrinsic(x, y);

auto sat = 1.f + sin(duration_cast<milliseconds>(system_clock::now() - created_time).count() / 700.f) * 0.1f;

ImGui::PushStyleColor(ImGuiCol_Button, saturate(sensor_header_light_blue, sat));
Expand Down Expand Up @@ -1085,9 +1129,9 @@ namespace rs2
if (get_manager().allow_calib_keep()) return 170;
else return 80;
}
else if (update_state == RS2_CALIB_STATE_SELF_INPUT) return 85;
else if (update_state == RS2_CALIB_STATE_SELF_INPUT) return 110;
else if (update_state == RS2_CALIB_STATE_TARE_INPUT) return 85;
else if (update_state == RS2_CALIB_STATE_TARE_INPUT_ADVANCED) return 160;
else if (update_state == RS2_CALIB_STATE_TARE_INPUT_ADVANCED) return 210;
else return 100;
}

Expand Down
3 changes: 3 additions & 0 deletions common/on-chip-calib.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@ namespace rs2
int accuracy = 2;
int speed = 3;
bool tare = false;
bool intrinsic_scan = true;
bool apply_preset = true;

void calibrate();

Expand Down Expand Up @@ -115,6 +117,7 @@ namespace rs2
void draw_content(ux_window& win, int x, int y, float t, std::string& error_message) override;
void draw_dismiss(ux_window& win, int x, int y) override;
void draw_expanded(ux_window& win, std::string& error_message) override;
void draw_intrinsic_extrinsic(int x, int y);
int calc_height() override;
void dismiss(bool snooze) override;

Expand Down
114 changes: 73 additions & 41 deletions src/ds5/ds5-auto-calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,50 @@

namespace librealsense
{

#pragma pack(push, 1)
#pragma pack(1)
struct DirectSearchCalibrationResult
{
uint16_t status; // DscStatus
uint16_t stepCount;
uint16_t stepSize; // 1/1000 of a pixel
uint32_t pixelCountThreshold; // minimum number of pixels in
// selected bin
uint16_t minDepth; // Depth range for FWHM
uint16_t maxDepth;
uint32_t rightPy; // 1/1000000 of normalized unit
float healthCheck;
float rightRotation[9]; // Right rotation
};

struct DscResultParams
{
uint16_t m_status;
float m_healthCheck;
};

struct DscResultBuffer
{
uint16_t m_paramSize;
DscResultParams m_dscResultParams;
uint16_t m_tableSize;
};

enum rs2_dsc_status : uint16_t
{
RS2_DSC_STATUS_SUCCESS = 0, /**< Self calibration succeeded*/
RS2_DSC_STATUS_RESULT_NOT_READY = 1, /**< Self calibration result is not ready yet*/
RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW = 2, /**< There are too little textures in the scene*/
RS2_DSC_STATUS_EDGE_TOO_CLOSE = 3, /**< Self calibration range is too small*/
RS2_DSC_STATUS_NOT_CONVERGE = 4, /**< For tare calibration only*/
RS2_DSC_STATUS_BURN_SUCCESS = 5,
RS2_DSC_STATUS_BURN_ERROR = 6,
RS2_DSC_STATUS_NO_DEPTH_AVERAGE = 7
};

#pragma pack(pop)

enum auto_calib_sub_cmd : uint8_t
{
auto_calib_begin = 0x08,
Expand Down Expand Up @@ -82,7 +126,7 @@ namespace librealsense
const int DEFAULT_SAMPLING = data_sampling::polling;

auto_calibrated::auto_calibrated(std::shared_ptr<hw_monitor>& hwm)
:_hw_monitor(hwm){}
: _hw_monitor(hwm){}

std::map<std::string, int> auto_calibrated::parse_json(std::string json_content)
{
Expand All @@ -100,27 +144,29 @@ namespace librealsense
return values;
}

void try_fetch(std::map<std::string, int> jsn, std::string key, int* value)
{
std::replace(key.begin(), key.end(), '_', ' '); // Treat _ as space
if (jsn.find(key) != jsn.end())
{
*value = jsn[key];
}
}

std::vector<uint8_t> auto_calibrated::run_on_chip_calibration(int timeout_ms, std::string json, float* health, update_progress_callback_ptr progress_callback)
{
int speed = DEFAULT_SPEED;
int scan_parameter = DEFAULT_SCAN;
int data_sampling = DEFAULT_SAMPLING;
int apply_preset = 1;

if (json.size() > 0)
{
auto jsn = parse_json(json);
if (jsn.find("speed") != jsn.end())
{
speed = jsn["speed"];
}
if (jsn.find("scan parameter") != jsn.end())
{
scan_parameter = jsn["scan parameter"];
}
if (jsn.find("data sampling") != jsn.end())
{
data_sampling = jsn["data sampling"];
}
try_fetch(jsn, "speed", &speed);
try_fetch(jsn, "scan parameter", &scan_parameter);
try_fetch(jsn, "data sampling", &data_sampling);
try_fetch(jsn, "apply preset", &apply_preset);
}

LOG_INFO("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling);
Expand All @@ -130,7 +176,7 @@ namespace librealsense
param4 param{ (byte)scan_parameter, 0, (byte)data_sampling };

std::shared_ptr<ds5_advanced_mode_base> preset_recover;
if (speed == speed_white_wall)
if (speed == speed_white_wall && apply_preset)
preset_recover = change_preset();

std::this_thread::sleep_for(std::chrono::milliseconds(200));
Expand Down Expand Up @@ -203,40 +249,26 @@ namespace librealsense
int speed = DEFAULT_SPEED;
int scan_parameter = DEFAULT_SCAN;
int data_sampling = DEFAULT_SAMPLING;
int apply_preset = 1;

if (json.size() > 0)
{
auto jsn = parse_json(json);
if (jsn.find("speed") != jsn.end())
{
speed = jsn["speed"];
}
if (jsn.find("average step count") != jsn.end())
{
average_step_count = jsn["average step count"];
}
if (jsn.find("step count") != jsn.end())
{
step_count = jsn["step count"];
}
if (jsn.find("accuracy") != jsn.end())
{
accuracy = jsn["accuracy"];
}
if (jsn.find("scan parameter") != jsn.end())
{
scan_parameter = jsn["scan parameter"];
}
if (jsn.find("data sampling") != jsn.end())
{
data_sampling = jsn["data sampling"];
}
try_fetch(jsn, "speed", &speed);
try_fetch(jsn, "average step count", &average_step_count);
try_fetch(jsn, "step count", &step_count);
try_fetch(jsn, "accuracy", &accuracy);
try_fetch(jsn, "scan parameter", &scan_parameter);
try_fetch(jsn, "data sampling", &data_sampling);
try_fetch(jsn, "apply preset", &apply_preset);
}

LOG_INFO("run_tare_calibration with parameters: speed = " << speed << " average_step_count = " << average_step_count << " step_count = " << step_count << " accuracy = " << accuracy << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling);
check_tare_params(speed, scan_parameter, data_sampling, average_step_count, step_count, accuracy);

auto preset_recover = change_preset();
std::shared_ptr<ds5_advanced_mode_base> preset_recover;
if (apply_preset)
preset_recover = change_preset();

auto param2 = (int)ground_truth_mm * 100;

Expand Down Expand Up @@ -268,7 +300,7 @@ namespace librealsense
if (res.size() < sizeof(DirectSearchCalibrationResult))
throw std::runtime_error("Not enough data from CALIB_STATUS!");

auto result = *reinterpret_cast<DirectSearchCalibrationResult*>(res.data());
result = *reinterpret_cast<DirectSearchCalibrationResult*>(res.data());
done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY;
}

Expand Down Expand Up @@ -353,7 +385,7 @@ namespace librealsense
throw invalid_value_exception(to_string() << "Auto calibration failed! Given value of 'subpixel accuracy' " << accuracy << " is out of range (0 - 3).");
}

void auto_calibrated::handle_calibration_error(rs2_dsc_status status) const
void auto_calibrated::handle_calibration_error(int status) const
{
if (status == RS2_DSC_STATUS_EDGE_TOO_CLOSE)
{
Expand Down
45 changes: 1 addition & 44 deletions src/ds5/ds5-auto-calibration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,49 +10,6 @@ namespace librealsense
{
class auto_calibrated : public auto_calibrated_interface
{
#pragma pack(push, 1)
#pragma pack(1)
struct DirectSearchCalibrationResult
{
uint16_t status; // DscStatus
uint16_t stepCount;
uint16_t stepSize; // 1/1000 of a pixel
uint32_t pixelCountThreshold; // minimum number of pixels in
// selected bin
uint16_t minDepth; // Depth range for FWHM
uint16_t maxDepth;
uint32_t rightPy; // 1/1000000 of normalized unit
float healthCheck;
float rightRotation[9]; // Right rotation
};

struct DscResultParams
{
uint16_t m_status;
float m_healthCheck;
};

struct DscResultBuffer
{
uint16_t m_paramSize;
DscResultParams m_dscResultParams;
uint16_t m_tableSize;
};

enum rs2_dsc_status : uint16_t
{
RS2_DSC_STATUS_SUCCESS = 0, /**< Self calibration succeeded*/
RS2_DSC_STATUS_RESULT_NOT_READY = 1, /**< Self calibration result is not ready yet*/
RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW = 2, /**< There are too little textures in the scene*/
RS2_DSC_STATUS_EDGE_TOO_CLOSE = 3, /**< Self calibration range is too small*/
RS2_DSC_STATUS_NOT_CONVERGE = 4, /**< For tare calibration only*/
RS2_DSC_STATUS_BURN_SUCCESS = 5,
RS2_DSC_STATUS_BURN_ERROR = 6,
RS2_DSC_STATUS_NO_DEPTH_AVERAGE = 7
};

#pragma pack(pop)

public:
auto_calibrated(std::shared_ptr<hw_monitor>& hwm);
void write_calibration() const override;
Expand All @@ -64,7 +21,7 @@ namespace librealsense

private:
std::vector<uint8_t> get_calibration_results(float* health = nullptr) const;
void handle_calibration_error(rs2_dsc_status status) const;
void handle_calibration_error(int status) const;
std::map<std::string, int> parse_json(std::string json);
std::shared_ptr< ds5_advanced_mode_base> change_preset();
void check_params(int speed, int scan_parameter, int data_sampling) const;
Expand Down
Loading