rs-depth-quality.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #include <numeric>
5 #include <librealsense2/rs.hpp>
6 #include "depth-quality-model.h"
7 
8 #ifdef BUILD_EASYLOGGINGPP
9 #include "easylogging++.h"
10 #ifdef BUILD_SHARED_LIBS
11 // With static linkage, ELPP is initialized by librealsense, so doing it here will
12 // create errors. When we're using the shared .so/.dll, the two are separate and we have
13 // to initialize ours if we want to use the APIs!
15 #endif
16 #endif
17 
18 int main(int argc, const char * argv[]) try
19 {
21  rs2::ux_window window("Depth Quality Tool", ctx);
23 
24  using namespace rs2::depth_quality;
25 
26  // ===============================
27  // Metrics Definitions
28  // ===============================
29  metric fill = model.make_metric(
30  "Fill-Rate", 0, 100, false, "%",
31  "Fill Rate.\n"
32  "Percentage of pixels with valid depth\n"
33  "values out of all pixels within the ROI\n");
34 
35  metric z_accuracy = model.make_metric(
36  "Z Accuracy", -10, 10, true, "%",
37  "Z-Accuracy given Ground Truth (GT)\n"
38  " as percentage of the range.\n"
39  "Algorithm:\n"
40  "1. Transpose Z values from the Fitted to the GT plane\n"
41  "2. Calculate depth errors:\n"
42  " err= signed(Transposed Z - GT).\n"
43  "3. Retrieve the median of the depth errors:\n"
44  "4. Interpret results:\n"
45  " - Positive value indicates that the Plane Fit\n"
46  "is further than the Ground Truth (overshot)\n"
47  " - Negative value indicates the Plane Fit\n"
48  "is in front of Ground Truth (undershot)\n");
49 
50  metric plane_fit_rms_error = model.make_metric(
51  "Plane Fit RMS Error", 0.f, 5.f, true, "%",
52  "Plane Fit RMS Error .\n"
53  "This metric provides RMS of Z-Error (Spatial Noise)\n"
54  "and is calculated as follows:\n"
55  "Zi - depth range of i-th pixel (mm)\n"
56  "Zpi -depth of Zi projection onto plane fit (mm)\n"
57  " n \n"
58  "RMS = SQRT((SUM(Zi-Zpi)^2)/n)\n"
59  " i=1 ");
60 
61  metric sub_pixel_rms_error = model.make_metric(
62  "Subpixel RMS Error", 0.f, 1.f, true, "pixel",
63  "Subpixel RMS Error .\n"
64  "This metric provides the subpixel accuracy\n"
65  "and is calculated as follows:\n"
66  "Zi - depth range of i-th pixel (mm)\n"
67  "Zpi -depth of Zi projection onto plane fit (mm)\n"
68  "BL - optical baseline (mm)\n"
69  "FL - focal length, as a multiple of pixel width\n"
70  "Di = BL*FL/Zi; Dpi = Bl*FL/Zpi\n"
71  " n \n"
72  "RMS = SQRT((SUM(Di-Dpi)^2)/n)\n"
73  " i=1 ");
74 
75  // ===============================
76  // Metrics Calculation
77  // ===============================
78 
79  model.on_frame([&](
80  const std::vector<rs2::float3>& points,
81  const rs2::plane p,
82  const rs2::region_of_interest roi,
83  const float baseline_mm,
84  const float focal_length_pixels,
85  const int ground_truth_mm,
86  const bool plane_fit,
87  const float plane_fit_to_ground_truth_mm,
88  const float distance_mm,
89  bool record,
90  std::vector<single_metric_data>& samples)
91  {
92  float TO_METERS = model.get_depth_scale();
93  static const float TO_MM = 1000.f;
94  static const float TO_PERCENT = 100.f;
95 
96  // Calculate fill rate relative to the ROI
97  auto fill_rate = points.size() / float((roi.max_x - roi.min_x)*(roi.max_y - roi.min_y)) * TO_PERCENT;
98  fill->add_value(fill_rate);
99  if(record) samples.push_back({fill->get_name(), fill_rate });
100 
101  if (!plane_fit) return;
102 
103  const float bf_factor = baseline_mm * focal_length_pixels * TO_METERS; // also convert point units from mm to meter
104 
105  std::vector<rs2::float3> points_set = points;
106  std::vector<float> distances;
107  std::vector<float> disparities;
108  std::vector<float> gt_errors;
109 
110  // Reserve memory for the data
111  distances.reserve(points.size());
112  disparities.reserve(points.size());
113  if (ground_truth_mm) gt_errors.reserve(points.size());
114 
115  // Remove outliers [below 0.5% and above 99.5%)
116  std::sort(points_set.begin(), points_set.end(), [](const rs2::float3& a, const rs2::float3& b) { return a.z < b.z; });
117  size_t outliers = points_set.size() / 200;
118  points_set.erase(points_set.begin(), points_set.begin() + outliers); // crop min 0.5% of the dataset
119  points_set.resize(points_set.size() - outliers); // crop max 0.5% of the dataset
120 
121  // Convert Z values into Depth values by aligning the Fitted plane with the Ground Truth (GT) plane
122  // Calculate distance and disparity of Z values to the fitted plane.
123  // Use the rotated plane fit to calculate GT errors
124  for (auto point : points_set)
125  {
126  // Find distance from point to the reconstructed plane
127  auto dist2plane = p.a*point.x + p.b*point.y + p.c*point.z + p.d;
128  // Project the point to plane in 3D and find distance to the intersection point
129  rs2::float3 plane_intersect = { float(point.x - dist2plane*p.a),
130  float(point.y - dist2plane*p.b),
131  float(point.z - dist2plane*p.c) };
132 
133  // Store distance, disparity and gt- error
134  distances.push_back(dist2plane * TO_MM);
135  disparities.push_back(bf_factor / point.length() - bf_factor / plane_intersect.length());
136  // The negative dist2plane represents a point closer to the camera than the fitted plane
137  if (ground_truth_mm) gt_errors.push_back(plane_fit_to_ground_truth_mm + (dist2plane * TO_MM));
138  }
139 
140  // Show Z accuracy metric only when Ground Truth is available
141  z_accuracy->enable(ground_truth_mm > 0);
142  if (ground_truth_mm)
143  {
144  std::sort(begin(gt_errors), end(gt_errors));
145  auto gt_median = gt_errors[gt_errors.size() / 2];
146  auto accuracy = TO_PERCENT * (gt_median / ground_truth_mm);
147  z_accuracy->add_value(accuracy);
148  if (record) samples.push_back({ z_accuracy->get_name(), accuracy });
149  }
150 
151  // Calculate Sub-pixel RMS for Stereo-based Depth sensors
152  double total_sq_disparity_diff = 0;
153  for (auto disparity : disparities)
154  {
155  total_sq_disparity_diff += disparity*disparity;
156  }
157  auto rms_subpixel_val = static_cast<float>(std::sqrt(total_sq_disparity_diff / disparities.size()));
158  sub_pixel_rms_error->add_value(rms_subpixel_val);
159  if (record) samples.push_back({ sub_pixel_rms_error->get_name(), rms_subpixel_val });
160 
161  // Calculate Plane Fit RMS (Spatial Noise) mm
162  double plane_fit_err_sqr_sum = std::inner_product(distances.begin(), distances.end(), distances.begin(), 0.);
163  auto rms_error_val = static_cast<float>(std::sqrt(plane_fit_err_sqr_sum / distances.size()));
164  auto rms_error_val_per = TO_PERCENT * (rms_error_val / distance_mm);
165  plane_fit_rms_error->add_value(rms_error_val_per);
166  if (record)
167  {
168  samples.push_back({ plane_fit_rms_error->get_name(), rms_error_val_per });
169  samples.push_back({ plane_fit_rms_error->get_name() + " mm", rms_error_val });
170  }
171 
172  });
173 
174  // ===============================
175  // Rendering Loop
176  // ===============================
177 
178  window.on_load = [&]()
179  {
180  return model.start(window);
181  };
182 
183  while(window)
184  {
185  model.render(window);
186  }
187 
188  return EXIT_SUCCESS;
189 }
190 catch (const rs2::error& e)
191 {
192  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << std::endl;
193  return EXIT_FAILURE;
194 }
195 catch (const std::exception& e)
196 {
197  std::cerr << e.what() << std::endl;
198  return EXIT_FAILURE;
199 }
GLuint GLuint end
GLboolean GLboolean GLboolean b
void on_frame(callback_type callback)
GLfloat GLfloat p
Definition: glext.h:12687
std::function< bool()> on_load
Definition: ux-window.h:36
static GLFWwindow * window
Definition: joysticks.c:55
e
Definition: rmse.py:177
point
Definition: rmse.py:166
float a
Definition: rendering.h:129
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
GLboolean GLboolean GLboolean GLboolean a
const std::string & get_failed_args() const
Definition: rs_types.hpp:117
not_this_one begin(...)
GLdouble f
#define INITIALIZE_EASYLOGGINGPP
int main(int argc, const char *argv[])
float c
Definition: rendering.h:131
std::shared_ptr< metric_plot > metric
std::shared_ptr< metric_plot > make_metric(const std::string &name, float min, float max, bool plane_fit, const std::string &units, const std::string &description)
float disparity
Definition: t265_stereo.py:241
float length() const
Definition: rendering.h:140
GLsizei samples
std::ostream & cerr()
float d
Definition: rendering.h:132
float b
Definition: rendering.h:130
const std::string & get_failed_function() const
Definition: rs_types.hpp:112
GLdouble GLdouble GLint GLint const GLdouble * points


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:40