From e621f9aed7302044f4ccd692ebf0437467963e33 Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Tue, 19 Jul 2016 14:48:41 +0200 Subject: [PATCH 01/10] python3 compatibility --- cyni.pyx | 30 +++++++++++++++--------------- setup.py | 10 +++++----- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 78c099b..55072ea 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()]) @@ -166,8 +166,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 +200,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 +208,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 +233,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 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) From 36b4e8b8af04b59ba2874a76e8b21e4c7dc7b78f Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Wed, 20 Jul 2016 10:22:32 +0200 Subject: [PATCH 02/10] update 2 examples to work with python3 and python2 --- examples/cloud.py | 19 ++++++++++--------- examples/simple.py | 2 +- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/examples/cloud.py b/examples/cloud.py index 3d9e6b2..4243b8a 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.pcd", ascii=True) +#readCloud = cyni.readPCD("cloud.pcd") +#cyni.writePCD(readCloud, "cloud2.pcd", ascii=True) 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() From 179095f50c63638bbc4622b925a355877077fcd5 Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Wed, 20 Jul 2016 12:28:53 +0200 Subject: [PATCH 03/10] fix reading of PCD files with rgb and make it work from python3 --- cyni.pyx | 6 +++--- examples/cloud.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 55072ea..bf513d9 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -578,7 +578,7 @@ def writePCD(pointCloud, filename, ascii=False): pointCloud_tmp.tofile(f) 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 +631,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 4243b8a..6cc855b 100644 --- a/examples/cloud.py +++ b/examples/cloud.py @@ -13,6 +13,6 @@ depthFrame = depthStream.readFrame() cloud = cyni.depthMapToPointCloud(depthFrame.data, depthStream) #cloud = cyni.depthMapToPointCloud(depthFrame.data, depthStream, colorFrame.data) -cyni.writePCD(cloud, "cloud.pcd", ascii=True) -#readCloud = cyni.readPCD("cloud.pcd") -#cyni.writePCD(readCloud, "cloud2.pcd", ascii=True) +cyni.writePCD(cloud, "cloud_bin.pcd") +readCloud = cyni.readPCD("cloud_bin.pcd") +cyni.writePCD(readCloud, "cloud_ascii.pcd", ascii=True) From c48e527aa11fee24d23fee936c3efd9c94810375 Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Tue, 9 Aug 2016 14:56:18 +0200 Subject: [PATCH 04/10] do not crash but raise exception if no device found --- cyni.pyx | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/cyni.pyx b/cyni.pyx index bf513d9..29e6d49 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -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 ret def enumerateDevices(): @@ -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): From 008b2200e4af9da5af21a63c2630a16d671e549a Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Tue, 9 Aug 2016 15:23:44 +0200 Subject: [PATCH 05/10] show return code in error message --- cyni.pyx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cyni.pyx b/cyni.pyx index 29e6d49..6ec132a 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -38,7 +38,7 @@ def error(*args): def initialize(): ret = c_openni2.initialize() if ret != c_openni2.STATUS_OK: - raise RuntimeError("Failed to initialize OpenNi2") + raise RuntimeError("Failed to initialize OpenNi2, return code was {}".format(ret)) return ret From 17b85673a1a50785c81486b515f4c25ce6cfd252 Mon Sep 17 00:00:00 2001 From: Alvaro Capellan Date: Thu, 11 Aug 2016 15:18:07 +0200 Subject: [PATCH 06/10] Save pointcloud in .obj format --- cyni.pyx | 137 ++++++++++++++++++++++++++++++++----------------------- 1 file changed, 79 insertions(+), 58 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 6ec132a..505d113 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -500,7 +500,7 @@ 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") + raise RuntimeError("No device found") return Device(deviceList[0]['uri']) def depthMapToImage(image): @@ -524,63 +524,84 @@ 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: - height = pointCloud.shape[0] - width = pointCloud.shape[1] - f.write("# .PCD v.7 - Point Cloud Data file format\n") - f.write("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") - 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)) - if ascii: - f.write("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, :])) - else: - f.write("%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) - else: - f.write("DATA binary\n") - if pointCloud.shape[2] == 6: - dt = np.dtype([('x', np.float32), - ('y', np.float32), - ('z', np.float32), - ('r', np.uint8), - ('g', np.uint8), - ('b', np.uint8), - ('I', np.uint8)]) - pointCloud_tmp = np.zeros((6, height*width, 1), 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) + + #fmt = filename.split(".")[-1] + fmt = filename[-3:] + print("File format: {}".format(fmt)) + + height = pointCloud.shape[0] + width = pointCloud.shape[1] + + if fmt=="pcd": + with open(filename, 'w') as f: + f.write("# .PCD v.7 - Point Cloud Data file format\n") + f.write("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") + 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)) + if ascii: + f.write("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, :])) + else: + f.write("%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) + else: + f.write("DATA binary\n") + if pointCloud.shape[2] == 6: + dt = np.dtype([('x', np.float32), + ('y', np.float32), + ('z', np.float32), + ('r', np.uint8), + ('g', np.uint8), + ('b', np.uint8), + ('I', np.uint8)]) + pointCloud_tmp = np.zeros((6, height*width, 1), 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) + + elif fmt=="obj": + with open(filename, 'w') as f: + f.write("# Wavefront .obj file format\n") + if ascii: + for row in range(height): + for col in range(width): + if pointCloud.shape[2]== 3: + f.write("v %f %f %f\n" % tuple(pointCloud[row, col, :])) + else: + print("Color not supported") + else: + print("Binary not supported") + def readPCD(filename): with open(filename, 'rb') as f: From c92ddc5693faa361813c20d1fef72ca78e6de61f Mon Sep 17 00:00:00 2001 From: Alvaro Capellan Date: Tue, 23 Aug 2016 08:59:23 +0200 Subject: [PATCH 07/10] Made separate methods for storing .pcd and .obj files --- cyni.pyx | 152 +++++++++++++++++++++++++++++-------------------------- 1 file changed, 79 insertions(+), 73 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 505d113..c152179 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -524,85 +524,91 @@ def depthMapToPointCloud(depthMap, depthStream, colorImage=None): raise Exception("Depth and color images must have save dimensions.") def writePCD(pointCloud, filename, ascii=False): + """ + Stores pointcloud in .pcd format (PCD v0.7) - #fmt = filename.split(".")[-1] - fmt = filename[-3:] - print("File format: {}".format(fmt)) + Format description: http://www.pointclouds.org/documentation/tutorials/pcd_file_format.php + """ height = pointCloud.shape[0] width = pointCloud.shape[1] - if fmt=="pcd": - with open(filename, 'w') as f: - f.write("# .PCD v.7 - Point Cloud Data file format\n") - f.write("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") - 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)) - if ascii: - f.write("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, :])) - else: - f.write("%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) - else: - f.write("DATA binary\n") - if pointCloud.shape[2] == 6: - dt = np.dtype([('x', np.float32), - ('y', np.float32), - ('z', np.float32), - ('r', np.uint8), - ('g', np.uint8), - ('b', np.uint8), - ('I', np.uint8)]) - pointCloud_tmp = np.zeros((6, height*width, 1), 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) - - elif fmt=="obj": - with open(filename, 'w') as f: - f.write("# Wavefront .obj file format\n") - if ascii: - for row in range(height): - for col in range(width): - if pointCloud.shape[2]== 3: - f.write("v %f %f %f\n" % tuple(pointCloud[row, col, :])) - else: - print("Color not supported") - else: - print("Binary not supported") + with open(filename, 'w') as f: + f.write("# .PCD v.7 - Point Cloud Data file format\n") + f.write("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") + 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)) + if ascii: + f.write("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, :])) + else: + f.write("%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) + else: + f.write("DATA binary\n") + if pointCloud.shape[2] == 6: + dt = np.dtype([('x', np.float32), + ('y', np.float32), + ('z', np.float32), + ('r', np.uint8), + ('g', np.uint8), + ('b', np.uint8), + ('I', np.uint8)]) + pointCloud_tmp = np.zeros((6, height*width, 1), 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) + +def writeOBJ(pointCloud, filename): + """ + Stores pointcloud as geometric vertices in .obj (Wavefront) format + + Format description: https://en.wikipedia.org/wiki/Wavefront_.obj_file + This method only represents pointclouds as vertices without w coords + """ + + height = pointCloud.shape[0] + width = pointCloud.shape[1] + + with open(filename, 'w') as f: + f.write("# Wavefront .obj file format\n") + for row in range(height): + for col in range(width): + if pointCloud.shape[2]== 3: + f.write("v %f %f %f\n" % tuple(pointCloud[row, col, :])) + else: + print("Color not supported") - def readPCD(filename): with open(filename, 'rb') as f: #"# .PCD v.7 - Point Cloud Data file format\n" From cff00884d0dc2c950a1e725e3fddc05c4c5e5e1d Mon Sep 17 00:00:00 2001 From: olivier R-D Date: Thu, 8 Sep 2016 13:34:10 +0200 Subject: [PATCH 08/10] do not mirror point cloud around Y --- cyni.pyx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 6ec132a..244af5a 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -425,7 +425,7 @@ cdef _depthMapToPointCloudXYZ(np.ndarray[np.float_t, ndim=3] pointCloud, c_openni2.convertDepthToWorld(depthStream._stream, x, y, depthMap[y,x], &worldX, &worldY, &worldZ) pointCloud[y,x,0] = worldX - pointCloud[y,x,1] = -worldY + pointCloud[y,x,1] = worldY pointCloud[y,x,2] = worldZ cdef _depthMapToPointCloudXYZRGB(np.ndarray[np.float_t, ndim=3] pointCloud, @@ -445,7 +445,7 @@ cdef _depthMapToPointCloudXYZRGB(np.ndarray[np.float_t, ndim=3] pointCloud, c_openni2.convertDepthToWorld(depthStream._stream, x, y, depthMap[y,x], &worldX, &worldY, &worldZ) pointCloud[y,x,0] = worldX - pointCloud[y,x,1] = -worldY + pointCloud[y,x,1] = worldY pointCloud[y,x,2] = worldZ pointCloud[y,x,3] = colorImage[y,x,0] pointCloud[y,x,4] = colorImage[y,x,1] @@ -468,7 +468,7 @@ cdef _depthMapToPointCloudXYZRGB_IR(np.ndarray[np.float_t, ndim=3] pointCloud, c_openni2.convertDepthToWorld(depthStream._stream, x, y, depthMap[y,x], &worldX, &worldY, &worldZ) pointCloud[y,x,0] = worldX - pointCloud[y,x,1] = -worldY + pointCloud[y,x,1] = worldY pointCloud[y,x,2] = worldZ pointCloud[y,x,3] = irImage[y,x] pointCloud[y,x,4] = irImage[y,x] From 535df7944e9e7f17cf5bcb5d526ab51d7671a962 Mon Sep 17 00:00:00 2001 From: Alvaro Capellan Date: Mon, 12 Sep 2016 10:39:58 +0200 Subject: [PATCH 09/10] Solved mirror effect for Structure sensor --- cyni.pyx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index c152179..16bfff1 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -70,7 +70,7 @@ cdef class Device(object): _stream.stop() _stream.destroy() self._streams.clear() - + self._device.close() def __init__(self, uri): @@ -425,7 +425,7 @@ cdef _depthMapToPointCloudXYZ(np.ndarray[np.float_t, ndim=3] pointCloud, c_openni2.convertDepthToWorld(depthStream._stream, x, y, depthMap[y,x], &worldX, &worldY, &worldZ) pointCloud[y,x,0] = worldX - pointCloud[y,x,1] = -worldY + pointCloud[y,x,1] = worldY pointCloud[y,x,2] = worldZ cdef _depthMapToPointCloudXYZRGB(np.ndarray[np.float_t, ndim=3] pointCloud, From c491d11b74bbdb43d4da14b4ea8594b92836f05f Mon Sep 17 00:00:00 2001 From: Alvaro Capellan Date: Mon, 12 Sep 2016 13:50:30 +0200 Subject: [PATCH 10/10] readPCL: decode all binaries for correct parsing --- cyni.pyx | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/cyni.pyx b/cyni.pyx index 16bfff1..a1fcf31 100644 --- a/cyni.pyx +++ b/cyni.pyx @@ -618,7 +618,7 @@ def readPCD(filename): f.readline() # "FIELDS x y z\n" - fields = f.readline().strip().split()[1:] + fields = [i.decode() for i in f.readline().strip().split()[1:]] if len(fields) == 3: rgb = False @@ -632,7 +632,7 @@ def readPCD(filename): pointSize = np.sum(sizes) #"TYPE F F F\n" - types = f.readline().strip().split()[1:] + types = [i.decode() for i in f.readline().strip().split()[1:]] #"COUNT 1 1 1\n" counts = [int(x) for x in f.readline().strip().split()[1:]] @@ -644,14 +644,15 @@ def readPCD(filename): height = int(f.readline().strip().split()[1]) #"VIEWPOINT 0 0 0 1 0 0 0\n" - viewpoint = np.array(f.readline().strip().split()[1:]) + bytelist = f.readline().strip().split()[1:] + viewpoint = np.array([int(i) for i in bytelist]) #"POINTS %d\n" % height * width points = int(f.readline().strip().split()[1]) #"DATA ascii\n" - format = f.readline().strip().split()[1] - ascii = format == 'ascii' + fmt = f.readline().strip().split()[1].decode() + ascii = fmt == 'ascii' if rgb: pointCloud = np.empty((height, width, 6))