Skip to content

Commit

Permalink
Merge pull request #5768 from dorodnic/occ_fixes
Browse files Browse the repository at this point in the history 
On-Chip and Tare Calibration Fixes
  • Loading branch information
@dorodnic
dorodnic authored Feb 5, 2020
2 parents 9e8133b + 5fb13ec commit d363eb9
Show file tree
Hide file tree
Showing 7 changed files with 141 additions and 102 deletions.
6 changes: 3 additions & 3 deletions common/model-views.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2762,17 +2762,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
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
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
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
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

0 comments on commit d363eb9

Please sign in to comment.