pyglet_pointcloud_viewer.py
Go to the documentation of this file.
1 # License: Apache 2.0. See LICENSE file in root directory.
2 # Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
3
4 """
5 OpenGL Pointcloud viewer with http://pyglet.org
6
7 Usage:
8 ------
9 Mouse:
10  Drag with left button to rotate around pivot (thick small axes),
11  with right button to translate and the wheel to zoom.
12
13 Keyboard:
14  [p] Pause
15  [r] Reset View
16  [d] Cycle through decimation values
17  [z] Toggle point scaling
18  [x] Toggle point distance attenuation
19  [c] Toggle color source
20  [l] Toggle lighting
21  [f] Toggle depth post-processing
22  [s] Save PNG (./out.png)
23  [e] Export points to ply (./out.ply)
24  [q/ESC] Quit
25
26 Notes:
27 ------
28 Using deprecated OpenGL (FFP lighting, matrix stack...) however, draw calls
29 are kept low with pyglet.graphics.* which uses glDrawArrays internally.
30
31 Normals calculation is done with numpy on CPU which is rather slow, should really
32 be done with shaders but was omitted for several reasons - brevity, for lowering
33 dependencies (pyglet doesn't ship with shader support & recommends pyshaders)
34 and for reference.
35 """
36
37 import math
38 import ctypes
39 import pyglet
40 import pyglet.gl as gl
41 import numpy as np
42 import pyrealsense2 as rs
43
44
45 # https://stackoverflow.com/a/6802723
46 def rotation_matrix(axis, theta):
47  """
48  Return the rotation matrix associated with counterclockwise rotation about
49  the given axis by theta radians.
50  """
51  axis = np.asarray(axis)
52  axis = axis / math.sqrt(np.dot(axis, axis))
53  a = math.cos(theta / 2.0)
54  b, c, d = -axis * math.sin(theta / 2.0)
55  aa, bb, cc, dd = a * a, b * b, c * c, d * d
56  bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
57  return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
58  [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
59  [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])
60
61
62 class AppState:
63
64  def __init__(self, *args, **kwargs):
65  self.pitch, self.yaw = math.radians(-10), math.radians(-15)
66  self.translation = np.array([0, 0, 1], np.float32)
67  self.distance = 2
68  self.mouse_btns = [False, False, False]
69  self.paused = False
70  self.decimate = 0
71  self.scale = True
72  self.attenuation = False
73  self.color = True
74  self.lighting = False
75  self.postprocessing = False
76
77  def reset(self):
78  self.pitch, self.yaw, self.distance = 0, 0, 2
79  self.translation[:] = 0, 0, 1
80
81  @property
82  def rotation(self):
83  Rx = rotation_matrix((1, 0, 0), math.radians(-self.pitch))
84  Ry = rotation_matrix((0, 1, 0), math.radians(-self.yaw))
85  return np.dot(Ry, Rx).astype(np.float32)
86
87
88 state = AppState()
89
90 # Configure streams
91 pipeline = rs.pipeline()
92 config = rs.config()
93
94 config.enable_stream(rs.stream.depth, rs.format.z16, 30)
95 other_stream, other_format = rs.stream.color, rs.format.rgb8
96 config.enable_stream(other_stream, other_format, 30)
97
98 # Start streaming
99 pipeline.start(config)
100 profile = pipeline.get_active_profile()
101
102 depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
103 depth_intrinsics = depth_profile.get_intrinsics()
104 w, h = depth_intrinsics.width, depth_intrinsics.height
105
106 # Processing blocks
107 pc = rs.pointcloud()
108 decimate = rs.decimation_filter()
109 decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
110 colorizer = rs.colorizer()
111 filters = [rs.disparity_transform(),
112  rs.spatial_filter(),
113  rs.temporal_filter(),
114  rs.disparity_transform(False)]
115
116
117 # pyglet
118 window = pyglet.window.Window(
119  config=gl.Config(
120  double_buffer=True,
121  samples=8 # MSAA
122  ),
123  resizable=True, vsync=True)
124 keys = pyglet.window.key.KeyStateHandler()
125 window.push_handlers(keys)
126
127
128 def convert_fmt(fmt):
129  """rs.format to pyglet format string"""
130  return {
131  rs.format.rgb8: 'RGB',
132  rs.format.bgr8: 'BGR',
133  rs.format.rgba8: 'RGBA',
134  rs.format.bgra8: 'BGRA',
135  rs.format.y8: 'L',
136  }[fmt]
137
138
139 # Create a VertexList to hold pointcloud data
140 # Will pre-allocates memory according to the attributes below
141 vertex_list = pyglet.graphics.vertex_list(
142  w * h, 'v3f/stream', 't2f/stream', 'n3f/stream')
143 # Create and allocate memory for our color data
144 other_profile = rs.video_stream_profile(profile.get_stream(other_stream))
145
146 image_w, image_h = w, h
147 color_intrinsics = other_profile.get_intrinsics()
148 color_w, color_h = color_intrinsics.width, color_intrinsics.height
149
150 if state.color:
151  image_w, image_h = color_w, color_h
152
153 image_data = pyglet.image.ImageData(image_w, image_h, convert_fmt(
154 other_profile.format()), (gl.GLubyte * (image_w * image_h * 3))())
155
156 if (pyglet.version.startswith('1.') and not pyglet.version.startswith('1.4')):
157  # pyglet.clock.ClockDisplay has be removed in 1.4
158  fps_display = pyglet.clock.ClockDisplay()
159 else:
160  fps_display = pyglet.window.FPSDisplay(window)
161
162
163 @window.event
164 def on_mouse_drag(x, y, dx, dy, buttons, modifiers):
165  w, h = map(float, window.get_size())
166
167  if buttons & pyglet.window.mouse.LEFT:
168  state.yaw -= dx * 0.5
169  state.pitch -= dy * 0.5
170
171  if buttons & pyglet.window.mouse.RIGHT:
172  dp = np.array((dx / w, -dy / h, 0), np.float32)
173  state.translation += np.dot(state.rotation, dp)
174
175  if buttons & pyglet.window.mouse.MIDDLE:
176  dz = dy * 0.01
177  state.translation -= (0, 0, dz)
178  state.distance -= dz
179
180
181 def handle_mouse_btns(x, y, button, modifiers):
182  state.mouse_btns[0] ^= (button & pyglet.window.mouse.LEFT)
183  state.mouse_btns[1] ^= (button & pyglet.window.mouse.RIGHT)
184  state.mouse_btns[2] ^= (button & pyglet.window.mouse.MIDDLE)
185
186
187 window.on_mouse_press = window.on_mouse_release = handle_mouse_btns
188
189
190 @window.event
191 def on_mouse_scroll(x, y, scroll_x, scroll_y):
192  dz = scroll_y * 0.1
193  state.translation -= (0, 0, dz)
194  state.distance -= dz
195
196
197 def on_key_press(symbol, modifiers):
198  if symbol == pyglet.window.key.R:
199  state.reset()
200
201  if symbol == pyglet.window.key.P:
202  state.paused ^= True
203
204  if symbol == pyglet.window.key.D:
205  state.decimate = (state.decimate + 1) % 3
206  decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
207
208  if symbol == pyglet.window.key.C:
209  state.color ^= True
210
211  if symbol == pyglet.window.key.Z:
212  state.scale ^= True
213
214  if symbol == pyglet.window.key.X:
215  state.attenuation ^= True
216
217  if symbol == pyglet.window.key.L:
218  state.lighting ^= True
219
220  if symbol == pyglet.window.key.F:
221  state.postprocessing ^= True
222
223  if symbol == pyglet.window.key.S:
224  pyglet.image.get_buffer_manager().get_color_buffer().save('out.png')
225
226  if symbol == pyglet.window.key.Q:
227  window.close()
228
229
230 window.push_handlers(on_key_press)
231
232
233 def axes(size=1, width=1):
234  """draw 3d axes"""
235  gl.glLineWidth(width)
236  pyglet.graphics.draw(6, gl.GL_LINES,
237  ('v3f', (0, 0, 0, size, 0, 0,
238  0, 0, 0, 0, size, 0,
239  0, 0, 0, 0, 0, size)),
240  ('c3f', (1, 0, 0, 1, 0, 0,
241  0, 1, 0, 0, 1, 0,
242  0, 0, 1, 0, 0, 1,
243  ))
244  )
245
246
247 def frustum(intrinsics):
248  """draw camera's frustum"""
249  w, h = intrinsics.width, intrinsics.height
250  batch = pyglet.graphics.Batch()
251
252  for d in range(1, 6, 2):
253  def get_point(x, y):
254  p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
255  batch.add(2, gl.GL_LINES, None, ('v3f', [0, 0, 0] + p))
256  return p
257
258  top_left = get_point(0, 0)
259  top_right = get_point(w, 0)
260  bottom_right = get_point(w, h)
261  bottom_left = get_point(0, h)
262
263  batch.add(2, gl.GL_LINES, None, ('v3f', top_left + top_right))
264  batch.add(2, gl.GL_LINES, None, ('v3f', top_right + bottom_right))
265  batch.add(2, gl.GL_LINES, None, ('v3f', bottom_right + bottom_left))
266  batch.add(2, gl.GL_LINES, None, ('v3f', bottom_left + top_left))
267
268  batch.draw()
269
270
271 def grid(size=1, n=10, width=1):
272  """draw a grid on xz plane"""
273  gl.glLineWidth(width)
274  s = size / float(n)
275  s2 = 0.5 * size
276  batch = pyglet.graphics.Batch()
277
278  for i in range(0, n + 1):
279  x = -s2 + i * s
280  batch.add(2, gl.GL_LINES, None, ('v3f', (x, 0, -s2, x, 0, s2)))
281  for i in range(0, n + 1):
282  z = -s2 + i * s
283  batch.add(2, gl.GL_LINES, None, ('v3f', (-s2, 0, z, s2, 0, z)))
284
285  batch.draw()
286
287
288 @window.event
289 def on_draw():
290  window.clear()
291
292  gl.glEnable(gl.GL_DEPTH_TEST)
293  gl.glEnable(gl.GL_LINE_SMOOTH)
294
295  width, height = window.get_size()
296  gl.glViewport(0, 0, width, height)
297
298  gl.glMatrixMode(gl.GL_PROJECTION)
299  gl.glLoadIdentity()
300  gl.gluPerspective(60, width / float(height), 0.01, 20)
301
302  gl.glMatrixMode(gl.GL_TEXTURE)
303  gl.glLoadIdentity()
304  # texcoords are [0..1] and relative to top-left pixel corner, add 0.5 to center
305  gl.glTranslatef(0.5 / image_data.width, 0.5 / image_data.height, 0)
306  image_texture = image_data.get_texture()
307  # texture size may be increased by pyglet to a power of 2
308  tw, th = image_texture.owner.width, image_texture.owner.height
309  gl.glScalef(image_data.width / float(tw),
310  image_data.height / float(th), 1)
311
312  gl.glMatrixMode(gl.GL_MODELVIEW)
313  gl.glLoadIdentity()
314
315  gl.gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0)
316
317  gl.glTranslatef(0, 0, state.distance)
318  gl.glRotated(state.pitch, 1, 0, 0)
319  gl.glRotated(state.yaw, 0, 1, 0)
320
321  if any(state.mouse_btns):
322  axes(0.1, 4)
323
324  gl.glTranslatef(0, 0, -state.distance)
325  gl.glTranslatef(*state.translation)
326
327  gl.glColor3f(0.5, 0.5, 0.5)
328  gl.glPushMatrix()
329  gl.glTranslatef(0, 0.5, 0.5)
330  grid()
331  gl.glPopMatrix()
332
333  psz = max(window.get_size()) / float(max(w, h)) if state.scale else 1
334  gl.glPointSize(psz)
335  distance = (0, 0, 1) if state.attenuation else (1, 0, 0)
336  gl.glPointParameterfv(gl.GL_POINT_DISTANCE_ATTENUATION,
337  (gl.GLfloat * 3)(*distance))
338
339  if state.lighting:
340  ldir = [0.5, 0.5, 0.5] # world-space lighting
341  ldir = np.dot(state.rotation, (0, 0, 1)) # MeshLab style lighting
342  ldir = list(ldir) + [0] # w=0, directional light
343  gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, (gl.GLfloat * 4)(*ldir))
344  gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE,
345  (gl.GLfloat * 3)(1.0, 1.0, 1.0))
346  gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT,
347  (gl.GLfloat * 3)(0.75, 0.75, 0.75))
348  gl.glEnable(gl.GL_LIGHT0)
349  gl.glEnable(gl.GL_NORMALIZE)
350  gl.glEnable(gl.GL_LIGHTING)
351
352  gl.glColor3f(1, 1, 1)
353  texture = image_data.get_texture()
354  gl.glEnable(texture.target)
355  gl.glBindTexture(texture.target, texture.id)
356  gl.glTexParameteri(
357  gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
358
359  # comment this to get round points with MSAA on
360  gl.glEnable(gl.GL_POINT_SPRITE)
361
362  if not state.scale and not state.attenuation:
363  gl.glDisable(gl.GL_MULTISAMPLE) # for true 1px points with MSAA on
364  vertex_list.draw(gl.GL_POINTS)
365  gl.glDisable(texture.target)
366  if not state.scale and not state.attenuation:
367  gl.glEnable(gl.GL_MULTISAMPLE)
368
369  gl.glDisable(gl.GL_LIGHTING)
370
371  gl.glColor3f(0.25, 0.25, 0.25)
372  frustum(depth_intrinsics)
373  axes()
374
375  gl.glMatrixMode(gl.GL_PROJECTION)
376  gl.glLoadIdentity()
377  gl.glOrtho(0, width, 0, height, -1, 1)
378  gl.glMatrixMode(gl.GL_MODELVIEW)
379  gl.glLoadIdentity()
380  gl.glMatrixMode(gl.GL_TEXTURE)
381  gl.glLoadIdentity()
382  gl.glDisable(gl.GL_DEPTH_TEST)
383
384  fps_display.draw()
385
386
387 def run(dt):
388  global w, h
389  window.set_caption("RealSense (%dx%d) %dFPS (%.2fms) %s" %
390  (w, h, 0 if dt == 0 else 1.0 / dt, dt * 1000,
391  "PAUSED" if state.paused else ""))
392
393  if state.paused:
394  return
395
396  success, frames = pipeline.try_wait_for_frames(timeout_ms=0)
397  if not success:
398  return
399
400  depth_frame = frames.get_depth_frame().as_video_frame()
401  other_frame = frames.first(other_stream).as_video_frame()
402
403  depth_frame = decimate.process(depth_frame)
404
405  if state.postprocessing:
406  for f in filters:
407  depth_frame = f.process(depth_frame)
408
409  # Grab new intrinsics (may be changed by decimation)
410  depth_intrinsics = rs.video_stream_profile(
411  depth_frame.profile).get_intrinsics()
412  w, h = depth_intrinsics.width, depth_intrinsics.height
413
414  color_image = np.asanyarray(other_frame.get_data())
415
416  colorized_depth = colorizer.colorize(depth_frame)
417  depth_colormap = np.asanyarray(colorized_depth.get_data())
418
419  if state.color:
420  mapped_frame, color_source = other_frame, color_image
421  else:
422  mapped_frame, color_source = colorized_depth, depth_colormap
423
424  points = pc.calculate(depth_frame)
425  pc.map_to(mapped_frame)
426
427  # handle color source or size change
428  fmt = convert_fmt(mapped_frame.profile.format())
429  global image_data
430
431  if (image_data.format, image_data.pitch) != (fmt, color_source.strides[0]):
432  if state.color:
433  global color_w, color_h
434  image_w, image_h = color_w, color_h
435  else:
436  image_w, image_h = w, h
437
438  empty = (gl.GLubyte * (image_w * image_h * 3))()
439  image_data = pyglet.image.ImageData(image_w, image_h, fmt, empty)
440
441  # copy image data to pyglet
442  image_data.set_data(fmt, color_source.strides[0], color_source.ctypes.data)
443
444  verts = np.asarray(points.get_vertices(2)).reshape(h, w, 3)
445  texcoords = np.asarray(points.get_texture_coordinates(2))
446
447  if len(vertex_list.vertices) != verts.size:
448  vertex_list.resize(verts.size // 3)
449  # need to reassign after resizing
450  vertex_list.vertices = verts.ravel()
451  vertex_list.tex_coords = texcoords.ravel()
452
453  # copy our data to pre-allocated buffers, this is faster than assigning...
454  # pyglet will take care of uploading to GPU
455  def copy(dst, src):
456  """copy numpy array to pyglet array"""
457  # timeit was mostly inconclusive, favoring slice assignment for safety
458  np.array(dst, copy=False)[:] = src.ravel()
459  # ctypes.memmove(dst, src.ctypes.data, src.nbytes)
460
461  copy(vertex_list.vertices, verts)
462  copy(vertex_list.tex_coords, texcoords)
463
464  if state.lighting:
465  # compute normals
466  dy, dx = np.gradient(verts, axis=(0, 1))
467  n = np.cross(dx, dy)
468
469  # can use this, np.linalg.norm or similar to normalize, but OpenGL can do this for us, see GL_NORMALIZE above
470  # norm = np.sqrt((n*n).sum(axis=2, keepdims=True))
471  # np.divide(n, norm, out=n, where=norm != 0)
472
473  # import cv2
474  # n = cv2.bilateralFilter(n, 5, 1, 1)
475
476  copy(vertex_list.normals, n)
477
478  if keys[pyglet.window.key.E]:
479  points.export_to_ply('./out.ply', mapped_frame)
480
481
482 pyglet.clock.schedule(run)
483
484 try:
485  pyglet.app.run()
486 finally:
487  pipeline.stop()
def on_key_press(symbol, modifiers)
def handle_mouse_btns(x, y, button, modifiers)
void reshape(GLFWwindow *window, int w, int h)
Definition: boing.c:215
def on_mouse_scroll(x, y, scroll_x, scroll_y)
def on_mouse_drag(x, y, dx, dy, buttons, modifiers)
def grid(size=1, n=10, width=1)
void close()
Definition: example.hpp:622
GeneratorWrapper< T > map(Func &&function, GeneratorWrapper< U > &&generator)
Definition: catch.hpp:4271
void copy(void *dst, void const *src, size_t size)
Definition: types.cpp:836

librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Sat Apr 3 2021 02:48:23