diff --git a/cyni.pyx b/cyni.pyx index 78c099b..31a5da2 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -14,11 +14,11 @@ import sys from struct import pack, unpack, calcsize pixelFormats = { - "rgb": c_openni2.PIXEL_FORMAT_RGB888, - "yuv422": c_openni2.PIXEL_FORMAT_YUV422, - "gray16": c_openni2.PIXEL_FORMAT_GRAY16, - "depth1mm": c_openni2.PIXEL_FORMAT_DEPTH_1_MM, - "depth100um": c_openni2.PIXEL_FORMAT_DEPTH_100_UM, + b"rgb": c_openni2.PIXEL_FORMAT_RGB888, + b"yuv422": c_openni2.PIXEL_FORMAT_YUV422, + b"gray16": c_openni2.PIXEL_FORMAT_GRAY16, + b"depth1mm": c_openni2.PIXEL_FORMAT_DEPTH_1_MM, + b"depth100um": c_openni2.PIXEL_FORMAT_DEPTH_100_UM, } pixelFormatsReverse = dict([[v, k] for k, v in pixelFormats.items()]) @@ -36,7 +36,10 @@ def error(*args): def initialize(): - return c_openni2.initialize() + ret = c_openni2.initialize() + if ret != c_openni2.STATUS_OK: + raise RuntimeError("Failed to initialize OpenNi2, return code was {}".format(ret)) + return ret def enumerateDevices(): @@ -166,8 +169,8 @@ class Frame(object): cdef class VideoStream(object): cdef c_openni2.VideoStream _stream - cdef string _streamType - cdef string _pixelFormat + cdef bytes _streamType + cdef bytes _pixelFormat cdef int frameSize cdef readonly int width @@ -200,7 +203,7 @@ cdef class VideoStream(object): status = self._stream.create(_device, c_openni2.SENSOR_IR) if status != c_openni2.STATUS_OK: - error("Error opening %s stream." % self.streamType) + error("Error opening %s stream." % self._streamType) cdef const c_openni2.SensorInfo* _info = &self._stream.getSensorInfo() @@ -208,9 +211,9 @@ cdef class VideoStream(object): _modes = &(_info.getSupportedVideoModes()) foundMode = False - if self._streamType == b"color" and pixelFormat != "rgb": + if self._streamType == b"color" and pixelFormat != b"rgb": if pixelFormat is None: - pixelFormat = "rgb" + pixelFormat = b"rgb" else: error("Only RGB currently supported for color streams.") self.destroy() @@ -233,15 +236,15 @@ cdef class VideoStream(object): # Set the pixel format in case it was None pixelFormat = pixelFormatsReverse[mode.getPixelFormat()] - if pixelFormat == "rgb": + if pixelFormat == b"rgb": pixelSize = sizeof(c_openni2.RGB888Pixel) - elif pixelFormat == "yuv422": + elif pixelFormat == b"yuv422": pixelSize = sizeof(c_openni2.YUV422DoublePixel) - elif pixelFormat == "depth1mm": + elif pixelFormat == b"depth1mm": pixelSize = sizeof(c_openni2.DepthPixel) - elif pixelFormat == "depth100um": + elif pixelFormat == b"depth100um": pixelSize = sizeof(c_openni2.DepthPixel) - elif pixelFormat == "gray16": + elif pixelFormat == b"gray16": pixelSize = sizeof(c_openni2.Grayscale16Pixel) self._pixelFormat = pixelFormat @@ -496,6 +499,8 @@ def registerDepthMap(np.ndarray[np.uint16_t, ndim=2] depthMapIn, np.ndarray[np.u def getAnyDevice(): deviceList = enumerateDevices() + if not deviceList: + raise RuntimeError("Node device found") return Device(deviceList[0]['uri']) def depthMapToImage(image): @@ -519,42 +524,42 @@ def depthMapToPointCloud(depthMap, depthStream, colorImage=None): raise Exception("Depth and color images must have save dimensions.") def writePCD(pointCloud, filename, ascii=False): - with open(filename, 'w') as f: + with open(filename, 'wb') as f: height = pointCloud.shape[0] width = pointCloud.shape[1] - f.write("# .PCD v.7 - Point Cloud Data file format\n") - f.write("VERSION .7\n") + f.write(b"# .PCD v.7 - Point Cloud Data file format\n") + f.write(b"VERSION .7\n") if pointCloud.shape[2] == 3: - f.write("FIELDS x y z\n") - f.write("SIZE 4 4 4\n") - f.write("TYPE F F F\n") - f.write("COUNT 1 1 1\n") + f.write(b"FIELDS x y z\n") + f.write(b"SIZE 4 4 4\n") + f.write(b"TYPE F F F\n") + f.write(b"COUNT 1 1 1\n") else: - f.write("FIELDS x y z rgb\n") - f.write("SIZE 4 4 4 4\n") - f.write("TYPE F F F F\n") - f.write("COUNT 1 1 1 1\n") - f.write("WIDTH %d\n" % width) - f.write("HEIGHT %d\n" % height) - f.write("VIEWPOINT 0 0 0 1 0 0 0\n") - f.write("POINTS %d\n" % (height * width)) + f.write(b"FIELDS x y z rgb\n") + f.write(b"SIZE 4 4 4 4\n") + f.write(b"TYPE F F F F\n") + f.write(b"COUNT 1 1 1 1\n") + f.write(b"WIDTH %d\n" % width) + f.write(b"HEIGHT %d\n" % height) + f.write(b"VIEWPOINT 0 0 0 1 0 0 0\n") + f.write(b"POINTS %d\n" % (height * width)) if ascii: - f.write("DATA ascii\n") + f.write(b"DATA ascii\n") for row in range(height): for col in range(width): if pointCloud.shape[2]== 3: - f.write("%f %f %f\n" % tuple(pointCloud[row, col, :])) + f.write(b"%f %f %f\n" % tuple(pointCloud[row, col, :])) else: - f.write("%f %f %f" % tuple(pointCloud[row, col, :3])) + f.write(b"%f %f %f" % tuple(pointCloud[row, col, :3])) r = int(pointCloud[row, col, 3]) g = int(pointCloud[row, col, 4]) b = int(pointCloud[row, col, 5]) rgb_int = (r << 16) | (g << 8) | b packed = pack('i', rgb_int) rgb = unpack('f', packed)[0] - f.write(" %.12e\n" % rgb) + f.write(b" %.12e\n" % rgb) else: - f.write("DATA binary\n") + f.write(b"DATA binary\n") if pointCloud.shape[2] == 6: dt = np.dtype([('x', np.float32), ('y', np.float32), @@ -563,22 +568,24 @@ def writePCD(pointCloud, filename, ascii=False): ('g', np.uint8), ('b', np.uint8), ('I', np.uint8)]) - pointCloud_tmp = np.zeros((6, height*width, 1), dtype=dt) + pointCloud_tmp = np.zeros((height*width), dtype=dt) for i, k in enumerate(['x', 'y', 'z', 'r', 'g', 'b']): pointCloud_tmp[k] = pointCloud[:, :, i].reshape((height*width, 1)) pointCloud_tmp.tofile(f) else: - dt = np.dtype([('x', np.float32), - ('y', np.float32), - ('z', np.float32), - ('I', np.uint8)]) - pointCloud_tmp = np.zeros((3, height*width, 1), dtype=dt) - for i, k in enumerate(['x', 'y', 'z']): - pointCloud_tmp[k] = pointCloud[:, :, i].reshape((height*width, 1)) - pointCloud_tmp.tofile(f) - + # dt = np.dtype([('x', np.float32), + # ('y', np.float32), + # ('z', np.float32), + # #('I', np.uint8)] + # ) + # pointCloud_tmp = np.zeros((height*width), dtype=dt) + # for i, k in enumerate(['x', 'y', 'z']): + # pointCloud_tmp[k] = pointCloud[:, :, i].reshape((height*width, 1)) + # pointCloud_tmp.tofile(f) + f.write(pointCloud.astype(np.float32).data.tobytes()) + def readPCD(filename): - with open(filename, 'r') as f: + with open(filename, 'rb') as f: #"# .PCD v.7 - Point Cloud Data file format\n" f.readline() @@ -631,13 +638,13 @@ def readPCD(filename): if ascii: data = [float(x) for x in f.readline().strip().split()] else: - data = unpack('ffff', f.read(pointSize)) + data = unpack('fff', f.read(pointSize)) pointCloud[row, col, 0] = data[0] pointCloud[row, col, 1] = data[1] pointCloud[row, col, 2] = data[2] if rgb: - rgb_float = data[3] + rgb_float = unpack('f', f.read(1)) packed = pack('f', rgb_float) rgb_int = unpack('i', packed)[0] r = rgb_int >> 16 & 0x0000ff diff --git a/examples/cloud.py b/examples/cloud.py index 3d9e6b2..d8e8995 100644 --- a/examples/cloud.py +++ b/examples/cloud.py @@ -1,17 +1,18 @@ import cyni import numpy as np -import Image +from PIL import Image cyni.initialize() device = cyni.getAnyDevice() device.open() -depthStream = device.createStream("depth", fps=30) -colorStream = device.createStream("color", fps=30) +depthStream = device.createStream(b"depth", fps=30) +#colorStream = device.createStream(b"color", fps=30) depthStream.start() -colorStream.start() -colorFrame = colorStream.readFrame() +#colorStream.start() +#colorFrame = colorStream.readFrame() depthFrame = depthStream.readFrame() -cloud = cyni.depthMapToPointCloud(depthFrame.data, depthStream, colorFrame.data) -cyni.writePCD(cloud, "cloud.pcd") -readCloud = cyni.readPCD("cloud.pcd") -cyni.writePCD(readCloud, "cloud2.pcd", ascii=True) +cloud = cyni.depthMapToPointCloud(depthFrame.data, depthStream) +#cloud = cyni.depthMapToPointCloud(depthFrame.data, depthStream, colorFrame.data) +cyni.writePCD(cloud, "cloud_ascii.pcd", ascii=True) +cyni.writePCD(cloud, "cloud_bin.pcd") +read_bin_cloud = cyni.readPCD("cloud_bin.pcd") diff --git a/examples/simple.py b/examples/simple.py index 9cb2af4..3aa8c99 100644 --- a/examples/simple.py +++ b/examples/simple.py @@ -1,6 +1,6 @@ import cyni import numpy as np -import Image +from PIL import Image cyni.initialize() device = cyni.getAnyDevice() diff --git a/setup.py b/setup.py index 0c82918..a3c75c1 100644 --- a/setup.py +++ b/setup.py @@ -10,25 +10,25 @@ openni2_lib = os.getenv('OPENNI2_REDIST') if openni2_include is None or openni2_lib is None: - print """ + print(""" Please make sure OPENNI2_INCLUDE and OPENNI2_REDIST are set. You can source the OpenNIDevEnvironment that the OpenNI2 installer generates to set these, or you can set them manually. To keep these environment variables when running with sudo, you can use sudo -E python setup.py install. - """ + """) sys.exit(1) has_emitter_control = os.getenv('OPENNI2_HAS_EMITTER_CONTROL', 0) has_emitter_control = bool(has_emitter_control) if has_emitter_control: - print "Using emitter control API" + print("Using emitter control API") class build_ext_with_config(build_ext): def build_extensions(self): - print 'Generate config.pxi' + print('Generate config.pxi') filename = os.path.join(os.path.dirname(__file__), 'config.pxi') with open(filename, 'w') as fd: - for k, v in c_options.iteritems(): + for k, v in c_options.items(): fd.write('DEF %s = %d\n' % (k.upper(), int(v))) build_ext.build_extensions(self) os.remove(filename)