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