1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
|
# coding: utf-8
# before install libfreenect2 & pylibfreenect2
import numpy as np
# import cv2
import pcl
import pcl.pcl_visualization
import sys
from pylibfreenect2 import Freenect2, SyncMultiFrameListener
from pylibfreenect2 import FrameType, Registration, Frame
from pylibfreenect2 import createConsoleLogger, setGlobalLogger
from pylibfreenect2 import LoggerLevel
try:
from pylibfreenect2 import OpenCLPacketPipeline
pipeline = OpenCLPacketPipeline()
except:
from pylibfreenect2 import CpuPacketPipeline
pipeline = CpuPacketPipeline()
# Create and set logger
logger = createConsoleLogger(LoggerLevel.Debug)
setGlobalLogger(logger)
fn = Freenect2()
num_devices = fn.enumerateDevices()
if num_devices == 0:
print("No device connected!")
sys.exit(1)
serial = fn.getDeviceSerialNumber(0)
device = fn.openDevice(serial, pipeline=pipeline)
listener = SyncMultiFrameListener(
FrameType.Color | FrameType.Ir | FrameType.Depth)
# Register listeners
device.setColorFrameListener(listener)
device.setIrAndDepthFrameListener(listener)
device.start()
# NOTE: must be called after device.start()
registration = Registration(device.getIrCameraParams(),
device.getColorCameraParams())
undistorted = Frame(512, 424, 4)
registered = Frame(512, 424, 4)
# Optinal parameters for registration
# set True if you need
need_bigdepth = False
need_color_depth_map = False
bigdepth = Frame(1920, 1082, 4) if need_bigdepth else None
color_depth_map = np.zeros((424, 512), np.int32).ravel() \
if need_color_depth_map else None
point = pcl.PointCloud()
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorCloud(cloud)
while True:
frames = listener.waitForNewFrame()
color = frames["color"]
ir = frames["ir"]
depth = frames["depth"]
registration.apply(color, depth, undistorted, registered,
bigdepth=bigdepth,
color_depth_map=color_depth_map)
# NOTE for visualization:
# cv2.imshow without OpenGL backend seems to be quite slow to draw all
# things below. Try commenting out some imshow if you don't have a fast
# visualization backend.
# cv2.imshow("ir", ir.asarray() / 65535.)
# cv2.imshow("depth", depth.asarray() / 4500.)
# cv2.imshow("color", cv2.resize(color.asarray(), (int(1920 / 3), int(1080 / 3))))
# cv2.imshow("registered", registered.asarray(np.uint8))
# if need_bigdepth:
# cv2.imshow("bigdepth", cv2.resize(bigdepth.asarray(np.float32),
# (int(1920 / 3), int(1082 / 3))))
# if need_color_depth_map:
# cv2.imshow("color_depth_map", color_depth_map.reshape(424, 512))
undistorted_arrray = undistorted.asarray(dtype=np.float32, ndim=2)
# registered_array = registered.asarray(dtype=np.uint8)
point = pcl.PointCloud(undistorted_arrray)
# visual.ShowColorCloud(cloud)
listener.release(frames)
# key = cv2.waitKey(delay=1)
# if key == ord('q'):
# break
if visual.WasStopped() == True:
break
device.stop()
device.close()
sys.exit(0)
|
