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 
7 
8 #include "../proc/synthetic-stream.h"
9 #include "align-gl.h"
10 #include "option.h"
11 #include "rendering.h"
12 
13 
14 #ifndef NOMINMAX
15 #define NOMINMAX
16 #endif // NOMINMAX
17 
18 #include <glad/glad.h>
19 
20 #include <iostream>
21 
22 #include <chrono>
23 
24 #include "synthetic-stream-gl.h"
25 
26 using namespace rs2;
27 using namespace librealsense::gl;
28 
29 rs2_extension align_gl::select_extension(const rs2::frame& input)
30 {
32  return ext;
33 }
34 
35 void align_gl::cleanup_gpu_resources()
36 {
37  _renderer.reset();
38  _pc.reset();
39  _other_texture.reset();
40  _upload.reset();
41  _enabled = 0;
42 }
43 
44 void align_gl::create_gpu_resources()
45 {
46  _renderer = std::make_shared<rs2::gl::pointcloud_renderer>();
47  _pc = std::make_shared<rs2::gl::pointcloud>();
48  _other_texture = std::make_shared<rs2::texture_buffer>();
49  _upload = std::make_shared<rs2::gl::uploader>();
50  _enabled = glsl_enabled() ? 1 : 0;
51 }
52 
53 void align_gl::align_z_to_other(rs2::video_frame& aligned,
54  const rs2::video_frame& depth,
56  float z_scale)
57 {
58  auto width = aligned.get_width();
59  auto height = aligned.get_height();
60 
61  _pc->map_to(depth);
62  auto p = _pc->calculate(depth);
63 
64  auto frame_ref = dynamic_cast<librealsense::depth_frame*>((librealsense::frame_interface*)aligned.get());
65  if (!frame_ref)
66  throw std::runtime_error("Frame is not depth frame, cannot cast");
67 
68  auto gf = dynamic_cast<gpu_addon_interface*>(frame_ref);
69  if (!gf)
70  throw std::runtime_error("Frame is not gpu_addon_interface, cannot output texture");
71 
72  gf->get_gpu_section().set_size(width, height);
73 
74  // Set the depth origin of new depth frame to the old depth frame
75  auto depth_ptr = dynamic_cast<librealsense::depth_frame*>((librealsense::frame_interface*)depth.get());
76  if (!depth_ptr)
77  throw std::runtime_error("Frame interface is not depth frame");
78 
79  frame_ref->set_sensor(depth_ptr->get_sensor());
80  depth_ptr->acquire();
81  frame_holder h{ depth_ptr };
82  frame_ref->set_original(std::move(h));
83 
84  uint32_t aligned_tex;
85  auto format = depth.get_profile().format();
86  auto tex_type = rs_format_to_gl_format(format);
87  gf->get_gpu_section().output_texture(0, &aligned_tex, tex_type.type);
88  glTexImage2D(GL_TEXTURE_2D, 0, tex_type.internal_format,
89  width, height, 0, tex_type.gl_format, tex_type.data_type, nullptr);
90 
91  auto prof = depth.get_profile();
92  auto intr = other_profile.get_intrinsics();
93  auto extr = prof.get_extrinsics_to(other_profile);
94 
95  render(p, depth, intr, extr, aligned_tex);
96 
97  //aligned.get_data();
98  aligned = _upload->process(aligned);
99  aligned = _upload->process(aligned);
100 }
101 
102 // From: https://jamesgregson.blogspot.com/2011/11/matching-calibrated-cameras-with-opengl.html
104  int *viewport, double alpha, double beta, double skew,
105  double u0, double v0, int img_width, int img_height, double near_clip, double far_clip )
106 {
107 
108  // These parameters define the final viewport that is rendered into by
109  // the camera.
110  int L = 0;
111  int R = img_width;
112  int B = 0;
113  int T = img_height;
114 
115  // near and far clipping planes, these only matter for the mapping from
116  // world-space z-coordinate into the depth coordinate for OpenGL
117  double N = near_clip;
118  double F = far_clip;
119 
120  // set the viewport parameters
121  viewport[0] = L;
122  viewport[1] = B;
123  viewport[2] = R-L;
124  viewport[3] = T-B;
125 
126  // construct an orthographic matrix which maps from projected
127  // coordinates to normalized device coordinates in the range
128  // [-1, 1]. OpenGL then maps coordinates in NDC to the current
129  // viewport
130  matrix4 ortho;
131  ortho(0,0) = 2.f/(R-L); ortho(0,3) = float(-(R+L)/(R-L));
132  ortho(1,1) = 2.f/(T-B); ortho(1,3) = float(-(T+B)/(T-B));
133  ortho(2,2) = -2.f/float(F-N); ortho(2,3) = float(-(F+N)/(F-N));
134  ortho(3,3) = 1.f;
135 
136  // construct a projection matrix, this is identical to the
137  // projection matrix computed for the intrinsicx, except an
138  // additional row is inserted to map the z-coordinate to
139  // OpenGL.
140  matrix4 tproj;
141  tproj(0,0) = float(alpha); tproj(0,1) = float(skew); tproj(0,2) = float(u0);
142  tproj(1,1) = float(beta); tproj(1,2) = float(v0);
143  tproj(2,2) = float(-(N+F)); tproj(2,3) = float(-N*F);
144  tproj(3,2) = 1.f;
145 
146  // resulting OpenGL frustum is the product of the orthographic
147  // mapping to normalized device coordinates and the augmented
148  // camera intrinsic matrix
149  frustum = ortho*tproj;
150 }
151 
152 void align_gl::render(const rs2::points& model,
153  const rs2::video_frame& tex,
154  const rs2_intrinsics& intr,
155  const rs2_extrinsics& extr,
156  uint32_t output_texture)
157 {
158  perform_gl_action([&] {
159  auto width = intr.width;
160  auto height = intr.height;
161 
163  if (auto input_frame = tex.as<rs2::gl::gpu_frame>())
164  {
165  texture = input_frame.get_texture_id(0);
166  }
167  else
168  {
169  _other_texture->upload(tex, tex.get_profile().format());
170  texture = _other_texture->get_gl_handle();
171  }
172 
173  fbo fbo(width, height);
174 
175  glBindTexture(GL_TEXTURE_2D, output_texture);
176 
177  auto format = tex.get_profile().format();
178  auto textype = rs_format_to_gl_format(format);
179  glTexImage2D(GL_TEXTURE_2D, 0, textype.internal_format,
180  width, height, 0, textype.gl_format, textype.data_type, nullptr);
181 
186 
189 
190  glBindTexture(GL_TEXTURE_2D, output_texture);
191  fbo.createTextureAttachment(output_texture);
192 
193  fbo.bind();
194  glClearColor(0, 0, 0, 1);
196 
197  matrix4 projection;
198  int viewport[4];
199  build_opengl_projection_for_intrinsics(projection, viewport,
200  intr.fx, intr.fy, 0.0, intr.width - intr.ppx, intr.height - intr.ppy, width, height, 0.001, 100);
201  projection(3, 2) *= -1.f;
202  projection(2, 3) *= -1.f;
203  projection(2, 2) *= -1.f;
204  float pm[16];
205  projection.to_column_major(pm);
206  _renderer->set_matrix(RS2_GL_MATRIX_PROJECTION, pm);
208  view(2, 2) = -1.f;
209  _renderer->set_matrix(RS2_GL_MATRIX_CAMERA, (float*)&view.mat);
210 
211  matrix4 other = matrix4::identity();
212  for (int i = 0; i < 3; i++)
213  {
214  for (int j = 0; j < 3; j++)
215  other(i,j) = extr.rotation[i*3 + j];
216  other(3, i) = extr.translation[i];
217  }
218  _renderer->set_matrix(RS2_GL_MATRIX_TRANSFORMATION, (float*)&other.mat);
219 
221  _renderer->process(model);
222 
223  fbo.unbind();
224 
226  }, [&] {
227  _enabled = false;
228  });
229 }
230 
231 void align_gl::align_other_to_z(rs2::video_frame& aligned,
232  const rs2::video_frame& depth,
233  const rs2::video_frame& other,
234  float z_scale)
235 {
236  auto width = aligned.get_width();
237  auto height = aligned.get_height();
238 
239  _pc->map_to(other);
240  auto p = _pc->calculate(depth);
241 
242  auto frame_ref = (frame_interface*)aligned.get();
243  auto gf = dynamic_cast<gpu_addon_interface*>(frame_ref);
244  if (!gf)
245  throw std::runtime_error("Frame interface is not gpu addon interface");
246 
247  uint32_t output_rgb;
248  auto format = other.get_profile().format();
249  auto tex_type = rs_format_to_gl_format(format);
250 
251  gf->get_gpu_section().output_texture(0, &output_rgb, tex_type.type);
252  glTexImage2D(GL_TEXTURE_2D, 0, tex_type.internal_format,
253  width, height, 0, tex_type.gl_format, tex_type.data_type, nullptr);
254 
255  gf->get_gpu_section().set_size(width, height);
256 
257  auto prof = depth.get_profile();
258  auto intr = prof.as<video_stream_profile>().get_intrinsics();
259  auto extr = prof.get_extrinsics_to(prof);
260  render(p, other, intr, extr, output_rgb);
261 }
262 
263 align_gl::align_gl(rs2_stream to_stream) : align(to_stream, "Align (GLSL)")
264 {
267 
268  auto opt = std::make_shared<librealsense::ptr_option<int>>(
269  0, 1, 0, 1, &_enabled, "GLSL enabled");
271 
272  initialize();
273 }
274 
276 {
277  try
278  {
279  perform_gl_action( [&]()
280  {
282  }, [] {} );
283  }
284  catch(...)
285  {
286  LOG_DEBUG( "Error while cleaning up gpu resources" );
287  }
288 }
format
GLint GLint GLsizei GLint GLenum format
Definition: glad/glad/glad.h:1412
librealsense::gl::gpu_addon_interface
Definition: synthetic-stream-gl.h:380
librealsense::gl::align_gl::cleanup_gpu_resources
void cleanup_gpu_resources() override
Definition: align-gl.cpp:35
GL_COLOR_BUFFER_BIT
#define GL_COLOR_BUFFER_BIT
Definition: glad/glad/glad.h:144
rs2::stream_profile::format
rs2_format format() const
Definition: rs_frame.hpp:44
rs2::gl::gpu_frame
Definition: rs_processing_gl.hpp:93
rs2::matrix4::identity
static matrix4 identity()
Definition: matrix4.h:25
rs2::frame
Definition: rs_frame.hpp:345
rs2_extrinsics::translation
float translation[3]
Definition: rs_sensor.h:105
rs-imu-calibration.input
input
Definition: rs-imu-calibration.py:35
rs2_extrinsics
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented.
Definition: rs_sensor.h:102
RS2_GL_MATRIX_TRANSFORMATION
@ RS2_GL_MATRIX_TRANSFORMATION
Definition: rs_processing_gl.h:35
librealsense::gl::gpu_processing_object::perform_gl_action
void perform_gl_action(T action, S fallback)
Definition: synthetic-stream-gl.h:320
GL_TEXTURE_WRAP_T
#define GL_TEXTURE_WRAP_T
Definition: glad/glad/glad.h:313
librealsense::gl::align_gl::~align_gl
~align_gl() override
Definition: align-gl.cpp:275
synthetic-stream-gl.h
rs2_extension
rs2_extension
Specifies advanced interfaces (capabilities) objects may implement.
Definition: rs_types.h:134
rs2_intrinsics
Video stream intrinsics.
Definition: rs_types.h:58
librealsense::gl::gpu_video_frame
Definition: synthetic-stream-gl.h:421
librealsense::gl::gpu_processing_object::initialize
void initialize()
Definition: synthetic-stream-gl.h:307
librealsense::gl::rs_format_to_gl_format
texture_mapping & rs_format_to_gl_format(rs2_format type)
Definition: synthetic-stream-gl.cpp:42
texture
The texture class.
Definition: example.hpp:417
librealsense::video_stream_profile
Definition: src/stream.h:108
librealsense::options_container::register_option
void register_option(rs2_option id, std::shared_ptr< option > option)
Definition: options-container.h:50
glTexImage2D
#define glTexImage2D
Definition: glad/glad/glad.h:1417
LOG_DEBUG
#define LOG_DEBUG(...)
Definition: easyloggingpp.h:70
align-gl.h
frustum
matrix4 frustum(float left, float right, float bottom, float top, float zNear, float zFar, float ox, float oy)
Definition: pc-shader.cpp:206
glTexParameteri
#define glTexParameteri
Definition: glad/glad/glad.h:1408
rendering.h
librealsense::frame_interface
Definition: frame-interface.h:20
GL_NEAREST
#define GL_NEAREST
Definition: glad/glad/glad.h:304
librealsense::gl::align_gl::_enabled
int _enabled
Definition: align-gl.h:57
librealsense::processing_block::_source
frame_source _source
Definition: synthetic-stream.h:65
width
GLint GLsizei width
Definition: glad/glad/glad.h:1397
beta
GLfloat beta
Definition: wave.c:31
rs2::fbo::createTextureAttachment
void createTextureAttachment(uint32_t texture)
Definition: opengl3.cpp:615
glDrawBuffer
#define glDrawBuffer
Definition: glad/glad/glad.h:1420
GL_TEXTURE_2D
#define GL_TEXTURE_2D
Definition: glad/glad/glad.h:249
RS2_GL_MATRIX_PROJECTION
@ RS2_GL_MATRIX_PROJECTION
Definition: rs_processing_gl.h:36
uint32_t
unsigned int uint32_t
Definition: stdint.h:80
extr
rs2_extrinsics extr
Definition: test-pose.cpp:249
rs2::video_stream_profile
Definition: rs_frame.hpp:201
rs2::matrix4::mat
float mat[4][4]
Definition: matrix4.h:18
height
GLint GLsizei GLsizei height
Definition: glad/glad/glad.h:1397
GL_COLOR_ATTACHMENT0
#define GL_COLOR_ATTACHMENT0
Definition: glad/glad/glad.h:1159
rs2::points
Definition: rs_frame.hpp:739
rs2::frame::get
rs2_frame * get() const
Definition: rs_frame.hpp:592
rs_sensor.hpp
GL_FRAMEBUFFER
#define GL_FRAMEBUFFER
Definition: glad/glad/glad.h:1193
i
int i
Definition: rs-pcl-color.cpp:54
librealsense::frame_source::add_extension
void add_extension(rs2_extension ex)
Definition: source.h:50
rs2_extrinsics::rotation
float rotation[9]
Definition: rs_sensor.h:104
j
GLint j
Definition: glad/glad/glad.h:2165
rs2
Definition: animated.h:9
librealsense::frame_holder
Definition: frame-holder.h:15
glClearColor
#define glClearColor
Definition: glad/glad/glad.h:1426
rs2::video_frame::get_width
int get_width() const
Definition: rs_frame.hpp:661
v0
GLfloat v0
Definition: glad/glad/glad.h:2867
librealsense::depth_frame
Definition: depth-frame.h:17
rs2::fbo::bind
void bind()
Definition: opengl3.cpp:643
rs2::matrix4::to_column_major
void to_column_major(float column_major[16])
Definition: matrix4.h:135
texture
GLuint texture
Definition: glad/glad/glad.h:2331
opencv_pointcloud_viewer.view
def view(v)
Definition: opencv_pointcloud_viewer.py:168
RS2_EXTENSION_VIDEO_FRAME_GL
#define RS2_EXTENSION_VIDEO_FRAME_GL
Definition: synthetic-stream-gl.h:23
alpha
GLfloat GLfloat GLfloat alpha
Definition: glad/glad/glad.h:1424
pyglet_pointcloud_viewer.other_profile
other_profile
Definition: pyglet_pointcloud_viewer.py:156
rs_processing_gl.hpp
glBindTexture
#define glBindTexture
Definition: glad/glad/glad.h:2333
rs_processing.hpp
glBindFramebuffer
#define glBindFramebuffer
Definition: glad/glad/glad.h:3261
rs2::fbo::get
uint32_t get() const
Definition: opengl3.h:325
test-unit-transform.model
model
Definition: test-unit-transform.py:27
rs2::fbo::unbind
void unbind()
Definition: opengl3.cpp:655
GL_CLAMP_TO_EDGE
#define GL_CLAMP_TO_EDGE
Definition: glad/glad/glad.h:696
rs2::depth_frame
Definition: rs_frame.hpp:813
p
double p[GRIDW][GRIDH]
Definition: wave.c:109
rs2::frame::as
T as() const
Definition: rs_frame.hpp:582
rs2::matrix4
Definition: matrix4.h:16
rs2::frame::get_profile
stream_profile get_profile() const
Definition: rs_frame.hpp:559
RS2_OPTION_COUNT
@ RS2_OPTION_COUNT
Definition: rs_option.h:127
GL_TEXTURE_MIN_FILTER
#define GL_TEXTURE_MIN_FILTER
Definition: glad/glad/glad.h:311
librealsense::align
Definition: align.h:14
glClear
#define glClear
Definition: glad/glad/glad.h:1423
rs2::video_frame
Definition: rs_frame.hpp:638
option.h
build_opengl_projection_for_intrinsics
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:103
rs2::fbo
Definition: opengl3.h:302
RS2_GL_MATRIX_CAMERA
@ RS2_GL_MATRIX_CAMERA
Definition: rs_processing_gl.h:37
librealsense::gl::gpu_depth_frame
Definition: synthetic-stream-gl.h:423
convert_to_bag.intr
intr
Definition: convert_to_bag.py:12
rs2_stream
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:43
GL_TEXTURE_MAG_FILTER
#define GL_TEXTURE_MAG_FILTER
Definition: glad/glad/glad.h:310
rs2::video_frame::get_height
int get_height() const
Definition: rs_frame.hpp:673
librealsense::gl
Definition: align-gl.h:26
depth
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glad/glad/glad.h:2398
RS2_EXTENSION_DEPTH_FRAME_GL
#define RS2_EXTENSION_DEPTH_FRAME_GL
Definition: synthetic-stream-gl.h:24
sw.h
int h
Definition: sw-dev/sw.py:11
GL_TEXTURE_WRAP_S
#define GL_TEXTURE_WRAP_S
Definition: glad/glad/glad.h:312


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:00