Binary writing of PCD

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,17 +203,17 @@ 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()
 
         cdef const c_openni2.Array[c_openni2.VideoMode]* _modes
         _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

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")

examples/simple.py

@@ -1,6 +1,6 @@
 import cyni
 import numpy as np
-import Image
+from PIL import Image
 
 cyni.initialize()
 device = cyni.getAnyDevice()

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)