realsense point cloud
# License: Apache 2.0. See LICENSE file in root directory.
# Copyright(c) 2015-2017 Intel Corporation. All Rights Reserved.
"""
OpenCV and Numpy Point cloud Software Renderer
This sample is mostly for demonstration and educational purposes.
It really doesn't offer the quality or performance that can be
achieved with hardware acceleration.
Usage:
------
Mouse:
Drag with left button to rotate around pivot (thick small axes),
with right button to translate and the wheel to zoom.
Keyboard:
[p] Pause
[r] Reset View
[d] Cycle through decimation values
[z] Toggle point scaling
[c] Toggle color source
[s] Save PNG (./out.png)
[e] Export points to ply (./out.ply)
[q\ESC] Quit
"""
import math
import time
import cv2
import numpy as np
import pyrealsense2 as rs
class AppState:
def __init__(self, *args, **kwargs):
self.WIN_NAME = 'RealSense'
self.pitch, self.yaw = math.radians(-10), math.radians(-15)
self.translation = np.array([0, 0, -1], dtype=np.float32)
self.distance = 2
self.prev_mouse = 0, 0
self.mouse_btns = [False, False, False]
self.paused = False
self.decimate = 1
self.scale = True
self.color = True
def reset(self):
self.pitch, self.yaw, self.distance = 0, 0, 2
self.translation[:] = 0, 0, -1
@property
def rotation(self):
Rx, _ = cv2.Rodrigues((self.pitch, 0, 0))
Ry, _ = cv2.Rodrigues((0, self.yaw, 0))
return np.dot(Ry, Rx).astype(np.float32)
@property
def pivot(self):
return self.translation + np.array((0, 0, self.distance), dtype=np.float32)
state = AppState()
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
# Start streaming
pipeline.start(config)
# Get stream profile and camera intrinsics
profile = pipeline.get_active_profile()
depth_profile = rs.video_stream_profile(profile.get_stream(rs.stream.depth))
depth_intrinsics = depth_profile.get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
# Processing blocks
pc = rs.pointcloud()
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
colorizer = rs.colorizer()
def mouse_cb(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
state.mouse_btns[0] = True
if event == cv2.EVENT_LBUTTONUP:
state.mouse_btns[0] = False
if event == cv2.EVENT_RBUTTONDOWN:
state.mouse_btns[1] = True
if event == cv2.EVENT_RBUTTONUP:
state.mouse_btns[1] = False
if event == cv2.EVENT_MBUTTONDOWN:
state.mouse_btns[2] = True
if event == cv2.EVENT_MBUTTONUP:
state.mouse_btns[2] = False
if event == cv2.EVENT_MOUSEMOVE:
h, w = out.shape[:2]
dx, dy = x - state.prev_mouse[0], y - state.prev_mouse[1]
if state.mouse_btns[0]:
state.yaw += float(dx) / w * 2
state.pitch -= float(dy) / h * 2
elif state.mouse_btns[1]:
dp = np.array((dx / w, dy / h, 0), dtype=np.float32)
state.translation -= np.dot(state.rotation, dp)
elif state.mouse_btns[2]:
dz = math.sqrt(dx**2 + dy**2) * math.copysign(0.01, -dy)
state.translation[2] += dz
state.distance -= dz
if event == cv2.EVENT_MOUSEWHEEL:
dz = math.copysign(0.1, flags)
state.translation[2] += dz
state.distance -= dz
state.prev_mouse = (x, y)
cv2.namedWindow(state.WIN_NAME, cv2.WINDOW_AUTOSIZE)
cv2.resizeWindow(state.WIN_NAME, w, h)
cv2.setMouseCallback(state.WIN_NAME, mouse_cb)
def project(v):
"""project 3d vector array to 2d"""
h, w = out.shape[:2]
view_aspect = float(h)/w
# ignore divide by zero for invalid depth
with np.errstate(divide='ignore', invalid='ignore'):
proj = v[:, :-1] / v[:, -1, np.newaxis] * \
(w*view_aspect, h) + (w/2.0, h/2.0)
# near clipping
znear = 0.03
proj[v[:, 2] < znear] = np.nan
return proj
def view(v):
"""apply view transformation on vector array"""
return np.dot(v - state.pivot, state.rotation) + state.pivot - state.translation
def line3d(out, pt1, pt2, color=(0x80, 0x80, 0x80), thickness=1):
"""draw a 3d line from pt1 to pt2"""
p0 = project(pt1.reshape(-1, 3))[0]
p1 = project(pt2.reshape(-1, 3))[0]
if np.isnan(p0).any() or np.isnan(p1).any():
return
p0 = tuple(p0.astype(int))
p1 = tuple(p1.astype(int))
rect = (0, 0, out.shape[1], out.shape[0])
inside, p0, p1 = cv2.clipLine(rect, p0, p1)
if inside:
cv2.line(out, p0, p1, color, thickness, cv2.LINE_AA)
def grid(out, pos, rotation=np.eye(3), size=1, n=10, color=(0x80, 0x80, 0x80)):
"""draw a grid on xz plane"""
pos = np.array(pos)
s = size / float(n)
s2 = 0.5 * size
for i in range(0, n+1):
x = -s2 + i*s
line3d(out, view(pos + np.dot((x, 0, -s2), rotation)),
view(pos + np.dot((x, 0, s2), rotation)), color)
for i in range(0, n+1):
z = -s2 + i*s
line3d(out, view(pos + np.dot((-s2, 0, z), rotation)),
view(pos + np.dot((s2, 0, z), rotation)), color)
def axes(out, pos, rotation=np.eye(3), size=0.075, thickness=2):
"""draw 3d axes"""
line3d(out, pos, pos +
np.dot((0, 0, size), rotation), (0xff, 0, 0), thickness)
line3d(out, pos, pos +
np.dot((0, size, 0), rotation), (0, 0xff, 0), thickness)
line3d(out, pos, pos +
np.dot((size, 0, 0), rotation), (0, 0, 0xff), thickness)
def frustum(out, intrinsics, color=(0x40, 0x40, 0x40)):
"""draw camera's frustum"""
orig = view([0, 0, 0])
w, h = intrinsics.width, intrinsics.height
for d in range(1, 6, 2):
def get_point(x, y):
p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d)
line3d(out, orig, view(p), color)
return p
top_left = get_point(0, 0)
top_right = get_point(w, 0)
bottom_right = get_point(w, h)
bottom_left = get_point(0, h)
line3d(out, view(top_left), view(top_right), color)
line3d(out, view(top_right), view(bottom_right), color)
line3d(out, view(bottom_right), view(bottom_left), color)
line3d(out, view(bottom_left), view(top_left), color)
def pointcloud(out, verts, texcoords, color, painter=True):
"""draw point cloud with optional painter's algorithm"""
if painter:
# Painter's algo, sort points from back to front
# get reverse sorted indices by z (in view-space)
# https://gist.github.com/stevenvo/e3dad127598842459b68
v = view(verts)
s = v[:, 2].argsort()[::-1]
proj = project(v[s])
else:
proj = project(view(verts))
if state.scale:
proj *= 0.5**state.decimate
h, w = out.shape[:2]
# proj now contains 2d image coordinates
j, i = proj.astype(np.uint32).T
# create a mask to ignore out-of-bound indices
im = (i >= 0) & (i < h)
jm = (j >= 0) & (j < w)
m = im & jm
cw, ch = color.shape[:2][::-1]
if painter:
# sort texcoord with same indices as above
# texcoords are [0..1] and relative to top-left pixel corner,
# multiply by size and add 0.5 to center
v, u = (texcoords[s] * (cw, ch) + 0.5).astype(np.uint32).T
else:
v, u = (texcoords * (cw, ch) + 0.5).astype(np.uint32).T
# clip texcoords to image
np.clip(u, 0, ch-1, out=u)
np.clip(v, 0, cw-1, out=v)
# perform uv-mapping
out[i[m], j[m]] = color[u[m], v[m]]
out = np.empty((h, w, 3), dtype=np.uint8)
while True:
# Grab camera data
if not state.paused:
# Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
depth_frame = decimate.process(depth_frame)
# Grab new intrinsics (may be changed by decimation)
depth_intrinsics = rs.video_stream_profile(
depth_frame.profile).get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
depth_image = np.asanyarray(depth_frame.get_data())
color_image = np.asanyarray(color_frame.get_data())
depth_colormap = np.asanyarray(
colorizer.colorize(depth_frame).get_data())
if state.color:
mapped_frame, color_source = color_frame, color_image
else:
mapped_frame, color_source = depth_frame, depth_colormap
points = pc.calculate(depth_frame)
pc.map_to(mapped_frame)
# Pointcloud data to arrays
v, t = points.get_vertices(), points.get_texture_coordinates()
verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz
texcoords = np.asanyarray(t).view(np.float32).reshape(-1, 2) # uv
# Render
now = time.time()
out.fill(0)
grid(out, (0, 0.5, 1), size=1, n=10)
frustum(out, depth_intrinsics)
axes(out, view([0, 0, 0]), state.rotation, size=0.1, thickness=1)
if not state.scale or out.shape[:2] == (h, w):
pointcloud(out, verts, texcoords, color_source)
else:
tmp = np.zeros((h, w, 3), dtype=np.uint8)
pointcloud(tmp, verts, texcoords, color_source)
tmp = cv2.resize(
tmp, out.shape[:2][::-1], interpolation=cv2.INTER_NEAREST)
np.putmask(out, tmp > 0, tmp)
if any(state.mouse_btns):
axes(out, view(state.pivot), state.rotation, thickness=4)
dt = time.time() - now
cv2.setWindowTitle(
state.WIN_NAME, "RealSense (%dx%d) %dFPS (%.2fms) %s" %
(w, h, 1.0/dt, dt*1000, "PAUSED" if state.paused else ""))
cv2.imshow(state.WIN_NAME, out)
key = cv2.waitKey(1)
if key == ord("r"):
state.reset()
if key == ord("p"):
state.paused ^= True
if key == ord("d"):
state.decimate = (state.decimate + 1) % 3
decimate.set_option(rs.option.filter_magnitude, 2 ** state.decimate)
if key == ord("z"):
state.scale ^= True
if key == ord("c"):
state.color ^= True
if key == ord("s"):
cv2.imwrite('./out.png', out)
if key == ord("e"):
points.export_to_ply('./out.ply', mapped_frame)
if key in (27, ord("q")) or cv2.getWindowProperty(state.WIN_NAME, cv2.WND_PROP_AUTOSIZE) < 0:
break
# Stop streaming
pipeline.stop()
#######################
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
