5 OpenGL Pointcloud viewer with http://pyglet.org 10 Drag with left button to rotate around pivot (thick small axes), 11 with right button to translate and the wheel to zoom. 16 [d] Cycle through decimation values 17 [z] Toggle point scaling 18 [x] Toggle point distance attenuation 19 [c] Toggle color source 21 [f] Toggle depth post-processing 22 [s] Save PNG (./out.png) 23 [e] Export points to ply (./out.ply) 28 Using deprecated OpenGL (FFP lighting, matrix stack...) however, draw calls 29 are kept low with pyglet.graphics.* which uses glDrawArrays internally. 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) 40 import pyglet.gl
as gl
42 import pyrealsense2
as rs
48 Return the rotation matrix associated with counterclockwise rotation about 49 the given axis by theta radians. 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]])
65 self.pitch, self.
yaw = math.radians(-10), math.radians(-15)
85 return np.dot(Ry, Rx).astype(np.float32)
91 pipeline = rs.pipeline()
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)
99 pipeline.start(config)
100 profile = pipeline.get_active_profile()
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
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(),
113 rs.temporal_filter(),
114 rs.disparity_transform(
False)]
118 window = pyglet.window.Window(
123 resizable=
True, vsync=
True)
124 keys = pyglet.window.key.KeyStateHandler()
125 window.push_handlers(keys)
129 """rs.format to pyglet format string""" 131 rs.format.rgb8:
'RGB',
132 rs.format.bgr8:
'BGR',
133 rs.format.rgba8:
'RGBA',
134 rs.format.bgra8:
'BGRA',
141 vertex_list = pyglet.graphics.vertex_list(
142 w * h,
'v3f/stream',
't2f/stream',
'n3f/stream')
144 other_profile = rs.video_stream_profile(profile.get_stream(other_stream))
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
151 image_w, image_h = color_w, color_h
153 image_data = pyglet.image.ImageData(image_w, image_h,
convert_fmt(
154 other_profile.format()), (gl.GLubyte * (image_w * image_h * 3))())
156 if (pyglet.version <
'1.4' ):
158 fps_display = pyglet.clock.ClockDisplay()
160 fps_display = pyglet.window.FPSDisplay(window)
165 w, h =
map(float, window.get_size())
167 if buttons & pyglet.window.mouse.LEFT:
168 state.yaw -= dx * 0.5
169 state.pitch -= dy * 0.5
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)
175 if buttons & pyglet.window.mouse.MIDDLE:
177 state.translation -= (0, 0, dz)
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)
187 window.on_mouse_press = window.on_mouse_release = handle_mouse_btns
193 state.translation -= (0, 0, dz)
198 if symbol == pyglet.window.key.R:
201 if symbol == pyglet.window.key.P:
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)
208 if symbol == pyglet.window.key.C:
211 if symbol == pyglet.window.key.Z:
214 if symbol == pyglet.window.key.X:
215 state.attenuation ^=
True 217 if symbol == pyglet.window.key.L:
218 state.lighting ^=
True 220 if symbol == pyglet.window.key.F:
221 state.postprocessing ^=
True 223 if symbol == pyglet.window.key.S:
224 pyglet.image.get_buffer_manager().get_color_buffer().save(
'out.png')
226 if symbol == pyglet.window.key.Q:
230 window.push_handlers(on_key_press)
235 gl.glLineWidth(width)
236 pyglet.graphics.draw(6, gl.GL_LINES,
237 (
'v3f', (0, 0, 0, size, 0, 0,
239 0, 0, 0, 0, 0, size)),
240 (
'c3f', (1, 0, 0, 1, 0, 0,
248 """draw camera's frustum""" 249 w, h = intrinsics.width, intrinsics.height
250 batch = pyglet.graphics.Batch()
252 for d
in range(1, 6, 2):
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))
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)
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))
271 def grid(size=1, n=10, width=1):
272 """draw a grid on xz plane""" 273 gl.glLineWidth(width)
276 batch = pyglet.graphics.Batch()
278 for i
in range(0, n + 1):
280 batch.add(2, gl.GL_LINES,
None, (
'v3f', (x, 0, -s2, x, 0, s2)))
281 for i
in range(0, n + 1):
283 batch.add(2, gl.GL_LINES,
None, (
'v3f', (-s2, 0, z, s2, 0, z)))
292 gl.glEnable(gl.GL_DEPTH_TEST)
293 gl.glEnable(gl.GL_LINE_SMOOTH)
295 width, height = window.get_size()
296 gl.glViewport(0, 0, width, height)
298 gl.glMatrixMode(gl.GL_PROJECTION)
300 gl.gluPerspective(60, width / float(height), 0.01, 20)
302 gl.glMatrixMode(gl.GL_TEXTURE)
305 gl.glTranslatef(0.5 / image_data.width, 0.5 / image_data.height, 0)
306 image_texture = image_data.get_texture()
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)
312 gl.glMatrixMode(gl.GL_MODELVIEW)
315 gl.gluLookAt(0, 0, 0, 0, 0, 1, 0, -1, 0)
317 gl.glTranslatef(0, 0, state.distance)
318 gl.glRotated(state.pitch, 1, 0, 0)
319 gl.glRotated(state.yaw, 0, 1, 0)
321 if any(state.mouse_btns):
324 gl.glTranslatef(0, 0, -state.distance)
325 gl.glTranslatef(*state.translation)
327 gl.glColor3f(0.5, 0.5, 0.5)
329 gl.glTranslatef(0, 0.5, 0.5)
333 psz = max(window.get_size()) / float(max(w, h))
if state.scale
else 1
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))
340 ldir = [0.5, 0.5, 0.5]
341 ldir = np.dot(state.rotation, (0, 0, 1))
342 ldir = list(ldir) + [0]
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)
352 gl.glColor3f(1, 1, 1)
353 texture = image_data.get_texture()
354 gl.glEnable(texture.target)
355 gl.glBindTexture(texture.target, texture.id)
357 gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
360 gl.glEnable(gl.GL_POINT_SPRITE)
362 if not state.scale
and not state.attenuation:
363 gl.glDisable(gl.GL_MULTISAMPLE)
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)
369 gl.glDisable(gl.GL_LIGHTING)
371 gl.glColor3f(0.25, 0.25, 0.25)
375 gl.glMatrixMode(gl.GL_PROJECTION)
377 gl.glOrtho(0, width, 0, height, -1, 1)
378 gl.glMatrixMode(gl.GL_MODELVIEW)
380 gl.glMatrixMode(gl.GL_TEXTURE)
382 gl.glDisable(gl.GL_DEPTH_TEST)
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 ""))
396 success, frames = pipeline.try_wait_for_frames(timeout_ms=0)
400 depth_frame = frames.get_depth_frame().as_video_frame()
401 other_frame = frames.first(other_stream).as_video_frame()
403 depth_frame = decimate.process(depth_frame)
405 if state.postprocessing:
407 depth_frame = f.process(depth_frame)
410 depth_intrinsics = rs.video_stream_profile(
411 depth_frame.profile).get_intrinsics()
412 w, h = depth_intrinsics.width, depth_intrinsics.height
414 color_image = np.asanyarray(other_frame.get_data())
416 colorized_depth = colorizer.colorize(depth_frame)
417 depth_colormap = np.asanyarray(colorized_depth.get_data())
420 mapped_frame, color_source = other_frame, color_image
422 mapped_frame, color_source = colorized_depth, depth_colormap
424 points = pc.calculate(depth_frame)
425 pc.map_to(mapped_frame)
431 if (image_data.format, image_data.pitch) != (fmt, color_source.strides[0]):
433 global color_w, color_h
434 image_w, image_h = color_w, color_h
436 image_w, image_h = w, h
438 empty = (gl.GLubyte * (image_w * image_h * 3))()
439 image_data = pyglet.image.ImageData(image_w, image_h, fmt, empty)
442 image_data.set_data(fmt, color_source.strides[0], color_source.ctypes.data)
444 verts = np.asarray(points.get_vertices(2)).
reshape(h, w, 3)
445 texcoords = np.asarray(points.get_texture_coordinates(2))
447 if len(vertex_list.vertices) != verts.size:
448 vertex_list.resize(verts.size // 3)
450 vertex_list.vertices = verts.ravel()
451 vertex_list.tex_coords = texcoords.ravel()
456 """copy numpy array to pyglet array""" 458 np.array(dst, copy=
False)[:] = src.ravel()
461 copy(vertex_list.vertices, verts)
462 copy(vertex_list.tex_coords, texcoords)
466 dy, dx = np.gradient(verts, axis=(0, 1))
476 copy(vertex_list.normals, n)
478 if keys[pyglet.window.key.E]:
479 points.export_to_ply(
'./out.ply', mapped_frame)
482 pyglet.clock.schedule(run)
def on_key_press(symbol, modifiers)
def __init__(self, args, kwargs)
def handle_mouse_btns(x, y, button, modifiers)
void reshape(GLFWwindow *window, int w, int h)
def rotation_matrix(axis, theta)
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)
GeneratorWrapper< T > map(Func &&function, GeneratorWrapper< U > &&generator)
void copy(void *dst, void const *src, size_t size)
def axes(size=1, width=1)