opencv_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 OpenCV and Numpy Point cloud Software Renderer
6 
7 This sample is mostly for demonstration and educational purposes.
8 It really doesn't offer the quality or performance that can be
9 achieved with hardware acceleration.
10 
11 Usage:
12 ------
13 Mouse:
14  Drag with left button to rotate around pivot (thick small axes),
15  with right button to translate and the wheel to zoom.
16 
17 Keyboard:
18  [p] Pause
19  [r] Reset View
20  [d] Cycle through decimation values
21  [z] Toggle point scaling
22  [c] Toggle color source
23  [s] Save PNG (./out.png)
24  [e] Export points to ply (./out.ply)
25  [q\ESC] Quit
26 """
27 
28 import math
29 import time
30 import cv2
31 import numpy as np
32 import pyrealsense2 as rs
33 
34 
35 class AppState:
36 
37  def __init__(self, *args, **kwargs):
38  self.WIN_NAME = 'RealSense'
39  self.pitch, self.yaw = math.radians(-10), math.radians(-15)
40  self.translation = np.array([0, 0, -1], dtype=np.float32)
41  self.distance = 2
42  self.prev_mouse = 0, 0
43  self.mouse_btns = [False, False, False]
44  self.paused = False
45  self.decimate = 1
46  self.scale = True
47  self.color = True
48 
49  def reset(self):
50  self.pitch, self.yaw, self.distance = 0, 0, 2
51  self.translation[:] = 0, 0, -1
52 
53  @property
54  def rotation(self):
55  Rx, _ = cv2.Rodrigues((self.pitch, 0, 0))
56  Ry, _ = cv2.Rodrigues((0, self.yaw, 0))
57  return np.dot(Ry, Rx).astype(np.float32)
58 
59  @property
60  def pivot(self):
61  return self.translation + np.array((0, 0, self.distance), dtype=np.float32)
62 
63 
64 state = AppState()
65 
66 # Configure depth and color streams
67 pipeline = rs.pipeline()
68 config = rs.config()
69 
70 config.enable_stream(rs.stream.depth, rs.format.z16, 30)
71 config.enable_stream(rs.stream.color, rs.format.bgr8, 30)
72 
73 # Start streaming
74 pipeline.start(config)
75 
76 # Get stream profile and camera intrinsics
77 profile = pipeline.get_active_profile()
78 depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
79 depth_intrinsics = depth_profile.get_intrinsics()
80 w, h = depth_intrinsics.width, depth_intrinsics.height
81 
82 # Processing blocks
83 pc = rs.pointcloud()
84 decimate = rs.decimation_filter()
85 decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
86 colorizer = rs.colorizer()
87 
88 
89 def mouse_cb(event, x, y, flags, param):
90 
91  if event == cv2.EVENT_LBUTTONDOWN:
92  state.mouse_btns[0] = True
93 
94  if event == cv2.EVENT_LBUTTONUP:
95  state.mouse_btns[0] = False
96 
97  if event == cv2.EVENT_RBUTTONDOWN:
98  state.mouse_btns[1] = True
99 
100  if event == cv2.EVENT_RBUTTONUP:
101  state.mouse_btns[1] = False
102 
103  if event == cv2.EVENT_MBUTTONDOWN:
104  state.mouse_btns[2] = True
105 
106  if event == cv2.EVENT_MBUTTONUP:
107  state.mouse_btns[2] = False
108 
109  if event == cv2.EVENT_MOUSEMOVE:
110 
111  h, w = out.shape[:2]
112  dx, dy = x - state.prev_mouse[0], y - state.prev_mouse[1]
113 
114  if state.mouse_btns[0]:
115  state.yaw += float(dx) / w * 2
116  state.pitch -= float(dy) / h * 2
117 
118  elif state.mouse_btns[1]:
119  dp = np.array((dx / w, dy / h, 0), dtype=np.float32)
120  state.translation -= np.dot(state.rotation, dp)
121 
122  elif state.mouse_btns[2]:
123  dz = math.sqrt(dx**2 + dy**2) * math.copysign(0.01, -dy)
124  state.translation[2] += dz
125  state.distance -= dz
126 
127  if event == cv2.EVENT_MOUSEWHEEL:
128  dz = math.copysign(0.1, flags)
129  state.translation[2] += dz
130  state.distance -= dz
131 
132  state.prev_mouse = (x, y)
133 
134 
135 cv2.namedWindow(state.WIN_NAME, cv2.WINDOW_AUTOSIZE)
136 cv2.resizeWindow(state.WIN_NAME, w, h)
137 cv2.setMouseCallback(state.WIN_NAME, mouse_cb)
138 
139 
140 def project(v):
141  """project 3d vector array to 2d"""
142  h, w = out.shape[:2]
143  view_aspect = float(h)/w
144 
145  # ignore divide by zero for invalid depth
146  with np.errstate(divide='ignore', invalid='ignore'):
147  proj = v[:, :-1] / v[:, -1, np.newaxis] * \
148  (w*view_aspect, h) + (w/2.0, h/2.0)
149 
150  # near clipping
151  znear = 0.03
152  proj[v[:, 2] < znear] = np.nan
153  return proj
154 
155 
156 def view(v):
157  """apply view transformation on vector array"""
158  return np.dot(v - state.pivot, state.rotation) + state.pivot - state.translation
159 
160 
161 def line3d(out, pt1, pt2, color=(0x80, 0x80, 0x80), thickness=1):
162  """draw a 3d line from pt1 to pt2"""
163  p0 = project(pt1.reshape(-1, 3))[0]
164  p1 = project(pt2.reshape(-1, 3))[0]
165  if np.isnan(p0).any() or np.isnan(p1).any():
166  return
167  p0 = tuple(p0.astype(int))
168  p1 = tuple(p1.astype(int))
169  rect = (0, 0, out.shape[1], out.shape[0])
170  inside, p0, p1 = cv2.clipLine(rect, p0, p1)
171  if inside:
172  cv2.line(out, p0, p1, color, thickness, cv2.LINE_AA)
173 
174 
175 def grid(out, pos, rotation=np.eye(3), size=1, n=10, color=(0x80, 0x80, 0x80)):
176  """draw a grid on xz plane"""
177  pos = np.array(pos)
178  s = size / float(n)
179  s2 = 0.5 * size
180  for i in range(0, n+1):
181  x = -s2 + i*s
182  line3d(out, view(pos + np.dot((x, 0, -s2), rotation)),
183  view(pos + np.dot((x, 0, s2), rotation)), color)
184  for i in range(0, n+1):
185  z = -s2 + i*s
186  line3d(out, view(pos + np.dot((-s2, 0, z), rotation)),
187  view(pos + np.dot((s2, 0, z), rotation)), color)
188 
189 
190 def axes(out, pos, rotation=np.eye(3), size=0.075, thickness=2):
191  """draw 3d axes"""
192  line3d(out, pos, pos +
193  np.dot((0, 0, size), rotation), (0xff, 0, 0), thickness)
194  line3d(out, pos, pos +
195  np.dot((0, size, 0), rotation), (0, 0xff, 0), thickness)
196  line3d(out, pos, pos +
197  np.dot((size, 0, 0), rotation), (0, 0, 0xff), thickness)
198 
199 
200 def frustum(out, intrinsics, color=(0x40, 0x40, 0x40)):
201  """draw camera's frustum"""
202  orig = view([0, 0, 0])
203  w, h = intrinsics.width, intrinsics.height
204 
205  for d in range(1, 6, 2):
206  def get_point(x, y):
207  p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
208  line3d(out, orig, view(p), color)
209  return p
210 
211  top_left = get_point(0, 0)
212  top_right = get_point(w, 0)
213  bottom_right = get_point(w, h)
214  bottom_left = get_point(0, h)
215 
216  line3d(out, view(top_left), view(top_right), color)
217  line3d(out, view(top_right), view(bottom_right), color)
218  line3d(out, view(bottom_right), view(bottom_left), color)
219  line3d(out, view(bottom_left), view(top_left), color)
220 
221 
222 def pointcloud(out, verts, texcoords, color, painter=True):
223  """draw point cloud with optional painter's algorithm"""
224  if painter:
225  # Painter's algo, sort points from back to front
226 
227  # get reverse sorted indices by z (in view-space)
228  # https://gist.github.com/stevenvo/e3dad127598842459b68
229  v = view(verts)
230  s = v[:, 2].argsort()[::-1]
231  proj = project(v[s])
232  else:
233  proj = project(view(verts))
234 
235  if state.scale:
236  proj *= 0.5**state.decimate
237 
238  h, w = out.shape[:2]
239 
240  # proj now contains 2d image coordinates
241  j, i = proj.astype(np.uint32).T
242 
243  # create a mask to ignore out-of-bound indices
244  im = (i >= 0) & (i < h)
245  jm = (j >= 0) & (j < w)
246  m = im & jm
247 
248  cw, ch = color.shape[:2][::-1]
249  if painter:
250  # sort texcoord with same indices as above
251  # texcoords are [0..1] and relative to top-left pixel corner,
252  # multiply by size and add 0.5 to center
253  v, u = (texcoords[s] * (cw, ch) + 0.5).astype(np.uint32).T
254  else:
255  v, u = (texcoords * (cw, ch) + 0.5).astype(np.uint32).T
256  # clip texcoords to image
257  np.clip(u, 0, ch-1, out=u)
258  np.clip(v, 0, cw-1, out=v)
259 
260  # perform uv-mapping
261  out[i[m], j[m]] = color[u[m], v[m]]
262 
263 
264 out = np.empty((h, w, 3), dtype=np.uint8)
265 
266 while True:
267  # Grab camera data
268  if not state.paused:
269  # Wait for a coherent pair of frames: depth and color
270  frames = pipeline.wait_for_frames()
271 
272  depth_frame = frames.get_depth_frame()
273  color_frame = frames.get_color_frame()
274 
275  depth_frame = decimate.process(depth_frame)
276 
277  # Grab new intrinsics (may be changed by decimation)
278  depth_intrinsics = rs.video_stream_profile(
279  depth_frame.profile).get_intrinsics()
280  w, h = depth_intrinsics.width, depth_intrinsics.height
281 
282  depth_image = np.asanyarray(depth_frame.get_data())
283  color_image = np.asanyarray(color_frame.get_data())
284 
285  depth_colormap = np.asanyarray(
286  colorizer.colorize(depth_frame).get_data())
287 
288  if state.color:
289  mapped_frame, color_source = color_frame, color_image
290  else:
291  mapped_frame, color_source = depth_frame, depth_colormap
292 
293  points = pc.calculate(depth_frame)
294  pc.map_to(mapped_frame)
295 
296  # Pointcloud data to arrays
297  v, t = points.get_vertices(), points.get_texture_coordinates()
298  verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz
299  texcoords = np.asanyarray(t).view(np.float32).reshape(-1, 2) # uv
300 
301  # Render
302  now = time.time()
303 
304  out.fill(0)
305 
306  grid(out, (0, 0.5, 1), size=1, n=10)
307  frustum(out, depth_intrinsics)
308  axes(out, view([0, 0, 0]), state.rotation, size=0.1, thickness=1)
309 
310  if not state.scale or out.shape[:2] == (h, w):
311  pointcloud(out, verts, texcoords, color_source)
312  else:
313  tmp = np.zeros((h, w, 3), dtype=np.uint8)
314  pointcloud(tmp, verts, texcoords, color_source)
315  tmp = cv2.resize(
316  tmp, out.shape[:2][::-1], interpolation=cv2.INTER_NEAREST)
317  np.putmask(out, tmp > 0, tmp)
318 
319  if any(state.mouse_btns):
320  axes(out, view(state.pivot), state.rotation, thickness=4)
321 
322  dt = time.time() - now
323 
324  cv2.setWindowTitle(
325  state.WIN_NAME, "RealSense (%dx%d) %dFPS (%.2fms) %s" %
326  (w, h, 1.0/dt, dt*1000, "PAUSED" if state.paused else ""))
327 
328  cv2.imshow(state.WIN_NAME, out)
329  key = cv2.waitKey(1)
330 
331  if key == ord("r"):
332  state.reset()
333 
334  if key == ord("p"):
335  state.paused ^= True
336 
337  if key == ord("d"):
338  state.decimate = (state.decimate + 1) % 3
339  decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
340 
341  if key == ord("z"):
342  state.scale ^= True
343 
344  if key == ord("c"):
345  state.color ^= True
346 
347  if key == ord("s"):
348  cv2.imwrite('./out.png', out)
349 
350  if key == ord("e"):
351  points.export_to_ply('./out.ply', mapped_frame)
352 
353  if key in (27, ord("q")) or cv2.getWindowProperty(state.WIN_NAME, cv2.WND_PROP_AUTOSIZE) < 0:
354  break
355 
356 # Stop streaming
357 pipeline.stop()
def axes(out, pos, rotation=np.eye(3), size=0.075, thickness=2)
def frustum(out, intrinsics, color=(0x40, 0x40, 0x40))
def line3d(out, pt1, pt2, color=(0x80, 0x80, 0x80), thickness=1)
void reshape(GLFWwindow *window, int w, int h)
Definition: boing.c:215
def pointcloud(out, verts, texcoords, color, painter=True)
def grid(out, pos, rotation=np.eye(3), size=1, n=10, color=(0x80, 0x80, 0x80))
def mouse_cb(event, x, y, flags, param)


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