align-gl.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
3 
4 #include "../include/librealsense2/hpp/rs_sensor.hpp"
5 #include "../include/librealsense2/hpp/rs_processing.hpp"
6 #include "../include/librealsense2-gl/rs_processing_gl.hpp"
7 
9 #include "align-gl.h"
10 #include "option.h"
11 
12 #ifndef NOMINMAX
13 #define NOMINMAX
14 #endif // NOMINMAX
15 
16 #include <glad/glad.h>
17 
18 #include <iostream>
19 
20 #include <chrono>
21 #include <strstream>
22 
23 #include "synthetic-stream-gl.h"
24 
25 using namespace rs2;
26 using namespace librealsense::gl;
27 
28 rs2_extension align_gl::select_extension(const rs2::frame& input)
29 {
31  return ext;
32 }
33 
34 void align_gl::cleanup_gpu_resources()
35 {
36  _renderer.reset();
37  _pc.reset();
38  _other_texture.reset();
39  _upload.reset();
40  _enabled = 0;
41 }
42 
43 void align_gl::create_gpu_resources()
44 {
45  _renderer = std::make_shared<rs2::gl::pointcloud_renderer>();
46  _pc = std::make_shared<rs2::gl::pointcloud>();
47  _other_texture = std::make_shared<rs2::texture_buffer>();
48  _upload = std::make_shared<rs2::gl::uploader>();
49  _enabled = glsl_enabled() ? 1 : 0;
50 }
51 
52 void align_gl::align_z_to_other(rs2::video_frame& aligned,
53  const rs2::video_frame& depth,
55  float z_scale)
56 {
57  auto width = aligned.get_width();
58  auto height = aligned.get_height();
59 
60  _pc->map_to(depth);
61  auto p = _pc->calculate(depth);
62 
63  auto frame_ref = dynamic_cast<librealsense::depth_frame*>((librealsense::frame_interface*)aligned.get());
64  auto gf = dynamic_cast<gpu_addon_interface*>(frame_ref);
65 
67 
68  // Set the depth origin of new depth frame to the old depth frame
69  auto depth_ptr = dynamic_cast<librealsense::depth_frame*>((librealsense::frame_interface*)depth.get());
70  frame_ref->set_sensor(depth_ptr->get_sensor());
71  depth_ptr->acquire();
72  frame_holder h{ depth_ptr };
73  frame_ref->set_original(std::move(h));
74 
75  uint32_t aligned_tex;
76  auto format = depth.get_profile().format();
77  auto tex_type = rs_format_to_gl_format(format);
78  gf->get_gpu_section().output_texture(0, &aligned_tex, tex_type.type);
79  glTexImage2D(GL_TEXTURE_2D, 0, tex_type.internal_format,
80  width, height, 0, tex_type.gl_format, tex_type.data_type, nullptr);
81 
82  auto prof = depth.get_profile();
83  auto intr = other_profile.get_intrinsics();
84  auto extr = prof.get_extrinsics_to(other_profile);
85 
86  render(p, depth, intr, extr, aligned_tex);
87 
88  //aligned.get_data();
89  aligned = _upload->process(aligned);
90  aligned = _upload->process(aligned);
91 }
92 
93 // From: https://jamesgregson.blogspot.com/2011/11/matching-calibrated-cameras-with-opengl.html
95  int *viewport, double alpha, double beta, double skew,
96  double u0, double v0, int img_width, int img_height, double near_clip, double far_clip )
97 {
98 
99  // These parameters define the final viewport that is rendered into by
100  // the camera.
101  int L = 0;
102  int R = img_width;
103  int B = 0;
104  int T = img_height;
105 
106  // near and far clipping planes, these only matter for the mapping from
107  // world-space z-coordinate into the depth coordinate for OpenGL
108  double N = near_clip;
109  double F = far_clip;
110 
111  // set the viewport parameters
112  viewport[0] = L;
113  viewport[1] = B;
114  viewport[2] = R-L;
115  viewport[3] = T-B;
116 
117  // construct an orthographic matrix which maps from projected
118  // coordinates to normalized device coordinates in the range
119  // [-1, 1]. OpenGL then maps coordinates in NDC to the current
120  // viewport
121  matrix4 ortho;
122  ortho(0,0) = 2.f/(R-L); ortho(0,3) = float(-(R+L)/(R-L));
123  ortho(1,1) = 2.f/(T-B); ortho(1,3) = float(-(T+B)/(T-B));
124  ortho(2,2) = -2.f/float(F-N); ortho(2,3) = float(-(F+N)/(F-N));
125  ortho(3,3) = 1.f;
126 
127  // construct a projection matrix, this is identical to the
128  // projection matrix computed for the intrinsicx, except an
129  // additional row is inserted to map the z-coordinate to
130  // OpenGL.
131  matrix4 tproj;
132  tproj(0,0) = float(alpha); tproj(0,1) = float(skew); tproj(0,2) = 0.f;
133  tproj(1,1) = float(beta); tproj(1,2) = float(v0);
134  tproj(2,2) = float(-(N+F)); tproj(2,3) = float(-N*F);
135  tproj(3,2) = 1.f;
136 
137  // resulting OpenGL frustum is the product of the orthographic
138  // mapping to normalized device coordinates and the augmented
139  // camera intrinsic matrix
140  frustum = ortho*tproj;
141 }
142 
143 void align_gl::render(const rs2::points& model,
144  const rs2::video_frame& tex,
145  const rs2_intrinsics& intr,
146  const rs2_extrinsics& extr,
147  uint32_t output_texture)
148 {
149  perform_gl_action([&] {
150  auto width = intr.width;
151  auto height = intr.height;
152 
154  if (auto input_frame = tex.as<rs2::gl::gpu_frame>())
155  {
156  texture = input_frame.get_texture_id(0);
157  }
158  else
159  {
160  _other_texture->upload(tex, tex.get_profile().format());
161  texture = _other_texture->get_gl_handle();
162  }
163 
164  fbo fbo(width, height);
165 
166  glBindTexture(GL_TEXTURE_2D, output_texture);
167 
168  auto format = tex.get_profile().format();
169  auto textype = rs_format_to_gl_format(format);
170  glTexImage2D(GL_TEXTURE_2D, 0, textype.internal_format,
171  width, height, 0, textype.gl_format, textype.data_type, nullptr);
172 
177 
180 
181  glBindTexture(GL_TEXTURE_2D, output_texture);
182  fbo.createTextureAttachment(output_texture);
183 
184  fbo.bind();
185  glClearColor(0, 0, 0, 1);
187 
188  matrix4 projection;
189  int viewport[4];
190  build_opengl_projection_for_intrinsics(projection, viewport,
191  intr.fx, intr.fy, 0.0, intr.width - intr.ppx, intr.height - intr.ppy, width, height, 0.001, 100);
192  projection(3, 2) *= -1.f;
193  projection(2, 3) *= -1.f;
194  projection(2, 2) *= -1.f;
195  float pm[16];
196  projection.to_column_major(pm);
197  _renderer->set_matrix(RS2_GL_MATRIX_PROJECTION, pm);
199  view(2, 2) = -1.f;
200  _renderer->set_matrix(RS2_GL_MATRIX_CAMERA, (float*)&view.mat);
201 
202  matrix4 other = matrix4::identity();
203  for (int i = 0; i < 3; i++)
204  {
205  for (int j = 0; j < 3; j++)
206  other(i,j) = extr.rotation[i*3 + j];
207  other(3, i) = extr.translation[i];
208  }
209  _renderer->set_matrix(RS2_GL_MATRIX_TRANSFORMATION, (float*)&other.mat);
210 
211  glBindTexture(GL_TEXTURE_2D, texture);
212  _renderer->process(model);
213 
214  fbo.unbind();
215 
217  }, [&] {
218  _enabled = false;
219  });
220 }
221 
222 void align_gl::align_other_to_z(rs2::video_frame& aligned,
223  const rs2::video_frame& depth,
224  const rs2::video_frame& other,
225  float z_scale)
226 {
227  auto width = aligned.get_width();
228  auto height = aligned.get_height();
229 
230  _pc->map_to(other);
231  auto p = _pc->calculate(depth);
232 
233  auto frame_ref = (frame_interface*)aligned.get();
234  auto gf = dynamic_cast<gpu_addon_interface*>(frame_ref);
235 
236  uint32_t output_rgb;
237  auto format = other.get_profile().format();
238  auto tex_type = rs_format_to_gl_format(format);
239 
240  gf->get_gpu_section().output_texture(0, &output_rgb, tex_type.type);
241  glTexImage2D(GL_TEXTURE_2D, 0, tex_type.internal_format,
242  width, height, 0, tex_type.gl_format, tex_type.data_type, nullptr);
243 
244  gf->get_gpu_section().set_size(width, height);
245 
246  auto prof = depth.get_profile();
247  auto intr = prof.as<video_stream_profile>().get_intrinsics();
248  auto extr = prof.get_extrinsics_to(prof);
249  render(p, other, intr, extr, output_rgb);
250 }
251 
252 align_gl::align_gl(rs2_stream to_stream) : align(to_stream, "Align (GLSL)")
253 {
256 
257  auto opt = std::make_shared<librealsense::ptr_option<int>>(
258  0, 1, 0, 1, &_enabled, "GLSL enabled");
260 
261  initialize();
262 }
263 
265 {
266  perform_gl_action([&]()
267  {
269  }, []{});
270 }
#define GL_TEXTURE_MAG_FILTER
static matrix4 identity()
Definition: rendering.h:281
rs2_frame * get() const
Definition: rs_frame.hpp:590
void add_extension(rs2_extension ex)
Definition: source.h:45
#define GL_TEXTURE_2D
void build_opengl_projection_for_intrinsics(matrix4 &frustum, int *viewport, double alpha, double beta, double skew, double u0, double v0, int img_width, int img_height, double near_clip, double far_clip)
Definition: align-gl.cpp:94
GLfloat GLfloat p
Definition: glext.h:12687
float translation[3]
Definition: rs_sensor.h:99
stream_profile get_profile() const
Definition: rs_frame.hpp:557
matrix4 frustum(float left, float right, float bottom, float top, float zNear, float zFar, float ox, float oy)
Definition: pc-shader.cpp:206
texture_mapping & rs_format_to_gl_format(rs2_format type)
#define GL_CLAMP_TO_EDGE
GLint GLint GLsizei GLsizei GLsizei depth
Definition: cah-model.h:10
GLfloat v0
GLfloat GLfloat GLfloat GLfloat h
Definition: glext.h:1960
#define glTexImage2D
uint32_t get() const
Definition: opengl3.h:328
float rotation[9]
Definition: rs_sensor.h:98
#define GL_TEXTURE_WRAP_T
void bind()
Definition: opengl3.cpp:635
void register_option(rs2_option id, std::shared_ptr< option > option)
Definition: options.h:86
bool is() const
Definition: rs_frame.hpp:570
GLfloat GLfloat GLfloat alpha
#define GL_COLOR_BUFFER_BIT
#define glBindFramebuffer
#define glTexParameteri
unsigned int uint32_t
Definition: stdint.h:80
#define glClear
GLint GLsizei GLsizei height
GLint GLint GLsizei GLint GLenum format
void set_size(uint32_t width, uint32_t height, bool preloaded=false)
GLfloat beta
Definition: wave.c:31
void unbind()
Definition: opengl3.cpp:647
#define GL_TEXTURE_MIN_FILTER
void cleanup_gpu_resources() override
Definition: align-gl.cpp:34
GLint j
float mat[4][4]
Definition: rendering.h:274
rs2_intrinsics get_intrinsics() const
Definition: rs_frame.hpp:238
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
void set_sensor(std::shared_ptr< sensor_interface > s) override
Definition: archive.cpp:24
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
rs2_format format() const
Definition: rs_frame.hpp:44
void createTextureAttachment(uint32_t texture)
Definition: opengl3.cpp:612
#define RS2_EXTENSION_VIDEO_FRAME_GL
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:166
GLenum GLenum GLenum input
Definition: glext.h:10805
int get_height() const
Definition: rs_frame.hpp:671
rs2_extrinsics extr
Definition: test-pose.cpp:258
#define glBindTexture
void perform_gl_action(T action, S fallback)
virtual gpu_section & get_gpu_section()=0
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
Video stream intrinsics.
Definition: rs_types.h:58
#define glClearColor
#define RS2_EXTENSION_DEPTH_FRAME_GL
#define GL_COLOR_ATTACHMENT0
GLuint texture
int i
void to_column_major(float column_major[16])
Definition: rendering.h:391
#define glDrawBuffer
#define GL_NEAREST
#define GL_TEXTURE_WRAP_S
int get_width() const
Definition: rs_frame.hpp:659
#define GL_FRAMEBUFFER
GLint GLsizei width
T as() const
Definition: rs_frame.hpp:580


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