depth-metrics.h
Go to the documentation of this file.
3 //
4 // Plane Fit implementation follows http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points algorithm
5
6 #pragma once
7 #include <vector>
8 #include <mutex>
9 #include <array>
10 #include <imgui.h>
11 #include <librealsense2/rsutil.h>
12 #include <librealsense2/rs.hpp>
13 #include "rendering.h"
14
15 namespace rs2
16 {
17  namespace depth_quality
18  {
20  {
21  int width;
22  int height;
23
25
26  float distance;
27  float angle;
28  float angle_x;
29  float angle_y;
30
32  std::array<float3, 4> plane_corners;
33  };
34
36  {
38  val(val), name(name) {}
39
40  float val;
42  };
43
44  using callback_type = std::function<void(
45  const std::vector<rs2::float3>& points,
46  const plane p,
48  const float baseline_mm,
49  const float focal_length_pixels,
50  const int ground_thruth_mm,
51  const bool plane_fit,
52  const float plane_fit_to_ground_truth_mm,
53  const float distance_mm,
54  bool record,
55  std::vector<single_metric_data>& samples)>;
56
58  {
59  return{ normal.x, normal.y, normal.z, -(normal.x*point.x + normal.y*point.y + normal.z*point.z) };
60  }
61
62  //Based on: http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
63  inline plane plane_from_points(const std::vector<rs2::float3> points)
64  {
65  if (points.size() < 3) throw std::runtime_error("Not enough points to calculate plane");
66
67  rs2::float3 sum = { 0,0,0 };
68  for (auto point : points) sum = sum + point;
69
70  rs2::float3 centroid = sum / float(points.size());
71
72  double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
73  for (auto point : points) {
74  rs2::float3 temp = point - centroid;
75  xx += temp.x * temp.x;
76  xy += temp.x * temp.y;
77  xz += temp.x * temp.z;
78  yy += temp.y * temp.y;
79  yz += temp.y * temp.z;
80  zz += temp.z * temp.z;
81  }
82
83  double det_x = yy*zz - yz*yz;
84  double det_y = xx*zz - xz*xz;
85  double det_z = xx*yy - xy*xy;
86
87  double det_max = std::max({ det_x, det_y, det_z });
88  if (det_max <= 0) return{ 0, 0, 0, 0 };
89
90  rs2::float3 dir{};
91  if (det_max == det_x)
92  {
93  float a = static_cast<float>((xz*yz - xy*zz) / det_x);
94  float b = static_cast<float>((xy*yz - xz*yy) / det_x);
95  dir = { 1, a, b };
96  }
97  else if (det_max == det_y)
98  {
99  float a = static_cast<float>((yz*xz - xy*zz) / det_y);
100  float b = static_cast<float>((xy*xz - yz*xx) / det_y);
101  dir = { a, 1, b };
102  }
103  else
104  {
105  float a = static_cast<float>((yz*xy - xz*yy) / det_z);
106  float b = static_cast<float>((xz*xy - yz*xx) / det_z);
107  dir = { a, b, 1 };
108  }
109
110  return plane_from_point_and_normal(centroid, dir.normalize());
111  }
112
113  inline double evaluate_pixel(const plane& p, const rs2_intrinsics* intrin, float x, float y, float distance, float3& output)
114  {
115  float pixel[2] = { x, y };
116  rs2_deproject_pixel_to_point(&output.x, intrin, pixel, distance);
117  return evaluate_plane(p, output);
118  }
119
120  inline float3 approximate_intersection(const plane& p, const rs2_intrinsics* intrin, float x, float y, float min, float max)
121  {
122  float3 point;
123  auto f = evaluate_pixel(p, intrin, x, y, max, point);
124  if (fabs(max - min) < 1e-3) return point;
125  auto n = evaluate_pixel(p, intrin, x, y, min, point);
126  if (f*n > 0) return{ 0, 0, 0 };
127
128  auto avg = (max + min) / 2;
129  auto mid = evaluate_pixel(p, intrin, x, y, avg, point);
130  if (mid*n < 0) return approximate_intersection(p, intrin, x, y, min, avg);
131  return approximate_intersection(p, intrin, x, y, avg, max);
132  }
133
134  inline float3 approximate_intersection(const plane& p, const rs2_intrinsics* intrin, float x, float y)
135  {
136  return approximate_intersection(p, intrin, x, y, 0.f, 1000.f);
137  }
138
140  const rs2::video_frame& frame,
141  float units, float baseline_mm,
142  const rs2_intrinsics * intrin,
144  const int ground_truth_mm,
145  bool plane_fit_present,
146  std::vector<single_metric_data>& samples,
147  bool record,
149  {
150  auto pixels = (const uint16_t*)frame.get_data();
151  const auto w = frame.get_width();
152  const auto h = frame.get_height();
153
154  snapshot_metrics result{ w, h, roi, {} };
155
156  std::mutex m;
157
158  std::vector<rs2::float3> roi_pixels;
159
160 //#pragma omp parallel for - TODO optimization envisaged
161  for (int y = roi.min_y; y < roi.max_y; ++y)
162  for (int x = roi.min_x; x < roi.max_x; ++x)
163  {
164  auto depth_raw = pixels[y*w + x];
165
166  if (depth_raw)
167  {
168  // units is float
169  float pixel[2] = { float(x), float(y) };
170  float point[3];
171  auto distance = depth_raw * units;
172
173  rs2_deproject_pixel_to_point(point, intrin, pixel, distance);
174
175  std::lock_guard<std::mutex> lock(m);
176  roi_pixels.push_back({ point[0], point[1], point[2] });
177  }
178  }
179
180  if (roi_pixels.size() < 3) { // Not enough pixels in RoI to fit a plane
181  return result;
182  }
183
184  plane p = plane_from_points(roi_pixels);
185
186  if (p == plane{ 0, 0, 0, 0 }) { // The points in RoI don't span a valid plane
187  return result;
188  }
189
190  // Calculate intersection of the plane fit with a ray along the center of ROI
191  // that by design coincides with the center of the frame
192  float3 plane_fit_pivot = approximate_intersection(p, intrin, intrin->width / 2.f, intrin->height / 2.f);
193  float plane_fit_to_gt_offset_mm = (ground_truth_mm > 0.f) ? (plane_fit_pivot.z * 1000 - ground_truth_mm) : 0;
194
195  result.p = p;
196  result.plane_corners[0] = approximate_intersection(p, intrin, float(roi.min_x), float(roi.min_y));
197  result.plane_corners[1] = approximate_intersection(p, intrin, float(roi.max_x), float(roi.min_y));
198  result.plane_corners[2] = approximate_intersection(p, intrin, float(roi.max_x), float(roi.max_y));
199  result.plane_corners[3] = approximate_intersection(p, intrin, float(roi.min_x), float(roi.max_y));
200
201  // Distance of origin (the camera) from the plane is encoded in parameter D of the plane
202  // The parameter represents the euclidian distance (along plane normal) from camera to the plane
203  result.distance = static_cast<float>(-p.d * 1000);
204  // Angle can be calculated from param C
205  result.angle = static_cast<float>(std::acos(std::abs(p.c)) / M_PI * 180.);
206
207  callback(roi_pixels, p, roi, baseline_mm, intrin->fx, ground_truth_mm, plane_fit_present,
208  plane_fit_to_gt_offset_mm, result.distance, record, samples);
209
210  // Calculate normal
211  auto n = float3{ p.a, p.b, p.c };
212  auto cam = float3{ 0.f, 0.f, -1.f };
213  auto dot = n * cam;
214  auto u = cam - n * dot;
215
216  result.angle_x = u.x;
217  result.angle_y = u.y;
218
219  return result;
220  }
221  }
222 }
static const textual_icon lock
Definition: model-views.h:218
GLboolean GLboolean GLboolean b
GLint y
snapshot_metrics analyze_depth_image(const rs2::video_frame &frame, float units, float baseline_mm, const rs2_intrinsics *intrin, rs2::region_of_interest roi, const int ground_truth_mm, bool plane_fit_present, std::vector< single_metric_data > &samples, bool record, callback_type callback)
plane plane_from_points(const std::vector< rs2::float3 > points)
Definition: depth-metrics.h:63
typedef void(APIENTRY *GLDEBUGPROC)(GLenum source
GLuint const GLchar * name
double evaluate_pixel(const plane &p, const rs2_intrinsics *intrin, float x, float y, float distance, float3 &output)
GLfloat GLfloat p
Definition: glext.h:12687
const GLfloat * m
Definition: glext.h:6814
float3 approximate_intersection(const plane &p, const rs2_intrinsics *intrin, float x, float y, float min, float max)
rs2_intrinsics intrin
const void * get_data() const
Definition: rs_frame.hpp:545
unsigned short uint16_t
Definition: stdint.h:79
Definition: cah-model.h:10
GLdouble GLdouble GLdouble w
GLsizei const GLchar *const * string
std::array< float3, 4 > plane_corners
Definition: depth-metrics.h:32
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
GLdouble n
Definition: glext.h:1966
normal
Definition: rmse.py:164
float evaluate_plane(const plane &plane, const float3 &point)
Definition: rendering.h:158
e
Definition: rmse.py:177
GLsizei GLsizei GLfloat distance
Definition: glext.h:10563
point
Definition: rmse.py:166
float a
Definition: rendering.h:129
GLboolean GLboolean GLboolean GLboolean a
GLuint GLfloat * val
GLdouble f
int xx
Definition: rmse.py:52
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics *intrin, const float pixel[2], float depth)
Definition: rsutil.h:83
int yy
Definition: rmse.py:53
GLdouble x
float dot(const rs2::float2 &a, const rs2::float2 &b)
Definition: rendering.h:200
def callback(frame)
Definition: t265_stereo.py:91
float c
Definition: rendering.h:131
GLint GLint GLsizei GLint GLenum GLenum const void * pixels
std::function< void(const std::vector< rs2::float3 > &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< single_metric_data > &samples)> callback_type
Definition: depth-metrics.h:55
GLfloat units
plane plane_from_point_and_normal(const rs2::float3 &point, const rs2::float3 &normal)
Definition: depth-metrics.h:57
int get_height() const
Definition: rs_frame.hpp:671
int min(int a, int b)
Definition: lz4s.c:73
GLsizei samples
single_metric_data(std::string name, float val)
Definition: depth-metrics.h:37
Video stream intrinsics.
Definition: rs_types.h:58
float d
Definition: rendering.h:132
float b
Definition: rendering.h:130
GLuint64EXT * result
Definition: glext.h:10921
int get_width() const
Definition: rs_frame.hpp:659

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