From fa8ad652b42d30cb4268f15037cd9f9f3194e431 Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 20 Jan 2020 04:43:51 -0800 Subject: [PATCH 1/8] Adding extrinsic/intrinsic scan toggle to on-chip and tare calibration UI --- common/on-chip-calib.cpp | 47 +++++++++++++++++++++++++++++++++++----- common/on-chip-calib.h | 2 ++ 2 files changed, 44 insertions(+), 5 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 7f02a98f06..4436f37534 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -310,6 +310,7 @@ namespace rs2 std::stringstream ss; ss << "{\n \"speed\":" << speed << ",\n \"average_step_count\":" << average_step_count << + ",\n \"scan parameter\":" << (intrinsic_scan ? 0 : 1) << ",\n \"step_count\":" << step_count << ",\n \"accuracy\":" << accuracy <<"}"; @@ -443,6 +444,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; @@ -571,19 +604,21 @@ namespace rs2 ImGui::SetCursorScreenPos({ float(x + 135), float(y + 35 + ImGui::GetTextLineHeightWithSpacing()) }); ImGui::PopItemWidth(); + + draw_intrinsic_extrinsic(x, y + 3 * ImGui::GetTextLineHeightWithSpacing() - 10); } 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 + 52 + 4 * 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 + 50 + 4 * 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()) @@ -652,6 +687,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(system_clock::now() - created_time).count() / 700.f) * 0.1f; ImGui::PushStyleColor(ImGuiCol_Button, saturate(sensor_header_light_blue, sat)); @@ -1085,9 +1122,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 185; else return 100; } diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index f87e7bcfed..6d35eb2e8b 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -53,6 +53,7 @@ namespace rs2 int accuracy = 2; int speed = 3; bool tare = false; + bool intrinsic_scan = true; void calibrate(); @@ -115,6 +116,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; From 5f9a51e999754587e8394c7549f956ec5af72493 Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 20 Jan 2020 04:55:58 -0800 Subject: [PATCH 2/8] Fixing on-chip and tare using USB2 in DQT --- common/on-chip-calib.cpp | 248 +-------------------------------------- common/on-chip-calib.h | 8 -- 2 files changed, 2 insertions(+), 254 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 4436f37534..60aadce91a 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -48,35 +48,6 @@ namespace rs2 catch (...) {} } - // Wait for next depth frame and return it - rs2::depth_frame on_chip_calib_manager::fetch_depth_frame(invoker invoke) - { - auto profiles = _sub->get_selected_profiles(); - bool frame_arrived = false; - rs2::depth_frame res = rs2::frame{}; - while (!frame_arrived) - { - for (auto&& stream : _viewer.streams) - { - if (std::find(profiles.begin(), profiles.end(), - stream.second.original_profile) != profiles.end()) - { - auto now = std::chrono::high_resolution_clock::now(); - if (now - stream.second.last_frame < std::chrono::milliseconds(100)) - { - if (auto f = stream.second.texture->get_last_frame(false).as()) - { - frame_arrived = true; - res = f; - } - } - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - return res; - } - void on_chip_calib_manager::start_viewer(int w, int h, int fps, invoker invoke) { try @@ -159,129 +130,6 @@ namespace rs2 catch (...) {} } - std::pair on_chip_calib_manager::get_metric(bool use_new) - { - return _metrics[use_new ? 1 : 0]; - } - - std::pair on_chip_calib_manager::get_depth_metrics(invoker invoke) - { - using namespace depth_quality; - - auto f = fetch_depth_frame(invoke); - auto sensor = _sub->s->as(); - auto intr = f.get_profile().as().get_intrinsics(); - rs2::region_of_interest roi { (int)(f.get_width() * 0.45f), (int)(f.get_height() * 0.45f), - (int)(f.get_width() * 0.55f), (int)(f.get_height() * 0.55f) }; - std::vector v; - - std::vector fill_rates; - std::vector rmses; - - auto show_plane = _viewer.draw_plane; - - auto on_frame = [sensor, &fill_rates, &rmses, this]( - const std::vector& points, - const plane p, - const rs2::region_of_interest roi, - const float baseline_mm, - const float focal_length_pixels, - const int ground_thruth_mm, - const bool plane_fit, - const float plane_fit_to_ground_truth_mm, - const float distance_mm, - bool record, - std::vector& samples) - { - float TO_METERS = sensor.get_depth_scale(); - static const float TO_MM = 1000.f; - static const float TO_PERCENT = 100.f; - - // Calculate fill rate relative to the ROI - auto fill_rate = points.size() / float((roi.max_x - roi.min_x)*(roi.max_y - roi.min_y)) * TO_PERCENT; - fill_rates.push_back(fill_rate); - - if (!plane_fit) return; - - std::vector points_set = points; - std::vector distances; - - // Reserve memory for the data - distances.reserve(points.size()); - - // Convert Z values into Depth values by aligning the Fitted plane with the Ground Truth (GT) plane - // Calculate distance and disparity of Z values to the fitted plane. - // Use the rotated plane fit to calculate GT errors - for (auto point : points_set) - { - // Find distance from point to the reconstructed plane - auto dist2plane = p.a*point.x + p.b*point.y + p.c*point.z + p.d; - // Project the point to plane in 3D and find distance to the intersection point - rs2::float3 plane_intersect = { float(point.x - dist2plane*p.a), - float(point.y - dist2plane*p.b), - float(point.z - dist2plane*p.c) }; - - // Store distance, disparity and gt- error - distances.push_back(dist2plane * TO_MM); - } - - // Remove outliers [below 1% and above 99%) - std::sort(points_set.begin(), points_set.end(), [](const rs2::float3& a, const rs2::float3& b) { return a.z < b.z; }); - size_t outliers = points_set.size() / 50; - points_set.erase(points_set.begin(), points_set.begin() + outliers); // crop min 0.5% of the dataset - points_set.resize(points_set.size() - outliers); // crop max 0.5% of the dataset - - // Calculate Plane Fit RMS (Spatial Noise) mm - double plane_fit_err_sqr_sum = std::inner_product(distances.begin(), distances.end(), distances.begin(), 0.); - auto rms_error_val = static_cast(std::sqrt(plane_fit_err_sqr_sum / distances.size())); - auto rms_error_val_per = TO_PERCENT * (rms_error_val / distance_mm); - rmses.push_back(rms_error_val_per); - }; - - auto rms_std = 1000.f; - auto new_rms_std = rms_std; - auto count = 0; - - // Capture metrics on bundles of 31 frame - // Repeat until get "decent" bundle or reach 10 sec - do - { - rms_std = new_rms_std; - - rmses.clear(); - - for (int i = 0; i < 31; i++) - { - f = fetch_depth_frame(invoke); - auto res = depth_quality::analyze_depth_image(f, sensor.get_depth_scale(), sensor.get_stereo_baseline(), - &intr, roi, 0, true, v, false, on_frame); - - _viewer.draw_plane = true; - _viewer.roi_rect = res.plane_corners; - } - - auto rmses_sum_sqr = std::inner_product(rmses.begin(), rmses.end(), rmses.begin(), 0.); - new_rms_std = static_cast(std::sqrt(rmses_sum_sqr / rmses.size())); - } while ((new_rms_std < rms_std * 0.8f && new_rms_std > 10.f) && count++ < 10); - - std::sort(fill_rates.begin(), fill_rates.end()); - std::sort(rmses.begin(), rmses.end()); - - float median_fill_rate, median_rms; - if (fill_rates.empty()) - median_fill_rate = 0; - else - median_fill_rate = fill_rates[fill_rates.size() / 2]; - if (rmses.empty()) - median_rms = 0; - else - median_rms = rmses[rmses.size() / 2]; - - _viewer.draw_plane = show_plane; - - return { median_fill_rate, median_rms }; - } - std::vector on_chip_calib_manager::safe_send_command( const std::vector& cmd, const std::string& name) { @@ -349,10 +197,6 @@ namespace rs2 { start_viewer(0,0,0, invoke); } - - // Capture metrics before - auto metrics_before = get_depth_metrics(invoke); - _metrics.push_back(metrics_before); stop_viewer(invoke); @@ -372,10 +216,6 @@ namespace rs2 // Make new calibration active apply_calib(true); - // Capture metrics after - auto metrics_after = get_depth_metrics(invoke); - _metrics.push_back(metrics_after); - _progress = 100; _done = true; @@ -805,92 +645,8 @@ namespace rs2 } } - auto old_fr = get_manager().get_metric(false).first; - auto new_fr = get_manager().get_metric(true).first; - - auto old_rms = fabs(get_manager().get_metric(false).second); - auto new_rms = fabs(get_manager().get_metric(true).second); - - auto fr_improvement = 100.f * ((new_fr - old_fr) / old_fr); - auto rms_improvement = 100.f * ((old_rms - new_rms) / old_rms); - - std::string old_units = "mm"; - if (old_rms > 10.f) - { - old_rms /= 10.f; - old_units = "cm"; - } - std::string new_units = "mm"; - if (new_rms > 10.f) - { - new_rms /= 10.f; - new_units = "cm"; - } - - // NOTE: Disabling metrics temporarily - // TODO: Re-enable in future release - if (/* fr_improvement > 1.f || rms_improvement > 1.f */ false) - { - std::string txt = to_string() << " Fill-Rate: " << std::setprecision(1) << std::fixed << new_fr << "%%"; - - if (!use_new_calib) - { - txt = to_string() << " Fill-Rate: " << std::setprecision(1) << std::fixed << old_fr << "%%\n"; - } - - ImGui::SetCursorScreenPos({ float(x + 12), float(y + 90) }); - ImGui::PushFont(win.get_large_font()); - ImGui::Text("%s", static_cast(textual_icons::check)); - ImGui::PopFont(); - - ImGui::SetCursorScreenPos({ float(x + 35), float(y + 92) }); - ImGui::Text("%s", txt.c_str()); - - if (use_new_calib) - { - ImGui::SameLine(); - - ImGui::PushStyleColor(ImGuiCol_Text, white); - txt = to_string() << " ( +" << std::fixed << std::setprecision(0) << fr_improvement << "%% )"; - ImGui::Text("%s", txt.c_str()); - ImGui::PopStyleColor(); - } - - if (rms_improvement > 1.f) - { - if (use_new_calib) - { - txt = to_string() << " Noise Estimate: " << std::setprecision(2) << std::fixed << new_rms << new_units; - } - else - { - txt = to_string() << " Noise Estimate: " << std::setprecision(2) << std::fixed << old_rms << old_units; - } - - ImGui::SetCursorScreenPos({ float(x + 12), float(y + 90 + ImGui::GetTextLineHeight() + 6) }); - ImGui::PushFont(win.get_large_font()); - ImGui::Text("%s", static_cast(textual_icons::check)); - ImGui::PopFont(); - - ImGui::SetCursorScreenPos({ float(x + 35), float(y + 92 + ImGui::GetTextLineHeight() + 6) }); - ImGui::Text("%s", txt.c_str()); - - if (use_new_calib) - { - ImGui::SameLine(); - - ImGui::PushStyleColor(ImGuiCol_Text, white); - txt = to_string() << " ( -" << std::setprecision(0) << std::fixed << rms_improvement << "%% )"; - ImGui::Text("%s", txt.c_str()); - ImGui::PopStyleColor(); - } - } - } - else - { - ImGui::SetCursorScreenPos({ float(x + 12), float(y + 100) }); - ImGui::Text("%s", "Please compare new vs old calibration\nand decide if to keep or discard the result..."); - } + ImGui::SetCursorScreenPos({ float(x + 12), float(y + 100) }); + ImGui::Text("%s", "Please compare new vs old calibration\nand decide if to keep or discard the result..."); ImGui::SetCursorScreenPos({ float(x + 9), float(y + 60) }); diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 6d35eb2e8b..17542755c2 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -42,9 +42,6 @@ namespace rs2 // Ask the firmware to use one of the before/after calibration tables void apply_calib(bool old); - // Get depth metrics for before/after calibration tables - std::pair get_metric(bool use_new); - void update_last_used(); uint32_t ground_truth = 2500; @@ -61,10 +58,6 @@ namespace rs2 std::vector safe_send_command(const std::vector& cmd, const std::string& name); - rs2::depth_frame fetch_depth_frame(invoker invoke); - - std::pair get_depth_metrics(invoker invoke); - void process_flow(std::function cleanup, invoker invoke) override; float _health = -1.0f; @@ -80,7 +73,6 @@ namespace rs2 std::shared_ptr _sub; std::vector _old_calib, _new_calib; - std::vector> _metrics; device_model& _model; bool _restored = true; From 741d3ec4cd25b3959b31804eb048fd3a96be93a6 Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 20 Jan 2020 05:36:56 -0800 Subject: [PATCH 3/8] Give special treatment to crop resolution used in OCC --- src/types.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/types.h b/src/types.h index fd2af617ee..7f9310427a 100644 --- a/src/types.h +++ b/src/types.h @@ -1808,6 +1808,9 @@ std::vector> subtract_sets(const std::vector Date: Sun, 2 Feb 2020 02:48:49 -0800 Subject: [PATCH 4/8] Revert "Fixing on-chip and tare using USB2 in DQT" and more clean fix --- common/on-chip-calib.cpp | 250 ++++++++++++++++++++++++++++++++++++++- common/on-chip-calib.h | 8 ++ 2 files changed, 256 insertions(+), 2 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index 60aadce91a..a143d90810 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -48,6 +48,37 @@ namespace rs2 catch (...) {} } + // Wait for next depth frame and return it + rs2::depth_frame on_chip_calib_manager::fetch_depth_frame(invoker invoke) + { + auto profiles = _sub->get_selected_profiles(); + bool frame_arrived = false; + rs2::depth_frame res = rs2::frame{}; + while (!frame_arrived) + { + for (auto&& stream : _viewer.streams) + { + 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)) + { + if (auto f = stream.second.texture->get_last_frame(false).as()) + { + frame_arrived = true; + res = f; + } + } + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return res; + } + void on_chip_calib_manager::start_viewer(int w, int h, int fps, invoker invoke) { try @@ -130,6 +161,129 @@ namespace rs2 catch (...) {} } + std::pair on_chip_calib_manager::get_metric(bool use_new) + { + return _metrics[use_new ? 1 : 0]; + } + + std::pair on_chip_calib_manager::get_depth_metrics(invoker invoke) + { + using namespace depth_quality; + + auto f = fetch_depth_frame(invoke); + auto sensor = _sub->s->as(); + auto intr = f.get_profile().as().get_intrinsics(); + rs2::region_of_interest roi { (int)(f.get_width() * 0.45f), (int)(f.get_height() * 0.45f), + (int)(f.get_width() * 0.55f), (int)(f.get_height() * 0.55f) }; + std::vector v; + + std::vector fill_rates; + std::vector rmses; + + auto show_plane = _viewer.draw_plane; + + auto on_frame = [sensor, &fill_rates, &rmses, this]( + const std::vector& points, + const plane p, + const rs2::region_of_interest roi, + const float baseline_mm, + const float focal_length_pixels, + const int ground_thruth_mm, + const bool plane_fit, + const float plane_fit_to_ground_truth_mm, + const float distance_mm, + bool record, + std::vector& samples) + { + float TO_METERS = sensor.get_depth_scale(); + static const float TO_MM = 1000.f; + static const float TO_PERCENT = 100.f; + + // Calculate fill rate relative to the ROI + auto fill_rate = points.size() / float((roi.max_x - roi.min_x)*(roi.max_y - roi.min_y)) * TO_PERCENT; + fill_rates.push_back(fill_rate); + + if (!plane_fit) return; + + std::vector points_set = points; + std::vector distances; + + // Reserve memory for the data + distances.reserve(points.size()); + + // Convert Z values into Depth values by aligning the Fitted plane with the Ground Truth (GT) plane + // Calculate distance and disparity of Z values to the fitted plane. + // Use the rotated plane fit to calculate GT errors + for (auto point : points_set) + { + // Find distance from point to the reconstructed plane + auto dist2plane = p.a*point.x + p.b*point.y + p.c*point.z + p.d; + // Project the point to plane in 3D and find distance to the intersection point + rs2::float3 plane_intersect = { float(point.x - dist2plane*p.a), + float(point.y - dist2plane*p.b), + float(point.z - dist2plane*p.c) }; + + // Store distance, disparity and gt- error + distances.push_back(dist2plane * TO_MM); + } + + // Remove outliers [below 1% and above 99%) + std::sort(points_set.begin(), points_set.end(), [](const rs2::float3& a, const rs2::float3& b) { return a.z < b.z; }); + size_t outliers = points_set.size() / 50; + points_set.erase(points_set.begin(), points_set.begin() + outliers); // crop min 0.5% of the dataset + points_set.resize(points_set.size() - outliers); // crop max 0.5% of the dataset + + // Calculate Plane Fit RMS (Spatial Noise) mm + double plane_fit_err_sqr_sum = std::inner_product(distances.begin(), distances.end(), distances.begin(), 0.); + auto rms_error_val = static_cast(std::sqrt(plane_fit_err_sqr_sum / distances.size())); + auto rms_error_val_per = TO_PERCENT * (rms_error_val / distance_mm); + rmses.push_back(rms_error_val_per); + }; + + auto rms_std = 1000.f; + auto new_rms_std = rms_std; + auto count = 0; + + // Capture metrics on bundles of 31 frame + // Repeat until get "decent" bundle or reach 10 sec + do + { + rms_std = new_rms_std; + + rmses.clear(); + + for (int i = 0; i < 31; i++) + { + f = fetch_depth_frame(invoke); + auto res = depth_quality::analyze_depth_image(f, sensor.get_depth_scale(), sensor.get_stereo_baseline(), + &intr, roi, 0, true, v, false, on_frame); + + _viewer.draw_plane = true; + _viewer.roi_rect = res.plane_corners; + } + + auto rmses_sum_sqr = std::inner_product(rmses.begin(), rmses.end(), rmses.begin(), 0.); + new_rms_std = static_cast(std::sqrt(rmses_sum_sqr / rmses.size())); + } while ((new_rms_std < rms_std * 0.8f && new_rms_std > 10.f) && count++ < 10); + + std::sort(fill_rates.begin(), fill_rates.end()); + std::sort(rmses.begin(), rmses.end()); + + float median_fill_rate, median_rms; + if (fill_rates.empty()) + median_fill_rate = 0; + else + median_fill_rate = fill_rates[fill_rates.size() / 2]; + if (rmses.empty()) + median_rms = 0; + else + median_rms = rmses[rmses.size() / 2]; + + _viewer.draw_plane = show_plane; + + return { median_fill_rate, median_rms }; + } + std::vector on_chip_calib_manager::safe_send_command( const std::vector& cmd, const std::string& name) { @@ -197,6 +351,10 @@ namespace rs2 { start_viewer(0,0,0, invoke); } + + // Capture metrics before + auto metrics_before = get_depth_metrics(invoke); + _metrics.push_back(metrics_before); stop_viewer(invoke); @@ -216,6 +374,10 @@ namespace rs2 // Make new calibration active apply_calib(true); + // Capture metrics after + auto metrics_after = get_depth_metrics(invoke); + _metrics.push_back(metrics_after); + _progress = 100; _done = true; @@ -645,8 +807,92 @@ namespace rs2 } } - ImGui::SetCursorScreenPos({ float(x + 12), float(y + 100) }); - ImGui::Text("%s", "Please compare new vs old calibration\nand decide if to keep or discard the result..."); + auto old_fr = get_manager().get_metric(false).first; + auto new_fr = get_manager().get_metric(true).first; + + auto old_rms = fabs(get_manager().get_metric(false).second); + auto new_rms = fabs(get_manager().get_metric(true).second); + + auto fr_improvement = 100.f * ((new_fr - old_fr) / old_fr); + auto rms_improvement = 100.f * ((old_rms - new_rms) / old_rms); + + std::string old_units = "mm"; + if (old_rms > 10.f) + { + old_rms /= 10.f; + old_units = "cm"; + } + std::string new_units = "mm"; + if (new_rms > 10.f) + { + new_rms /= 10.f; + new_units = "cm"; + } + + // NOTE: Disabling metrics temporarily + // TODO: Re-enable in future release + if (/* fr_improvement > 1.f || rms_improvement > 1.f */ false) + { + std::string txt = to_string() << " Fill-Rate: " << std::setprecision(1) << std::fixed << new_fr << "%%"; + + if (!use_new_calib) + { + txt = to_string() << " Fill-Rate: " << std::setprecision(1) << std::fixed << old_fr << "%%\n"; + } + + ImGui::SetCursorScreenPos({ float(x + 12), float(y + 90) }); + ImGui::PushFont(win.get_large_font()); + ImGui::Text("%s", static_cast(textual_icons::check)); + ImGui::PopFont(); + + ImGui::SetCursorScreenPos({ float(x + 35), float(y + 92) }); + ImGui::Text("%s", txt.c_str()); + + if (use_new_calib) + { + ImGui::SameLine(); + + ImGui::PushStyleColor(ImGuiCol_Text, white); + txt = to_string() << " ( +" << std::fixed << std::setprecision(0) << fr_improvement << "%% )"; + ImGui::Text("%s", txt.c_str()); + ImGui::PopStyleColor(); + } + + if (rms_improvement > 1.f) + { + if (use_new_calib) + { + txt = to_string() << " Noise Estimate: " << std::setprecision(2) << std::fixed << new_rms << new_units; + } + else + { + txt = to_string() << " Noise Estimate: " << std::setprecision(2) << std::fixed << old_rms << old_units; + } + + ImGui::SetCursorScreenPos({ float(x + 12), float(y + 90 + ImGui::GetTextLineHeight() + 6) }); + ImGui::PushFont(win.get_large_font()); + ImGui::Text("%s", static_cast(textual_icons::check)); + ImGui::PopFont(); + + ImGui::SetCursorScreenPos({ float(x + 35), float(y + 92 + ImGui::GetTextLineHeight() + 6) }); + ImGui::Text("%s", txt.c_str()); + + if (use_new_calib) + { + ImGui::SameLine(); + + ImGui::PushStyleColor(ImGuiCol_Text, white); + txt = to_string() << " ( -" << std::setprecision(0) << std::fixed << rms_improvement << "%% )"; + ImGui::Text("%s", txt.c_str()); + ImGui::PopStyleColor(); + } + } + } + else + { + ImGui::SetCursorScreenPos({ float(x + 12), float(y + 100) }); + ImGui::Text("%s", "Please compare new vs old calibration\nand decide if to keep or discard the result..."); + } ImGui::SetCursorScreenPos({ float(x + 9), float(y + 60) }); diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 17542755c2..6d35eb2e8b 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -42,6 +42,9 @@ namespace rs2 // Ask the firmware to use one of the before/after calibration tables void apply_calib(bool old); + // Get depth metrics for before/after calibration tables + std::pair get_metric(bool use_new); + void update_last_used(); uint32_t ground_truth = 2500; @@ -58,6 +61,10 @@ namespace rs2 std::vector safe_send_command(const std::vector& cmd, const std::string& name); + rs2::depth_frame fetch_depth_frame(invoker invoke); + + std::pair get_depth_metrics(invoker invoke); + void process_flow(std::function cleanup, invoker invoke) override; float _health = -1.0f; @@ -73,6 +80,7 @@ namespace rs2 std::shared_ptr _sub; std::vector _old_calib, _new_calib; + std::vector> _metrics; device_model& _model; bool _restored = true; From 3ca3135ba7d61f73cd273492defab1e0a620af36 Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Sun, 2 Feb 2020 07:58:31 -0800 Subject: [PATCH 5/8] Better depth hover text --- common/model-views.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 5966310f15..dfe9037be7 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -2744,7 +2744,7 @@ namespace rs2 float val{}; if (texture->try_pick(x, y, &val)) { - ss << ", *p: 0x" << std::hex << static_cast(round(val)); + ss << " 0x" << std::hex << static_cast(round(val)) << " ="; } if (texture->get_last_frame().is()) @@ -2752,9 +2752,9 @@ namespace rs2 auto meters = texture->get_last_frame().as().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()); From 21d8e4e07e34c62f6d4dd9442171d53b40bbc54e Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 3 Feb 2020 01:22:14 -0800 Subject: [PATCH 6/8] Fix integrity error in tare calib --- src/ds5/ds5-auto-calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 694b86b944..73c9025d20 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -268,7 +268,7 @@ namespace librealsense if (res.size() < sizeof(DirectSearchCalibrationResult)) throw std::runtime_error("Not enough data from CALIB_STATUS!"); - auto result = *reinterpret_cast(res.data()); + result = *reinterpret_cast(res.data()); done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY; } From de606ce5b2bc139f081093727a3f6586132b94da Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 3 Feb 2020 01:48:12 -0800 Subject: [PATCH 7/8] Add apply preset param to OCC and tare --- common/on-chip-calib.cpp | 15 +++++--- common/on-chip-calib.h | 1 + src/ds5/ds5-auto-calibration.cpp | 64 +++++++++++++------------------- 3 files changed, 37 insertions(+), 43 deletions(-) diff --git a/common/on-chip-calib.cpp b/common/on-chip-calib.cpp index a143d90810..85bd429e0c 100644 --- a/common/on-chip-calib.cpp +++ b/common/on-chip-calib.cpp @@ -311,9 +311,10 @@ namespace rs2 { std::stringstream ss; ss << "{\n \"speed\":" << speed << - ",\n \"average_step_count\":" << average_step_count << + ",\n \"average step count\":" << average_step_count << ",\n \"scan parameter\":" << (intrinsic_scan ? 0 : 1) << - ",\n \"step_count\":" << step_count << + ",\n \"step count\":" << step_count << + ",\n \"apply preset\":" << (apply_preset ? 1 : 0) << ",\n \"accuracy\":" << accuracy <<"}"; std::string json = ss.str(); @@ -608,13 +609,17 @@ namespace rs2 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 + 52 + 4 * 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 + 50 + 4 * ImGui::GetTextLineHeightWithSpacing()) }); + ImGui::SetCursorScreenPos({ float(x + 135), float(y + 58 + 5 * ImGui::GetTextLineHeightWithSpacing()) }); } else { @@ -1126,7 +1131,7 @@ namespace rs2 } 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 185; + else if (update_state == RS2_CALIB_STATE_TARE_INPUT_ADVANCED) return 210; else return 100; } diff --git a/common/on-chip-calib.h b/common/on-chip-calib.h index 6d35eb2e8b..f20bf7c8bc 100644 --- a/common/on-chip-calib.h +++ b/common/on-chip-calib.h @@ -54,6 +54,7 @@ namespace rs2 int speed = 3; bool tare = false; bool intrinsic_scan = true; + bool apply_preset = true; void calibrate(); diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 73c9025d20..91f83b1fe5 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -100,27 +100,29 @@ namespace librealsense return values; } + void try_fetch(std::map 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 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); @@ -130,7 +132,7 @@ namespace librealsense param4 param{ (byte)scan_parameter, 0, (byte)data_sampling }; std::shared_ptr 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)); @@ -203,40 +205,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 preset_recover; + if (apply_preset) + preset_recover = change_preset(); auto param2 = (int)ground_truth_mm * 100; From 5fb13ec0dd90ae95788f054b8b511690d10ac293 Mon Sep 17 00:00:00 2001 From: "Dorodnicov, Sergey" Date: Mon, 3 Feb 2020 01:58:58 -0800 Subject: [PATCH 8/8] Moving some code around --- src/ds5/ds5-auto-calibration.cpp | 48 ++++++++++++++++++++++++++++++-- src/ds5/ds5-auto-calibration.h | 45 +----------------------------- src/ds5/ds5-device.cpp | 10 +++---- 3 files changed, 52 insertions(+), 51 deletions(-) diff --git a/src/ds5/ds5-auto-calibration.cpp b/src/ds5/ds5-auto-calibration.cpp index 91f83b1fe5..569ab32d1a 100644 --- a/src/ds5/ds5-auto-calibration.cpp +++ b/src/ds5/ds5-auto-calibration.cpp @@ -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, @@ -82,7 +126,7 @@ namespace librealsense const int DEFAULT_SAMPLING = data_sampling::polling; auto_calibrated::auto_calibrated(std::shared_ptr& hwm) - :_hw_monitor(hwm){} + : _hw_monitor(hwm){} std::map auto_calibrated::parse_json(std::string json_content) { @@ -341,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) { diff --git a/src/ds5/ds5-auto-calibration.h b/src/ds5/ds5-auto-calibration.h index ab6413a385..3002278d9a 100644 --- a/src/ds5/ds5-auto-calibration.h +++ b/src/ds5/ds5-auto-calibration.h @@ -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& hwm); void write_calibration() const override; @@ -64,7 +21,7 @@ namespace librealsense private: std::vector 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 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; diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 75bc81d5a0..3eafab6200 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -568,11 +568,11 @@ namespace librealsense ds5_device::ds5_device(std::shared_ptr ctx, const platform::backend_device_group& group) : device(ctx, group), global_time_interface(), - auto_calibrated(_hw_monitor), - _device_capabilities(ds::d400_caps::CAP_UNDEFINED), - _depth_stream(new stream(RS2_STREAM_DEPTH)), - _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), - _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)) + auto_calibrated(_hw_monitor), + _device_capabilities(ds::d400_caps::CAP_UNDEFINED), + _depth_stream(new stream(RS2_STREAM_DEPTH)), + _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), + _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)) { _depth_device_idx = add_sensor(create_depth_device(ctx, group.uvc_devices)); init(ctx, group);