example.hpp
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 #pragma once
5 
6 #include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
7 
8 #define GL_SILENCE_DEPRECATION
9 #define GLFW_INCLUDE_GLU
10 #include <GLFW/glfw3.h>
11 
12 #include <string>
13 #include <sstream>
14 #include <iostream>
15 #include <algorithm>
16 #include <iomanip>
17 #include <cmath>
18 #include <map>
19 #include <functional>
20 
21 #include "../third-party/stb_easy_font.h"
22 
23 #ifndef PI
24 #define PI 3.14159265358979323846
25 #define PI_FL 3.141592f
26 #endif
27 const float IMU_FRAME_WIDTH = 1280.f;
28 const float IMU_FRAME_HEIGHT = 720.f;
29 enum class Priority { high = 0, medium = -1, low = -2 };
30 
32 // Basic Data Types //
34 
35 struct float3 {
36  float x, y, z;
38  {
39  return { x * t, y * t, z * t };
40  }
41 
43  {
44  return { x - t, y - t, z - t };
45  }
46 
47  void operator*=(float t)
48  {
49  x = x * t;
50  y = y * t;
51  z = z * t;
52  }
53 
54  void operator=(float3 other)
55  {
56  x = other.x;
57  y = other.y;
58  z = other.z;
59  }
60 
61  void add(float t1, float t2, float t3)
62  {
63  x += t1;
64  y += t2;
65  z += t3;
66  }
67 };
68 struct float2 { float x, y; };
69 
70 struct rect
71 {
72  float x, y;
73  float w, h;
74 
75  // Create new rect within original boundaries with give aspect ration
77  {
78  auto H = static_cast<float>(h), W = static_cast<float>(h) * size.x / size.y;
79  if (W > w)
80  {
81  auto scale = w / W;
82  W *= scale;
83  H *= scale;
84  }
85 
86  return{ x + (w - W) / 2, y + (h - H) / 2, W, H };
87  }
88 };
89 
91 {
92 
93  unsigned int x, y; //location of tile in the grid
94  unsigned int w, h; //width and height by number of tiles
95  Priority priority; //when should the tile be drawn?: high priority is on top of all, medium is a layer under top layer, low is a layer under medium layer
96 
97 };
98 
99 //name aliasing the map of pairs<frame, tile_properties>
100 using frame_and_tile_property = std::pair<rs2::frame, tile_properties>;
101 using frames_mosaic = std::map<int, frame_and_tile_property>;
102 
103 
105 // Simple font loading code //
107 
108 
109 inline void draw_text(int x, int y, const char* text)
110 {
111  std::vector<char> buffer;
113  glVertexPointer(2, GL_FLOAT, 16, &buffer);
114  glDrawArrays(GL_QUADS, 0, 4 * stb_easy_font_print((float)x, (float)(y - 7), (char*)text, nullptr, &buffer, sizeof(buffer)));
116 }
117 
118 void set_viewport(const rect& r)
119 {
120  glViewport((int)r.x, (int)r.y, (int)r.w, (int)r.h);
121  glLoadIdentity();
123  glOrtho(0, r.w, r.h, 0, -1, +1);
124 }
125 
127 {
128 public:
129  void render(const rs2::motion_frame& frame, const rect& r)
130  {
131  draw_motion(frame, r.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
132  }
133 
134  GLuint get_gl_handle() { return _gl_handle; }
135 
136 private:
137  GLuint _gl_handle = 0;
138 
139  void draw_motion(const rs2::motion_frame& f, const rect& r)
140  {
141  if (!_gl_handle)
142  glGenTextures(1, &_gl_handle);
143 
144  set_viewport(r);
145  draw_text(int(0.05f * r.w), int(0.05f * r.h), f.get_profile().stream_name().c_str());
146 
147  auto md = f.get_motion_data();
148  auto x = md.x;
149  auto y = md.y;
150  auto z = md.z;
151 
153  glPushMatrix();
154  glLoadIdentity();
155 
156  glOrtho(-2.8, 2.8, -2.4, 2.4, -7, 7);
157 
158  glRotatef(25, 1.0f, 0.0f, 0.0f);
159 
160  glTranslatef(0, -0.33f, -1.f);
161 
162  glRotatef(-135, 0.0f, 1.0f, 0.0f);
163 
164  glRotatef(180, 0.0f, 0.0f, 1.0f);
165  glRotatef(-90, 0.0f, 1.0f, 0.0f);
166 
167  draw_axes(1, 2);
168 
169  draw_circle(1, 0, 0, 0, 1, 0);
170  draw_circle(0, 1, 0, 0, 0, 1);
171  draw_circle(1, 0, 0, 0, 0, 1);
172 
173  const auto canvas_size = 230;
174  const auto vec_threshold = 0.01f;
175  float norm = std::sqrt(x * x + y * y + z * z);
176  if (norm < vec_threshold)
177  {
178  const auto radius = 0.05;
179  static const int circle_points = 100;
180  static const float angle = 2.0f * 3.1416f / circle_points;
181 
182  glColor3f(1.0f, 1.0f, 1.0f);
184  double angle1 = 0.0;
185  glVertex2d(radius * cos(0.0), radius * sin(0.0));
186  int i;
187  for (i = 0; i < circle_points; i++)
188  {
189  glVertex2d(radius * cos(angle1), radius * sin(angle1));
190  angle1 += angle;
191  }
192  glEnd();
193  }
194  else
195  {
196  auto vectorWidth = 3.f;
197  glLineWidth(vectorWidth);
198  glBegin(GL_LINES);
199  glColor3f(1.0f, 1.0f, 1.0f);
200  glVertex3f(0.0f, 0.0f, 0.0f);
201  glVertex3f(x / norm, y / norm, z / norm);
202  glEnd();
203 
204  // Save model and projection matrix for later
205  GLfloat model[16];
207  GLfloat proj[16];
209 
210  glLoadIdentity();
211  glOrtho(-canvas_size, canvas_size, -canvas_size, canvas_size, -1, +1);
212 
213  std::ostringstream s1;
214  const auto precision = 3;
215 
216  glRotatef(180, 1.0f, 0.0f, 0.0f);
217 
218  s1 << "(" << std::fixed << std::setprecision(precision) << x << "," << std::fixed << std::setprecision(precision) << y << "," << std::fixed << std::setprecision(precision) << z << ")";
219  print_text_in_3d(x, y, z, s1.str().c_str(), false, model, proj, 1 / norm);
220 
221  std::ostringstream s2;
222  s2 << std::setprecision(precision) << norm;
223  print_text_in_3d(x / 2, y / 2, z / 2, s2.str().c_str(), true, model, proj, 1 / norm);
224  }
226  glPopMatrix();
227  }
228 
229  //IMU drawing helper functions
231  {
232  const auto N = 4;
233  for (int i = 0; i < N; i++)
234  {
235  result[i] = 0;
236  for (int j = 0; j < N; j++)
237  {
238  result[i] += vec[j] * mat[N * j + i];
239  }
240  }
241  return;
242  }
243 
244  float2 xyz_to_xy(float x, float y, float z, GLfloat model[], GLfloat proj[], float vec_norm)
245  {
246  GLfloat vec[4] = { x, y, z, 0 };
247  float tmp_result[4];
248  float result[4];
249 
250  const auto canvas_size = 230;
251 
252  multiply_vector_by_matrix(vec, model, tmp_result);
253  multiply_vector_by_matrix(tmp_result, proj, result);
254 
255  return{ canvas_size * vec_norm * result[0], canvas_size * vec_norm * result[1] };
256  }
257 
258  void print_text_in_3d(float x, float y, float z, const char* text, bool center_text, GLfloat model[], GLfloat proj[], float vec_norm)
259  {
260  auto xy = xyz_to_xy(x, y, z, model, proj, vec_norm);
261  auto w = (center_text) ? stb_easy_font_width((char*)text) : 0;
262  glColor3f(1.0f, 1.0f, 1.0f);
263  draw_text((int)(xy.x - w / 2), (int)xy.y, text);
264  }
265 
266  static void draw_axes(float axis_size = 1.f, float axisWidth = 4.f)
267  {
268  // Triangles For X axis
270  glColor3f(1.0f, 0.0f, 0.0f);
271  glVertex3f(axis_size * 1.1f, 0.f, 0.f);
272  glVertex3f(axis_size, -axis_size * 0.05f, 0.f);
273  glVertex3f(axis_size, axis_size * 0.05f, 0.f);
274  glVertex3f(axis_size * 1.1f, 0.f, 0.f);
275  glVertex3f(axis_size, 0.f, -axis_size * 0.05f);
276  glVertex3f(axis_size, 0.f, axis_size * 0.05f);
277  glEnd();
278 
279  // Triangles For Y axis
281  glColor3f(0.f, 1.f, 0.f);
282  glVertex3f(0.f, axis_size * 1.1f, 0.0f);
283  glVertex3f(0.f, axis_size, 0.05f * axis_size);
284  glVertex3f(0.f, axis_size, -0.05f * axis_size);
285  glVertex3f(0.f, axis_size * 1.1f, 0.0f);
286  glVertex3f(0.05f * axis_size, axis_size, 0.f);
287  glVertex3f(-0.05f * axis_size, axis_size, 0.f);
288  glEnd();
289 
290  // Triangles For Z axis
292  glColor3f(0.0f, 0.0f, 1.0f);
293  glVertex3f(0.0f, 0.0f, 1.1f * axis_size);
294  glVertex3f(0.0f, 0.05f * axis_size, 1.0f * axis_size);
295  glVertex3f(0.0f, -0.05f * axis_size, 1.0f * axis_size);
296  glVertex3f(0.0f, 0.0f, 1.1f * axis_size);
297  glVertex3f(0.05f * axis_size, 0.f, 1.0f * axis_size);
298  glVertex3f(-0.05f * axis_size, 0.f, 1.0f * axis_size);
299  glEnd();
300 
301  glLineWidth(axisWidth);
302 
303  // Drawing Axis
304  glBegin(GL_LINES);
305  // X axis - Red
306  glColor3f(1.0f, 0.0f, 0.0f);
307  glVertex3f(0.0f, 0.0f, 0.0f);
308  glVertex3f(axis_size, 0.0f, 0.0f);
309 
310  // Y axis - Green
311  glColor3f(0.0f, 1.0f, 0.0f);
312  glVertex3f(0.0f, 0.0f, 0.0f);
313  glVertex3f(0.0f, axis_size, 0.0f);
314 
315  // Z axis - Blue
316  glColor3f(0.0f, 0.0f, 1.0f);
317  glVertex3f(0.0f, 0.0f, 0.0f);
318  glVertex3f(0.0f, 0.0f, axis_size);
319  glEnd();
320  }
321 
322  // intensity is grey intensity
323  static void draw_circle(float xx, float xy, float xz, float yx, float yy, float yz, float radius = 1.1, float3 center = { 0.0, 0.0, 0.0 }, float intensity = 0.5f)
324  {
325  const auto N = 50;
326  glColor3f(intensity, intensity, intensity);
327  glLineWidth(2);
329 
330  for (int i = 0; i <= N; i++)
331  {
332  const double theta = (2 * PI / N) * i;
333  const auto cost = static_cast<float>(cos(theta));
334  const auto sint = static_cast<float>(sin(theta));
335  glVertex3f(
336  center.x + radius * (xx * cost + yx * sint),
337  center.y + radius * (xy * cost + yy * sint),
338  center.z + radius * (xz * cost + yz * sint)
339  );
340  }
341  glEnd();
342  }
343 
344 };
345 
347 {
348 public:
349  void render(const rs2::pose_frame& frame, const rect& r)
350  {
351  draw_pose(frame, r.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
352  }
353 
354  GLuint get_gl_handle() { return _gl_handle; }
355 
356 private:
357  mutable GLuint _gl_handle = 0;
358 
359  // Provide textual representation only
360  void draw_pose(const rs2::pose_frame& f, const rect& r)
361  {
362  if (!_gl_handle)
363  glGenTextures(1, &_gl_handle);
364 
365  set_viewport(r);
366  std::string caption(f.get_profile().stream_name());
367  if (f.get_profile().stream_index())
368  caption += std::to_string(f.get_profile().stream_index());
369  draw_text(int(0.05f * r.w), int(0.05f * r.h), caption.c_str());
370 
371  auto pose = f.get_pose_data();
372  std::stringstream ss;
373  ss << "Pos (meter): \t\t" << std::fixed << std::setprecision(2) << pose.translation.x << ", " << pose.translation.y << ", " << pose.translation.z;
374  draw_text(int(0.05f * r.w), int(0.2f * r.h), ss.str().c_str());
375  ss.clear(); ss.str("");
376  ss << "Orient (quaternion): \t" << pose.rotation.x << ", " << pose.rotation.y << ", " << pose.rotation.z << ", " << pose.rotation.w;
377  draw_text(int(0.05f * r.w), int(0.3f * r.h), ss.str().c_str());
378  ss.clear(); ss.str("");
379  ss << "Lin Velocity (m/sec): \t" << pose.velocity.x << ", " << pose.velocity.y << ", " << pose.velocity.z;
380  draw_text(int(0.05f * r.w), int(0.4f * r.h), ss.str().c_str());
381  ss.clear(); ss.str("");
382  ss << "Ang. Velocity (rad/sec): \t" << pose.angular_velocity.x << ", " << pose.angular_velocity.y << ", " << pose.angular_velocity.z;
383  draw_text(int(0.05f * r.w), int(0.5f * r.h), ss.str().c_str());
384  }
385 };
386 
389 {
390  // Provide textual representation only
391  void put_text(const std::string& msg, float norm_x_pos, float norm_y_pos, const rect& r)
392  {
393  set_viewport(r);
394  draw_text(int(norm_x_pos * r.w), int(norm_y_pos * r.h), msg.c_str());
395  }
396 };
397 
399 // Image display code //
402 class texture
403 {
404 public:
405 
406  void upload(const rs2::video_frame& frame)
407  {
408  if (!frame) return;
409 
410  if (!_gl_handle)
411  glGenTextures(1, &_gl_handle);
412  GLenum err = glGetError();
413 
414  auto format = frame.get_profile().format();
415  auto width = frame.get_width();
416  auto height = frame.get_height();
417  _stream_type = frame.get_profile().stream_type();
418  _stream_index = frame.get_profile().stream_index();
419 
420  glBindTexture(GL_TEXTURE_2D, _gl_handle);
421 
422  switch (format)
423  {
424  case RS2_FORMAT_RGB8:
426  break;
427  case RS2_FORMAT_RGBA8:
429  break;
430  case RS2_FORMAT_Y8:
432  break;
433  case RS2_FORMAT_Y10BPACK:
435  break;
436  default:
437  throw std::runtime_error("The requested format is not supported by this demo!");
438  }
439 
446  }
447 
448  void show(const rect& r, float alpha = 1.f) const
449  {
450  if (!_gl_handle)
451  return;
452 
453  set_viewport(r);
454 
455  glBindTexture(GL_TEXTURE_2D, _gl_handle);
456  glColor4f(1.0f, 1.0f, 1.0f, alpha);
458  glBegin(GL_QUADS);
459  glTexCoord2f(0, 0); glVertex2f(0, 0);
460  glTexCoord2f(0, 1); glVertex2f(0, r.h);
461  glTexCoord2f(1, 1); glVertex2f(r.w, r.h);
462  glTexCoord2f(1, 0); glVertex2f(r.w, 0);
463  glEnd();
466  draw_text(int(0.05f * r.w), int(0.05f * r.h), rs2_stream_to_string(_stream_type));
467  }
468 
469  GLuint get_gl_handle() { return _gl_handle; }
470 
471  void render(const rs2::frame& frame, const rect& rect, float alpha = 1.f)
472  {
473  if (auto vf = frame.as<rs2::video_frame>())
474  {
475  upload(vf);
476  show(rect.adjust_ratio({ (float)vf.get_width(), (float)vf.get_height() }), alpha);
477  }
478  else if (auto mf = frame.as<rs2::motion_frame>())
479  {
480  _imu_render.render(frame, rect.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
481  }
482  else if (auto pf = frame.as<rs2::pose_frame>())
483  {
484  _pose_render.render(frame, rect.adjust_ratio({ IMU_FRAME_WIDTH, IMU_FRAME_HEIGHT }));
485  }
486  else
487  throw std::runtime_error("Rendering is currently supported for video, motion and pose frames only");
488  }
489 
490 private:
491  GLuint _gl_handle = 0;
492  rs2_stream _stream_type = RS2_STREAM_ANY;
493  int _stream_index{};
496 };
497 
498 class window
499 {
500 public:
501  std::function<void(bool)> on_left_mouse = [](bool) {};
502  std::function<void(double, double)> on_mouse_scroll = [](double, double) {};
503  std::function<void(double, double)> on_mouse_move = [](double, double) {};
504  std::function<void(int)> on_key_release = [](int) {};
505 
506  window(int width, int height, const char* title)
507  : _width(width), _height(height), _canvas_left_top_x(0), _canvas_left_top_y(0), _canvas_width(width), _canvas_height(height)
508  {
509  glfwInit();
510  win = glfwCreateWindow(width, height, title, nullptr, nullptr);
511  if (!win)
512  throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools");
514 
515  glfwSetWindowUserPointer(win, this);
516  glfwSetMouseButtonCallback(win, [](GLFWwindow* w, int button, int action, int mods)
517  {
518  auto s = (window*)glfwGetWindowUserPointer(w);
519  if (button == 0) s->on_left_mouse(action == GLFW_PRESS);
520  });
521 
522  glfwSetScrollCallback(win, [](GLFWwindow* w, double xoffset, double yoffset)
523  {
524  auto s = (window*)glfwGetWindowUserPointer(w);
525  s->on_mouse_scroll(xoffset, yoffset);
526  });
527 
528  glfwSetCursorPosCallback(win, [](GLFWwindow* w, double x, double y)
529  {
530  auto s = (window*)glfwGetWindowUserPointer(w);
531  s->on_mouse_move(x, y);
532  });
533 
534  glfwSetKeyCallback(win, [](GLFWwindow* w, int key, int scancode, int action, int mods)
535  {
536  auto s = (window*)glfwGetWindowUserPointer(w);
537  if (0 == action) // on key release
538  {
539  s->on_key_release(key);
540  }
541  });
542  }
543 
544  //another c'tor for adjusting specific frames in specific tiles, this window is NOT resizeable
545  window(unsigned width, unsigned height, const char* title, unsigned tiles_in_row, unsigned tiles_in_col, float canvas_width = 0.8f,
546  float canvas_height = 0.6f, float canvas_left_top_x = 0.1f, float canvas_left_top_y = 0.075f)
547  : _width(width), _height(height), _tiles_in_row(tiles_in_row), _tiles_in_col(tiles_in_col)
548 
549  {
550  //user input verification for mosaic size, if invalid values were given - set to default
551  if (canvas_width < 0 || canvas_width > 1 || canvas_height < 0 || canvas_height > 1 ||
552  canvas_left_top_x < 0 || canvas_left_top_x > 1 || canvas_left_top_y < 0 || canvas_left_top_y > 1)
553  {
554  std::cout << "Invalid window's size parameter entered, setting to default values" << std::endl;
555  canvas_width = 0.8f;
556  canvas_height = 0.6f;
557  canvas_left_top_x = 0.15f;
558  canvas_left_top_y = 0.075f;
559  }
560 
561  //user input verification for number of tiles in row and column
562  if (_tiles_in_row <= 0) {
563  _tiles_in_row = 4;
564  }
565  if (_tiles_in_col <= 0) {
566  _tiles_in_col = 2;
567  }
568 
569  //calculate canvas size
570  _canvas_width = int(_width * canvas_width);
571  _canvas_height = int(_height * canvas_height);
572  _canvas_left_top_x = _width * canvas_left_top_x;
573  _canvas_left_top_y = _height * canvas_left_top_y;
574 
575  //calculate tile size
576  _tile_width_pixels = float(std::floor(_canvas_width / _tiles_in_row));
577  _tile_height_pixels = float(std::floor(_canvas_height / _tiles_in_col));
578 
579  glfwInit();
580  // we don't want to enable resizing the window
582  win = glfwCreateWindow(width, height, title, nullptr, nullptr);
583  if (!win)
584  throw std::runtime_error("Could not open OpenGL window, please check your graphic drivers or use the textual SDK tools");
586 
587  glfwSetWindowUserPointer(win, this);
588  glfwSetMouseButtonCallback(win, [](GLFWwindow* w, int button, int action, int mods)
589  {
590  auto s = (window*)glfwGetWindowUserPointer(w);
591  if (button == 0) s->on_left_mouse(action == GLFW_PRESS);
592  });
593 
594  glfwSetScrollCallback(win, [](GLFWwindow* w, double xoffset, double yoffset)
595  {
596  auto s = (window*)glfwGetWindowUserPointer(w);
597  s->on_mouse_scroll(xoffset, yoffset);
598  });
599 
600  glfwSetCursorPosCallback(win, [](GLFWwindow* w, double x, double y)
601  {
602  auto s = (window*)glfwGetWindowUserPointer(w);
603  s->on_mouse_move(x, y);
604  });
605 
606  glfwSetKeyCallback(win, [](GLFWwindow* w, int key, int scancode, int action, int mods)
607  {
608  auto s = (window*)glfwGetWindowUserPointer(w);
609  if (0 == action) // on key release
610  {
611  s->on_key_release(key);
612  }
613  });
614  }
615 
617  {
618  glfwDestroyWindow(win);
619  glfwTerminate();
620  }
621 
622  void close()
623  {
624  glfwSetWindowShouldClose(win, 1);
625  }
626 
627  float width() const { return float(_width); }
628  float height() const { return float(_height); }
629 
630  operator bool()
631  {
632  glPopMatrix();
633  glfwSwapBuffers(win);
634 
635  auto res = !glfwWindowShouldClose(win);
636 
637  glfwPollEvents();
638  glfwGetFramebufferSize(win, &_width, &_height);
639 
640  // Clear the framebuffer
642  glViewport(0, 0, _width, _height);
643 
644  // Draw the images
645  glPushMatrix();
646  glfwGetWindowSize(win, &_width, &_height);
647  glOrtho(0, _width, _height, 0, -1, +1);
648 
649  return res;
650  }
651 
652  void show(rs2::frame frame)
653  {
654  show(frame, { 0, 0, (float)_width, (float)_height });
655  }
656 
657  void show(const rs2::frame& frame, const rect& rect)
658  {
659  if (auto fs = frame.as<rs2::frameset>())
660  render_frameset(fs, rect);
661  if (auto vf = frame.as<rs2::video_frame>())
662  render_video_frame(vf, rect);
663  if (auto mf = frame.as<rs2::motion_frame>())
664  render_motion_frame(mf, rect);
665  if (auto pf = frame.as<rs2::pose_frame>())
666  render_pose_frame(pf, rect);
667  }
668 
669  void show(const std::map<int, rs2::frame>& frames)
670  {
671  // Render openGl mosaic of frames
672  if (frames.size())
673  {
674  int cols = int(std::ceil(std::sqrt(frames.size())));
675  int rows = int(std::ceil(frames.size() / static_cast<float>(cols)));
676 
677  float view_width = float(_width / cols);
678  float view_height = float(_height / rows);
679  unsigned int stream_no = 0;
680  for (auto& frame : frames)
681  {
682  rect viewport_loc{ view_width * (stream_no % cols), view_height * (stream_no / cols), view_width, view_height };
683  show(frame.second, viewport_loc);
684  stream_no++;
685  }
686  }
687  else
688  {
689  _main_win.put_text("Connect one or more Intel RealSense devices and rerun the example",
690  0.4f, 0.5f, { 0.f,0.f, float(_width) , float(_height) });
691  }
692  }
693 
694  //gets as argument a map of the -need to be drawn- frames with their tiles properties,
695  //which indicates where and what size should the frame be drawn on the canvas
696  void show(const frames_mosaic& frames)
697  {
698  // Render openGl mosaic of frames
699  if (frames.size())
700  {
701  // create vector of frames from map, and sort it by priority
702  std::vector <frame_and_tile_property> vector_frames;
703  //copy: map (values) -> vector
704  for (const auto& frame : frames) { vector_frames.push_back(frame.second); }
705  //sort in ascending order of the priority
706  std::sort(vector_frames.begin(), vector_frames.end(),
707  [](const frame_and_tile_property& frame1, const frame_and_tile_property& frame2)
708  {
709  return frame1.second.priority < frame2.second.priority;
710  });
711  //create margin to the shown frame on tile
712  float frame_width_size_from_tile_width = 0.98f;
713  //iterate over frames in ascending priority order (so that lower priority frame is drawn first, and can be over-written by higher priority frame )
714  for (const auto& frame : vector_frames)
715  {
716  tile_properties attr = frame.second;
717  rect viewport_loc{ _tile_width_pixels * attr.x + _canvas_left_top_x, _tile_height_pixels * attr.y + _canvas_left_top_y,
718  _tile_width_pixels * attr.w * frame_width_size_from_tile_width, _tile_height_pixels * attr.h };
719  show(frame.first, viewport_loc);
720  }
721  }
722  else
723  {
724  _main_win.put_text("Connect one or more Intel RealSense devices and rerun the example",
725  0.3f, 0.5f, { float(_canvas_left_top_x), float(_canvas_left_top_y), float(_canvas_width) , float(_canvas_height) });
726  }
727  }
728 
729  operator GLFWwindow* () { return win; }
730 
731 private:
733  std::map<int, texture> _textures;
734  std::map<int, imu_renderer> _imus;
735  std::map<int, pose_renderer> _poses;
737  int _width, _height;
738  float _canvas_left_top_x, _canvas_left_top_y;
739  int _canvas_width, _canvas_height;
740  unsigned _tiles_in_row, _tiles_in_col;
741  float _tile_width_pixels, _tile_height_pixels;
742 
744  {
745  auto& t = _textures[f.get_profile().unique_id()];
746  t.render(f, r);
747  }
748 
750  {
751  auto& i = _imus[f.get_profile().unique_id()];
752  i.render(f, r);
753  }
754 
755  void render_pose_frame(const rs2::pose_frame& f, const rect& r)
756  {
757  auto& i = _poses[f.get_profile().unique_id()];
758  i.render(f, r);
759  }
760 
762  {
763  std::vector<rs2::frame> supported_frames;
764  for (auto f : frames)
765  {
766  if (can_render(f))
767  supported_frames.push_back(f);
768  }
769  if (supported_frames.empty())
770  return;
771 
772  std::sort(supported_frames.begin(), supported_frames.end(), [](rs2::frame first, rs2::frame second)
773  { return first.get_profile().stream_type() < second.get_profile().stream_type(); });
774 
775  auto image_grid = calc_grid(r, supported_frames);
776 
777  int image_index = 0;
778  for (auto f : supported_frames)
779  {
780  auto r = image_grid.at(image_index);
781  show(f, r);
782  image_index++;
783  }
784  }
785 
786  bool can_render(const rs2::frame& f) const
787  {
788  auto format = f.get_profile().format();
789  switch (format)
790  {
791  case RS2_FORMAT_RGB8:
792  case RS2_FORMAT_RGBA8:
793  case RS2_FORMAT_Y8:
795  case RS2_FORMAT_Y10BPACK:
796  return true;
797  default:
798  return false;
799  }
800  }
801 
803  {
804  if (r.w <= 0 || r.h <= 0 || streams <= 0)
805  throw std::runtime_error("invalid window configuration request, failed to calculate window grid");
806  float ratio = r.w / r.h;
807  auto x = sqrt(ratio * (float)streams);
808  auto y = (float)streams / x;
809  auto w = round(x);
810  auto h = round(y);
811  if (w == 0 || h == 0)
812  throw std::runtime_error("invalid window configuration request, failed to calculate window grid");
813  while (w * h > streams)
814  h > w ? h-- : w--;
815  while (w* h < streams)
816  h > w ? w++ : h++;
817  auto new_w = round(r.w / w);
818  auto new_h = round(r.h / h);
819  // column count, line count, cell width cell height
820  return rect{ static_cast<float>(w), static_cast<float>(h), static_cast<float>(new_w), static_cast<float>(new_h) };
821  }
822 
823  std::vector<rect> calc_grid(rect r, std::vector<rs2::frame>& frames)
824  {
825  auto grid = calc_grid(r, frames.size());
826 
827  std::vector<rect> rv;
828  int curr_line = -1;
829 
830  for (int i = 0; i < frames.size(); i++)
831  {
832  auto mod = i % (int)grid.x;
833  float fw = IMU_FRAME_WIDTH;
834  float fh = IMU_FRAME_HEIGHT;
835  if (auto vf = frames[i].as<rs2::video_frame>())
836  {
837  fw = (float)vf.get_width();
838  fh = (float)vf.get_height();
839  }
840  float cell_x_postion = (float)(mod * grid.w);
841  if (mod == 0) curr_line++;
842  float cell_y_position = curr_line * grid.h;
843  float2 margin = { grid.w * 0.02f, grid.h * 0.02f };
844  auto r = rect{ cell_x_postion + margin.x, cell_y_position + margin.y, grid.w - 2 * margin.x, grid.h };
845  rv.push_back(r.adjust_ratio(float2{ fw, fh }));
846  }
847 
848  return rv;
849  }
850 };
851 
852 // Struct to get keys pressed on window
854  int last_key = GLFW_KEY_UNKNOWN;
855 
858  }
859 
860  void on_key_release(int key) {
861  last_key = key;
862  }
863 
864  int get_key() {
865  int key = last_key;
866  last_key = GLFW_KEY_UNKNOWN;
867  return key;
868  }
869 };
870 
871 // Struct for managing rotation of pointcloud view
872 struct glfw_state {
873  glfw_state(float yaw = 15.0, float pitch = 15.0) : yaw(yaw), pitch(pitch), last_x(0.0), last_y(0.0),
874  ml(false), offset_x(2.f), offset_y(2.f), tex() {}
875  double yaw;
876  double pitch;
877  double last_x;
878  double last_y;
879  bool ml;
880  float offset_x;
881  float offset_y;
883 };
884 
885 // Handles all the OpenGL calls needed to display the point cloud
886 void draw_pointcloud(float width, float height, glfw_state& app_state, rs2::points& points)
887 {
888  if (!points)
889  return;
890 
891  // OpenGL commands that prep screen for the pointcloud
892  glLoadIdentity();
894 
895  glClearColor(153.f / 255, 153.f / 255, 153.f / 255, 1);
897 
899  glPushMatrix();
900  gluPerspective(60, width / height, 0.01f, 10.0f);
901 
903  glPushMatrix();
904  gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0);
905 
906  glTranslatef(0, 0, +0.5f + app_state.offset_y * 0.05f);
907  glRotated(app_state.pitch, 1, 0, 0);
908  glRotated(app_state.yaw, 0, 1, 0);
909  glTranslatef(0, 0, -0.5f);
910 
911  glPointSize(width / 640);
915  float tex_border_color[] = { 0.8f, 0.8f, 0.8f, 0.8f };
917  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, 0x812F); // GL_CLAMP_TO_EDGE
918  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, 0x812F); // GL_CLAMP_TO_EDGE
920 
921 
922  /* this segment actually prints the pointcloud */
923  auto vertices = points.get_vertices(); // get vertices
924  auto tex_coords = points.get_texture_coordinates(); // and texture coordinates
925  for (int i = 0; i < points.size(); i++)
926  {
927  if (vertices[i].z)
928  {
929  // upload the point and texture coordinates only for points we have depth data for
931  glTexCoord2fv(tex_coords[i]);
932  }
933  }
934 
935  // OpenGL cleanup
936  glEnd();
937  glPopMatrix();
939  glPopMatrix();
940  glPopAttrib();
941 }
942 
943 void quat2mat(rs2_quaternion& q, GLfloat H[16]) // to column-major matrix
944 {
945  H[0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z; H[4] = 2 * q.x * q.y - 2 * q.z * q.w; H[8] = 2 * q.x * q.z + 2 * q.y * q.w; H[12] = 0.0f;
946  H[1] = 2 * q.x * q.y + 2 * q.z * q.w; H[5] = 1 - 2 * q.x * q.x - 2 * q.z * q.z; H[9] = 2 * q.y * q.z - 2 * q.x * q.w; H[13] = 0.0f;
947  H[2] = 2 * q.x * q.z - 2 * q.y * q.w; H[6] = 2 * q.y * q.z + 2 * q.x * q.w; H[10] = 1 - 2 * q.x * q.x - 2 * q.y * q.y; H[14] = 0.0f;
948  H[3] = 0.0f; H[7] = 0.0f; H[11] = 0.0f; H[15] = 1.0f;
949 }
950 
951 // Handles all the OpenGL calls needed to display the point cloud w.r.t. static reference frame
952 void draw_pointcloud_wrt_world(float width, float height, glfw_state& app_state, rs2::points& points, rs2_pose& pose, float H_t265_d400[16], std::vector<rs2_vector>& trajectory)
953 {
954  if (!points)
955  return;
956 
957  // OpenGL commands that prep screen for the pointcloud
958  glLoadIdentity();
960 
961  glClearColor(153.f / 255, 153.f / 255, 153.f / 255, 1);
963 
965  glPushMatrix();
966  gluPerspective(60, width / height, 0.01f, 10.0f);
967 
968 
969  // viewing matrix
971  glPushMatrix();
972 
973  // rotated from depth to world frame: z => -z, y => -y
974  glTranslatef(0, 0, -0.75f - app_state.offset_y * 0.05f);
975  glRotated(app_state.pitch, 1, 0, 0);
976  glRotated(app_state.yaw, 0, -1, 0);
977  glTranslatef(0, 0, 0.5f);
978 
979  // draw trajectory
981  glLineWidth(2.0f);
983  for (auto&& v : trajectory)
984  {
985  glColor3f(0.0f, 1.0f, 0.0f);
986  glVertex3f(v.x, v.y, v.z);
987  }
988  glEnd();
989  glLineWidth(0.5f);
990  glColor3f(1.0f, 1.0f, 1.0f);
991 
992  // T265 pose
993  GLfloat H_world_t265[16];
994  quat2mat(pose.rotation, H_world_t265);
995  H_world_t265[12] = pose.translation.x;
996  H_world_t265[13] = pose.translation.y;
997  H_world_t265[14] = pose.translation.z;
998 
999  glMultMatrixf(H_world_t265);
1000 
1001  // T265 to D4xx extrinsics
1002  glMultMatrixf(H_t265_d400);
1003 
1004 
1005  glPointSize(width / 640);
1009  float tex_border_color[] = { 0.8f, 0.8f, 0.8f, 0.8f };
1011  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, 0x812F); // GL_CLAMP_TO_EDGE
1012  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, 0x812F); // GL_CLAMP_TO_EDGE
1013  glBegin(GL_POINTS);
1014 
1015  /* this segment actually prints the pointcloud */
1016  auto vertices = points.get_vertices(); // get vertices
1017  auto tex_coords = points.get_texture_coordinates(); // and texture coordinates
1018  for (int i = 0; i < points.size(); i++)
1019  {
1020  if (vertices[i].z)
1021  {
1022  // upload the point and texture coordinates only for points we have depth data for
1024  glTexCoord2fv(tex_coords[i]);
1025  }
1026  }
1027 
1028  // OpenGL cleanup
1029  glEnd();
1030  glPopMatrix();
1032  glPopMatrix();
1033  glPopAttrib();
1034 }
1035 
1036 // Registers the state variable and callbacks to allow mouse control of the pointcloud
1038 {
1039  app.on_left_mouse = [&](bool pressed)
1040  {
1041  app_state.ml = pressed;
1042  };
1043 
1044  app.on_mouse_scroll = [&](double xoffset, double yoffset)
1045  {
1046  app_state.offset_x -= static_cast<float>(xoffset);
1047  app_state.offset_y -= static_cast<float>(yoffset);
1048  };
1049 
1050  app.on_mouse_move = [&](double x, double y)
1051  {
1052  if (app_state.ml)
1053  {
1054  app_state.yaw -= (x - app_state.last_x);
1055  app_state.yaw = std::max(app_state.yaw, -120.0);
1056  app_state.yaw = std::min(app_state.yaw, +120.0);
1057  app_state.pitch += (y - app_state.last_y);
1058  app_state.pitch = std::max(app_state.pitch, -80.0);
1059  app_state.pitch = std::min(app_state.pitch, +80.0);
1060  }
1061  app_state.last_x = x;
1062  app_state.last_y = y;
1063  };
1064 
1065  app.on_key_release = [&](int key)
1066  {
1067  if (key == 32) // Escape
1068  {
1069  app_state.yaw = app_state.pitch = 0; app_state.offset_x = app_state.offset_y = 0.0;
1070  }
1071  };
1072 }
static void draw_axes(float axis_size=1.f, float axisWidth=4.f)
Definition: example.hpp:266
bool can_render(const rs2::frame &f) const
Definition: example.hpp:786
#define glDisableClientState
std::function< void(bool)> on_left_mouse
Definition: example.hpp:501
float z
Definition: example.hpp:36
#define GL_TEXTURE_MAG_FILTER
void render(const rs2::motion_frame &frame, const rect &r)
Definition: example.hpp:129
#define glEnableClientState
GLint y
float h
Definition: example.hpp:73
#define GLFW_KEY_UNKNOWN
Definition: glfw3.h:357
void print_text_in_3d(float x, float y, float z, const char *text, bool center_text, GLfloat model[], GLfloat proj[], float vec_norm)
Definition: example.hpp:258
GLFWAPI void glfwGetWindowSize(GLFWwindow *window, int *width, int *height)
Retrieves the size of the client area of the specified window.
Definition: window.c:544
void render(const rs2::frame &frame, const rect &rect, float alpha=1.f)
Definition: example.hpp:471
khronos_float_t GLfloat
window_key_listener(window &win)
Definition: example.hpp:856
void draw_axes()
Definition: rs-motion.cpp:15
#define glRotatef
#define GL_VERTEX_ARRAY
Priority
Definition: example.hpp:29
The header of the GLFW 3 API.
void draw_motion(const rs2::motion_frame &f, const rect &r)
Definition: example.hpp:139
unsigned int w
Definition: example.hpp:94
float z
Definition: rs_types.h:131
window(int width, int height, const char *title)
Definition: example.hpp:506
unsigned int x
Definition: example.hpp:93
GLdouble s
#define GL_TEXTURE_2D
void put_text(const std::string &msg, float norm_x_pos, float norm_y_pos, const rect &r)
Definition: example.hpp:391
#define GL_RGBA
#define glVertexPointer
#define glOrtho
#define glBegin
float x
Definition: example.hpp:68
imu_renderer _imu_render
Definition: example.hpp:494
void quat2mat(rs2_quaternion &q, GLfloat H[16])
Definition: example.hpp:943
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:10806
#define GL_UNSIGNED_BYTE
double yaw
Definition: example.hpp:875
void render(const rs2::pose_frame &frame, const rect &r)
Definition: example.hpp:349
#define glPopMatrix
#define GL_PROJECTION
static int stb_easy_font_print(float x, float y, char *text, unsigned char color[4], void *vertex_buffer, int vbuf_size)
GLFWAPI void * glfwGetWindowUserPointer(GLFWwindow *window)
Returns the user pointer of the specified window.
Definition: window.c:964
stream_profile get_profile() const
Definition: rs_frame.hpp:557
#define glPushAttrib
#define glDrawArrays
rs2_vector translation
Definition: rs_types.h:142
std::vector< rect > calc_grid(rect r, std::vector< rs2::frame > &frames)
Definition: example.hpp:823
#define glTexCoord2fv
#define glPointSize
void operator*=(float t)
Definition: example.hpp:47
GLuint GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat t1
Definition: glext.h:9721
GLFWAPI GLFWmousebuttonfun glfwSetMouseButtonCallback(GLFWwindow *window, GLFWmousebuttonfun cbfun)
Sets the mouse button callback.
Definition: input.c:821
double pitch
Definition: example.hpp:876
float2 xyz_to_xy(float x, float y, float z, GLfloat model[], GLfloat proj[], float vec_norm)
Definition: example.hpp:244
#define glVertex3fv
void show(const rs2::frame &frame, const rect &rect)
Definition: example.hpp:657
GLfloat angle
Definition: glext.h:6819
float3 operator*(float t)
Definition: example.hpp:37
#define GL_POLYGON
const void * get_data() const
Definition: rs_frame.hpp:545
std::string stream_name() const
Definition: rs_frame.hpp:113
GLdouble GLdouble GLdouble w
~window()
Definition: example.hpp:616
GLsizei const GLchar *const * string
#define GL_LINEAR
rect adjust_ratio(float2 size) const
Definition: example.hpp:76
static void draw_circle(float xx, float xy, float xz, float yx, float yy, float yz, float radius=1.1, float3 center={0.0, 0.0, 0.0}, float intensity=0.5f)
Definition: example.hpp:323
#define glLoadIdentity
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
GLdouble GLdouble z
#define glTexImage2D
#define glColor4f
#define GL_CLAMP
#define glVertex3f
GLenum GLfloat * buffer
void operator=(float3 other)
Definition: example.hpp:54
const texture_coordinate * get_texture_coordinates() const
Definition: rs_frame.hpp:792
#define glEnable
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
rs2_pose get_pose_data() const
Definition: rs_frame.hpp:933
The texture class.
Definition: example.hpp:402
#define GL_LUMINANCE
#define glGetFloatv
void add(float t1, float t2, float t3)
Definition: example.hpp:61
::realsense_legacy_msgs::pose_< std::allocator< void > > pose
Definition: pose.h:88
GLuint get_gl_handle()
Definition: example.hpp:469
#define GL_TEXTURE_WRAP_T
GLdouble t
GLuint64 key
Definition: glext.h:8966
size_t size() const
Definition: rs_frame.hpp:800
const int H
void on_key_release(int key)
Definition: example.hpp:860
Quaternion used to represent rotation.
Definition: rs_types.h:135
Print flat 2D text over openGl window.
Definition: example.hpp:388
float offset_x
Definition: example.hpp:880
float y
Definition: rs_types.h:131
GLFWAPI int glfwInit(void)
Initializes the GLFW library.
Definition: init.c:198
GLdouble f
const int W
static const textual_icon grid
Definition: model-views.h:253
int xx
Definition: rmse.py:52
std::map< int, pose_renderer > _poses
Definition: example.hpp:735
double last_x
Definition: example.hpp:877
GLfloat GLfloat GLfloat alpha
rs2_vector get_motion_data() const
Definition: rs_frame.hpp:905
GLFWAPI void glfwSetWindowUserPointer(GLFWwindow *window, void *pointer)
Sets the user pointer of the specified window.
Definition: window.c:955
#define glGenTextures
#define GL_FLOAT
dictionary streams
Definition: t265_stereo.py:140
GLsizeiptr size
#define GL_COLOR_BUFFER_BIT
unsigned _tiles_in_row
Definition: example.hpp:740
int yy
Definition: rmse.py:53
float y
Definition: example.hpp:68
std::ostream & cout()
float w
Definition: example.hpp:73
#define glTexParameteri
std::map< int, imu_renderer > _imus
Definition: example.hpp:734
GLdouble GLdouble r
#define glTexParameterfv
#define GL_LINES
std::map< int, texture > _textures
Definition: example.hpp:733
GLenum GLint GLint * precision
Definition: glext.h:1883
GLdouble x
float width() const
Definition: example.hpp:627
#define glLineWidth
Priority priority
Definition: example.hpp:95
void upload(const rs2::video_frame &frame)
Definition: example.hpp:406
float pitch
Definition: t265_rpy.py:44
#define glPushMatrix
void render_video_frame(const rs2::video_frame &f, const rect &r)
Definition: example.hpp:743
pose_renderer _pose_render
Definition: example.hpp:495
#define GL_PROJECTION_MATRIX
#define glPixelStorei
#define glClear
static const textual_icon upload
Definition: model-views.h:235
GLint GLsizei GLsizei height
float offset_y
Definition: example.hpp:881
#define GL_UNSIGNED_SHORT
std::map< int, frame_and_tile_property > frames_mosaic
Definition: example.hpp:101
GLFWAPI void glfwSwapBuffers(GLFWwindow *window)
Swaps the front and back buffers of the specified window.
Definition: context.c:641
GLint GLint GLsizei GLint GLenum format
GLFWAPI void glfwMakeContextCurrent(GLFWwindow *window)
Makes the context of the specified window current for the calling thread.
Definition: context.c:611
const vertex * get_vertices() const
Definition: rs_frame.hpp:767
rs2_quaternion rotation
Definition: rs_types.h:145
#define glTranslatef
#define PI
Definition: example.hpp:24
def on_mouse_scroll(x, y, scroll_x, scroll_y)
void calc_grid(void)
Definition: wave.c:217
#define GL_TEXTURE_MIN_FILTER
#define GL_QUADS
void draw_pose(const rs2::pose_frame &f, const rect &r)
Definition: example.hpp:360
void show(rs2::frame frame)
Definition: example.hpp:652
std::pair< rs2::frame, tile_properties > frame_and_tile_property
Definition: example.hpp:100
GLFWAPI GLFWcursorposfun glfwSetCursorPosCallback(GLFWwindow *window, GLFWcursorposfun cbfun)
Sets the cursor position callback.
Definition: input.c:832
#define glEnd
GLint j
GLint first
#define glGetError
float y
Definition: example.hpp:36
void multiply_vector_by_matrix(GLfloat vec[], GLfloat mat[], GLfloat *result)
Definition: example.hpp:230
int stream_index() const
Definition: rs_frame.hpp:34
GLFWwindow * win
Definition: example.hpp:732
float yaw
Definition: t265_rpy.py:46
GLuint get_gl_handle()
Definition: example.hpp:354
void show(const frames_mosaic &frames)
Definition: example.hpp:696
float x
Definition: rs_types.h:131
#define GLFW_RESIZABLE
Window resize-ability window hint and attribute.
Definition: glfw3.h:774
action
Definition: enums.py:62
#define glViewport
#define GL_DEPTH_BUFFER_BIT
#define glTexCoord2f
#define GL_MODELVIEW
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
GLFWAPI void glfwSetWindowShouldClose(GLFWwindow *window, int value)
Sets the close flag of the specified window.
Definition: window.c:486
void show(const std::map< int, rs2::frame > &frames)
Definition: example.hpp:669
float height() const
Definition: example.hpp:628
GLFWAPI GLFWwindow * glfwCreateWindow(int width, int height, const char *title, GLFWmonitor *monitor, GLFWwindow *share)
Creates a window and its associated context.
Definition: window.c:151
std::function< void(int)> on_key_release
Definition: example.hpp:504
GLuint get_gl_handle()
Definition: example.hpp:134
Definition: example.hpp:70
rect calc_grid(rect r, size_t streams)
Definition: example.hpp:802
rs2_format format() const
Definition: rs_frame.hpp:44
#define GL_FALSE
#define glVertex2d
#define glColor3f
void draw_pointcloud(float width, float height, glfw_state &app_state, rs2::points &points)
Definition: example.hpp:886
glfw_state(float yaw=15.0, float pitch=15.0)
Definition: example.hpp:873
GLdouble GLdouble GLdouble q
float _canvas_left_top_y
Definition: example.hpp:738
const char * rs2_stream_to_string(rs2_stream stream)
Definition: rs.cpp:1262
static const struct @18 vertices[3]
unsigned int GLuint
GLint GLint GLint yoffset
static int stb_easy_font_width(char *text)
window(unsigned width, unsigned height, const char *title, unsigned tiles_in_row, unsigned tiles_in_col, float canvas_width=0.8f, float canvas_height=0.6f, float canvas_left_top_x=0.1f, float canvas_left_top_y=0.075f)
Definition: example.hpp:545
void show(const rect &r, float alpha=1.f) const
Definition: example.hpp:448
int get_height() const
Definition: rs_frame.hpp:671
int min(int a, int b)
Definition: lz4s.c:73
unsigned int h
Definition: example.hpp:94
GLFWAPI GLFWscrollfun glfwSetScrollCallback(GLFWwindow *window, GLFWscrollfun cbfun)
Sets the scroll callback.
Definition: input.c:854
void close()
Definition: example.hpp:622
GLFWAPI void glfwDestroyWindow(GLFWwindow *window)
Destroys the specified window and its context.
Definition: window.c:444
#define glPopAttrib
GLFWAPI void glfwGetFramebufferSize(GLFWwindow *window, int *width, int *height)
Retrieves the size of the framebuffer of the specified window.
Definition: window.c:647
#define glBindTexture
void draw_text(int x, int y, const char *text)
Definition: example.hpp:109
const float IMU_FRAME_HEIGHT
Definition: example.hpp:28
double last_y
Definition: example.hpp:878
GLFWAPI void glfwTerminate(void)
Terminates the GLFW library.
Definition: init.c:243
#define GL_MODELVIEW_MATRIX
#define glClearColor
BOOST_MPL_AUX_ADL_BARRIER_NAMESPACE_OPEN typedef arg< 1 > _1
GLFWAPI GLFWkeyfun glfwSetKeyCallback(GLFWwindow *window, GLFWkeyfun cbfun)
Sets the key callback.
Definition: input.c:791
#define GL_LINE_STRIP
GLFWAPI void glfwPollEvents(void)
Processes all pending events.
Definition: window.c:1072
#define GL_TEXTURE_BORDER_COLOR
std::function< void(double, double)> on_mouse_scroll
Definition: example.hpp:502
void render_motion_frame(const rs2::motion_frame &f, const rect &r)
Definition: example.hpp:749
unsigned int GLenum
int i
GLuint res
Definition: glext.h:8856
#define glRotated
#define GL_RGB
int _width
Definition: example.hpp:737
GLuint GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat GLfloat s1
Definition: glext.h:9721
#define glVertex2f
int unique_id() const
Definition: rs_frame.hpp:54
void render_frameset(const rs2::frameset &frames, const rect &r)
Definition: example.hpp:761
#define GL_ALL_ATTRIB_BITS
#define glMultMatrixf
#define GL_UNPACK_ROW_LENGTH
float3 operator-(float t)
Definition: example.hpp:42
std::function< void(double, double)> on_mouse_move
Definition: example.hpp:503
float x
Definition: example.hpp:72
rs2_stream stream_type() const
Definition: rs_frame.hpp:39
void register_glfw_callbacks(window &app, glfw_state &app_state)
Definition: example.hpp:1037
#define GLFW_PRESS
The key or mouse button was pressed.
Definition: glfw3.h:304
texture tex
Definition: example.hpp:882
GLuint64EXT * result
Definition: glext.h:10921
GLdouble v
#define GL_TEXTURE_WRAP_S
GLint GLint xoffset
float _tile_width_pixels
Definition: example.hpp:741
#define GL_POINTS
int get_width() const
Definition: rs_frame.hpp:659
#define glMatrixMode
struct GLFWwindow GLFWwindow
GLFWAPI void glfwWindowHint(int hint, int value)
Sets the specified window hint to the desired value.
Definition: window.c:291
unsigned int y
Definition: example.hpp:93
void draw_pointcloud_wrt_world(float width, float height, glfw_state &app_state, rs2::points &points, rs2_pose &pose, float H_t265_d400[16], std::vector< rs2_vector > &trajectory)
Definition: example.hpp:952
GLint GLsizei width
GLdouble GLdouble GLint GLint const GLdouble * points
float x
Definition: example.hpp:36
#define GL_TRIANGLES
#define GL_DEPTH_TEST
int _canvas_width
Definition: example.hpp:739
void set_viewport(const rect &r)
Definition: example.hpp:118
float y
Definition: example.hpp:72
void render_pose_frame(const rs2::pose_frame &f, const rect &r)
Definition: example.hpp:755
#define glDisable
text_renderer _main_win
Definition: example.hpp:736
T as() const
Definition: rs_frame.hpp:580
GLFWAPI int glfwWindowShouldClose(GLFWwindow *window)
Checks the close flag of the specified window.
Definition: window.c:477
const float IMU_FRAME_WIDTH
Definition: example.hpp:27
std::string to_string(T value)


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