5 OpenCV and Numpy Point cloud Software Renderer 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. 14 Drag with left button to rotate around pivot (thick small axes), 15 with right button to translate and the wheel to zoom. 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) 32 import pyrealsense2
as rs
39 self.pitch, self.
yaw = math.radians(-10), math.radians(-15)
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)
67 pipeline = rs.pipeline()
70 config.enable_stream(rs.stream.depth, rs.format.z16, 30)
71 config.enable_stream(rs.stream.color, rs.format.bgr8, 30)
74 pipeline.start(config)
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
84 decimate = rs.decimation_filter()
85 decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
86 colorizer = rs.colorizer()
91 if event == cv2.EVENT_LBUTTONDOWN:
92 state.mouse_btns[0] =
True 94 if event == cv2.EVENT_LBUTTONUP:
95 state.mouse_btns[0] =
False 97 if event == cv2.EVENT_RBUTTONDOWN:
98 state.mouse_btns[1] =
True 100 if event == cv2.EVENT_RBUTTONUP:
101 state.mouse_btns[1] =
False 103 if event == cv2.EVENT_MBUTTONDOWN:
104 state.mouse_btns[2] =
True 106 if event == cv2.EVENT_MBUTTONUP:
107 state.mouse_btns[2] =
False 109 if event == cv2.EVENT_MOUSEMOVE:
112 dx, dy = x - state.prev_mouse[0], y - state.prev_mouse[1]
114 if state.mouse_btns[0]:
115 state.yaw += float(dx) / w * 2
116 state.pitch -= float(dy) / h * 2
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)
122 elif state.mouse_btns[2]:
123 dz = math.sqrt(dx**2 + dy**2) * math.copysign(0.01, -dy)
124 state.translation[2] += dz
127 if event == cv2.EVENT_MOUSEWHEEL:
128 dz = math.copysign(0.1, flags)
129 state.translation[2] += dz
132 state.prev_mouse = (x, y)
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)
141 """project 3d vector array to 2d""" 143 view_aspect = float(h)/w
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)
152 proj[v[:, 2] < znear] = np.nan
157 """apply view transformation on vector array""" 158 return np.dot(v - state.pivot, state.rotation) + state.pivot - state.translation
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():
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)
172 cv2.line(out, p0, p1, color, thickness, cv2.LINE_AA)
175 def grid(out, pos, rotation=np.eye(3), size=1, n=10, color=(0x80, 0x80, 0x80)):
176 """draw a grid on xz plane""" 180 for i
in range(0, n+1):
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):
186 line3d(out,
view(pos + np.dot((-s2, 0, z), rotation)),
187 view(pos + np.dot((s2, 0, z), rotation)), color)
190 def axes(out, pos, rotation=np.eye(3), size=0.075, thickness=2):
193 np.dot((0, 0, size), rotation), (0xff, 0, 0), thickness)
195 np.dot((0, size, 0), rotation), (0, 0xff, 0), thickness)
197 np.dot((size, 0, 0), rotation), (0, 0, 0xff), thickness)
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
205 for d
in range(1, 6, 2):
207 p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
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)
223 """draw point cloud with optional painter's algorithm""" 230 s = v[:, 2].argsort()[::-1]
236 proj *= 0.5**state.decimate
241 j, i = proj.astype(np.uint32).T
244 im = (i >= 0) & (i < h)
245 jm = (j >= 0) & (j < w)
248 cw, ch = color.shape[:2][::-1]
253 v, u = (texcoords[s] * (cw, ch) + 0.5).astype(np.uint32).T
255 v, u = (texcoords * (cw, ch) + 0.5).astype(np.uint32).T
257 np.clip(u, 0, ch-1, out=u)
258 np.clip(v, 0, cw-1, out=v)
261 out[i[m], j[m]] = color[u[m], v[m]]
264 out = np.empty((h, w, 3), dtype=np.uint8)
270 frames = pipeline.wait_for_frames()
272 depth_frame = frames.get_depth_frame()
273 color_frame = frames.get_color_frame()
275 depth_frame = decimate.process(depth_frame)
278 depth_intrinsics = rs.video_stream_profile(
279 depth_frame.profile).get_intrinsics()
280 w, h = depth_intrinsics.width, depth_intrinsics.height
282 depth_image = np.asanyarray(depth_frame.get_data())
283 color_image = np.asanyarray(color_frame.get_data())
285 depth_colormap = np.asanyarray(
286 colorizer.colorize(depth_frame).get_data())
289 mapped_frame, color_source = color_frame, color_image
291 mapped_frame, color_source = depth_frame, depth_colormap
293 points = pc.calculate(depth_frame)
294 pc.map_to(mapped_frame)
297 v, t = points.get_vertices(), points.get_texture_coordinates()
306 grid(out, (0, 0.5, 1), size=1, n=10)
308 axes(out,
view([0, 0, 0]), state.rotation, size=0.1, thickness=1)
310 if not state.scale
or out.shape[:2] == (h, w):
311 pointcloud(out, verts, texcoords, color_source)
313 tmp = np.zeros((h, w, 3), dtype=np.uint8)
314 pointcloud(tmp, verts, texcoords, color_source)
316 tmp, out.shape[:2][::-1], interpolation=cv2.INTER_NEAREST)
317 np.putmask(out, tmp > 0, tmp)
319 if any(state.mouse_btns):
320 axes(out,
view(state.pivot), state.rotation, thickness=4)
322 dt = time.time() - now
325 state.WIN_NAME,
"RealSense (%dx%d) %dFPS (%.2fms) %s" %
326 (w, h, 1.0/dt, dt*1000,
"PAUSED" if state.paused
else ""))
328 cv2.imshow(state.WIN_NAME, out)
338 state.decimate = (state.decimate + 1) % 3
339 decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
348 cv2.imwrite(
'./out.png', out)
351 points.export_to_ply(
'./out.ply', mapped_frame)
353 if key
in (27, ord(
"q"))
or cv2.getWindowProperty(state.WIN_NAME, cv2.WND_PROP_AUTOSIZE) < 0:
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)
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 __init__(self, args, kwargs)
def mouse_cb(event, x, y, flags, param)