diff --git a/python/.gitignore b/python/.gitignore
index 7e99e367f8443d86e5e8825b9fda39dfbb39630d..feae5c1921703133caa7d3cb82a79e2be6e85ab6 100644
--- a/python/.gitignore
+++ b/python/.gitignore
@@ -1 +1,2 @@
-*.pyc
\ No newline at end of file
+*.pyc
+__pycache__
\ No newline at end of file
diff --git a/python/ftl/__init__.py b/python/ftl/__init__.py
index 136f6fe7d7396d7672817e5120c02d8096211310..e45ef9c2b544bde13d59925041f765c83090c0d0 100644
--- a/python/ftl/__init__.py
+++ b/python/ftl/__init__.py
@@ -1 +1,4 @@
-from . ftlstream import FTLStream
+from . ftlstream import FTLStreamReader, FTLStreamWriter
+from . misc import disparity_to_depth
+
+from . import ftltypes as types
\ No newline at end of file
diff --git a/python/ftl/ftlstream.py b/python/ftl/ftlstream.py
index ef8a32aba4a195f5d26fa7fa84cfecfeefda21b8..e12f197caf1b246d23a354b1aadcdcf58f6bdaf9 100644
--- a/python/ftl/ftlstream.py
+++ b/python/ftl/ftlstream.py
@@ -6,6 +6,9 @@ import struct
from enum import IntEnum
from collections import namedtuple
+
+from . misc import is_iframe
+from . import ftltypes as ftl
from . import libde265
try:
@@ -35,132 +38,43 @@ except ImportError:
return rgb.round().astype(np.uint8)
-# FTL definitions
-
-# components/rgbd-sources/include/ftl/rgbd/camera.hpp
-_Camera = namedtuple("Camera", ["fx", "fy", "cx", "cy", "width", "height",
- "min_depth", "max_depth", "baseline", "doffs"])
-
-# components/codecs/include/ftl/codecs/packet.hpp
-_packet = namedtuple("Packet", ["codec", "definition", "block_total",
- "block_number", "flags", "data"])
-
-_stream_packet = namedtuple("StreamPacket", ["timestamp", "streamID",
- "chanel_count", "channel"])
-
-# components/codecs/include/ftl/codecs/bitrates.hpp
-class _codec_t(IntEnum):
- JPG = 0
- PNG = 1
- H264 = 2
- HEVC = 3
- WAV = 4
- JSON = 100
- CALIBRATION = 101
- POSE = 102
- RAW = 103
-
-_definition_t = {
- 0 : (7680, 4320),
- 1 : (2160, 3840),
- 2 : (1080, 1920),
- 3 : (720, 1280),
- 4 : (576, 1024),
- 5 : (480, 854),
- 6 : (360, 640),
- 7 : (0, 0),
- 8 : (2056, 1852)
-}
-
-# components/codecs/include/ftl/codecs/hevc.hpp
-class _NALType(IntEnum):
- CODED_SLICE_TRAIL_N = 0
- CODED_SLICE_TRAIL_R = 1
-
- CODED_SLICE_TSA_N = 2
- CODED_SLICE_TSA_R = 3
-
- CODED_SLICE_STSA_N = 4
- CODED_SLICE_STSA_R = 5
-
- CODED_SLICE_RADL_N = 6
- CODED_SLICE_RADL_R = 7
-
- CODED_SLICE_RASL_N = 8
- CODED_SLICE_RASL_R = 9
-
- RESERVED_VCL_N10 = 10
- RESERVED_VCL_R11 = 11
- RESERVED_VCL_N12 = 12
- RESERVED_VCL_R13 = 13
- RESERVED_VCL_N14 = 14
- RESERVED_VCL_R15 = 15
-
- CODED_SLICE_BLA_W_LP = 16
- CODED_SLICE_BLA_W_RADL = 17
- CODED_SLICE_BLA_N_LP = 18
- CODED_SLICE_IDR_W_RADL = 19
- CODED_SLICE_IDR_N_LP = 20
- CODED_SLICE_CRA = 21
- RESERVED_IRAP_VCL22 = 22
- RESERVED_IRAP_VCL23 = 23
-
- RESERVED_VCL24 = 24
- RESERVED_VCL25 = 25
- RESERVED_VCL26 = 26
- RESERVED_VCL27 = 27
- RESERVED_VCL28 = 28
- RESERVED_VCL29 = 29
- RESERVED_VCL30 = 30
- RESERVED_VCL31 = 31
-
- VPS = 32
- SPS = 33
- PPS = 34
- ACCESS_UNIT_DELIMITER = 35
- EOS = 36
- EOB = 37
- FILLER_DATA = 38
- PREFIX_SEI = 39
- SUFFIX_SEI = 40
-
- RESERVED_NVCL41 = 41
- RESERVED_NVCL42 = 42
- RESERVED_NVCL43 = 43
- RESERVED_NVCL44 = 44
- RESERVED_NVCL45 = 45
- RESERVED_NVCL46 = 46
- RESERVED_NVCL47 = 47
- UNSPECIFIED_48 = 48
- UNSPECIFIED_49 = 49
- UNSPECIFIED_50 = 50
- UNSPECIFIED_51 = 51
- UNSPECIFIED_52 = 52
- UNSPECIFIED_53 = 53
- UNSPECIFIED_54 = 54
- UNSPECIFIED_55 = 55
- UNSPECIFIED_56 = 56
- UNSPECIFIED_57 = 57
- UNSPECIFIED_58 = 58
- UNSPECIFIED_59 = 59
- UNSPECIFIED_60 = 60
- UNSPECIFIED_61 = 61
- UNSPECIFIED_62 = 62
- UNSPECIFIED_63 = 63
- INVALID = 64
-
-def get_NAL_type(data):
- if not isinstance(data, bytes):
- raise ValueError("expected bytes")
-
- return _NALType((data[4] >> 1) & 0x3f)
+class FTLStreamWriter:
+ def __init__(self, file):
+ self._file = open(file, "wb")
+ self._file.write(bytes(ord(c) for c in "FTLF"))
+ self._file.write(bytes([0]))
+
+ self._packer = msgpack.Packer(strict_types=False, use_bin_type=True)
+
+ def __del__(self):
+ self.close()
+
+ def close(self):
+ self._file.close()
+
+ def add_source(self, parameters, pose):
+ pass
+
+ def add_raw(self, sp, p):
+ if len(sp) != len(ftl.StreamPacket._fields) or len(p) != len(ftl.Packet._fields):
+ raise ValueError("invalid input")
+
+ self._file.write(self._packer.pack((sp, p)))
+
+ def add_frame(self, timestamp, src, channel, codec, data, encode=True):
+ pass
-class FTLStream:
+ def add_depth(self, timestamp, src, data):
+ pass
+
+class FTLStreamReader:
''' FTL file reader '''
def __init__(self, file):
self._file = open(file, "br")
self._decoders = {}
+ self._seen_iframe = set()
+
self._frames = {}
self._calibration = {}
self._pose = {}
@@ -168,8 +82,9 @@ class FTLStream:
try:
magic = self._file.read(5)
- if magic[:4] != bytearray(ord(c) for c in "FTLF"):
+ if magic[:4] != bytes(ord(c) for c in "FTLF"):
raise Exception("wrong magic")
+ print(magic[4])
self._unpacker = msgpack.Unpacker(self._file, raw=True, use_list=False)
@@ -184,7 +99,7 @@ class FTLStream:
def _read_next(self):
v1, v2 = self._unpacker.unpack()
- return _stream_packet._make(v1), _packet._make(v2)
+ return ftl.StreamPacket._make(v1), ftl.Packet._make(v2)
def _update_calib(self, sp, p):
''' Update calibration.
@@ -192,7 +107,7 @@ class FTLStream:
todo: fix endianess
'''
calibration = struct.unpack("@ddddIIdddd", p.data[:(4*8+2*4+4*8)])
- self._calibration[sp.streamID] = _Camera._make(calibration)
+ self._calibration[sp.streamID] = ftl.Camera._make(calibration)
def _update_pose(self, sp, p):
''' Update pose
@@ -209,33 +124,34 @@ class FTLStream:
def _process_json(self, sp, p):
raise NotImplementedError("json decoding not implemented")
- def _push_data_hevc(self, sp, p):
+ def _decode_hevc(self, sp, p):
''' Decode HEVC frame '''
k = (sp.streamID, sp.channel)
if k not in self._decoders:
- self._decoders[k] = libde265.Decoder(_definition_t[p.definition])
+ self._decoders[k] = libde265.Decoder(ftl.definition_t[p.definition])
decoder = self._decoders[k]
+
+ if k not in self._seen_iframe:
+ if not is_iframe(p.data):
+ # can't decode before first I-frame has been received
+ return
+
+ self._seen_iframe.add(k)
+
decoder.push_data(p.data)
- try:
+ decoder.push_end_of_frame()
+
+ while decoder.get_number_of_input_bytes_pending() > 0:
decoder.decode()
-
- except libde265.WaitingForInput:
- pass
-
- def _decode_hevc(self):
- for stream, decoder in self._decoders.items():
- try:
- decoder.decode()
-
- except libde265.WaitingForInput:
- pass
-
+
+ img = None
+ while img is None:
img = decoder.get_next_picture()
- if img is not None:
- self._frames[stream] = _ycrcb2rgb(img)
+
+ self._frames[k] = _ycrcb2rgb(img)
def _flush_decoders(self):
for decoder in self._decoders.values():
@@ -255,51 +171,43 @@ class FTLStream:
Reads data for until the next timestamp. Returns False if there is no
more data to read, otherwise returns True.
'''
- if self._packets_read == 0:
+
+ try:
self._sp, self._p = self._read_next()
self._packets_read += 1
-
- self._frames = {}
- self._ts = self._sp.timestamp
- ex = []
+ except msgpack.OutOfData:
+ return False
- while self._sp.timestamp == self._ts:
- # TODO: Can source be removed?
+ self._ts = self._sp.timestamp
- try:
- if self._p.codec == _codec_t.JSON:
- self._process_json(self._sp, self._p)
+ if self._p.block_total != 1 or self._p.block_number != 0:
+ raise Exception("Unsupported block format (todo)")
- elif self._p.codec == _codec_t.CALIBRATION:
- self._update_calib(self._sp, self._p)
+ if self._p.codec == ftl.codec_t.JSON:
+ self._process_json(self._sp, self._p)
- elif self._p.codec == _codec_t.POSE:
- self._update_pose(self._sp, self._p)
+ elif self._p.codec == ftl.codec_t.CALIBRATION:
+ self._update_calib(self._sp, self._p)
- elif self._p.codec == _codec_t.HEVC:
- self._push_data_hevc(self._sp, self._p)
+ elif self._p.codec == ftl.codec_t.POSE:
+ self._update_pose(self._sp, self._p)
- else:
- raise ValueError("unkowno codec %i" % self._p.codec)
-
- except Exception as e:
- ex.append(e)
-
- try:
- self._sp, self._p = self._read_next()
- self._packets_read += 1
-
- except msgpack.OutOfData:
- return False
-
- if len(ex) > 0:
- raise Exception(ex)
-
- self._decode_hevc()
+ elif self._p.codec == ftl.codec_t.HEVC:
+ self._decode_hevc(self._sp, self._p)
+ else:
+ raise Exception("unkowno codec %i" % self._p.codec)
+
+ print("read() took: %.4f" % (t_end - t_start))
return True
-
+
+ def get_packet_count(self):
+ return self._packets_read
+
+ def get_raw(self):
+ return self._sp, self._p
+
def get_timestamp(self):
return self._ts
@@ -310,6 +218,8 @@ class FTLStream:
raise ValueError("source id %i not found" % source)
def get_camera_matrix(self, source):
+ ''' Camera intrinsic parameters '''
+
calib = self.get_calibration(source)
K = np.identity(3, dtype=np.float64)
K[0,0] = calib.fx
@@ -325,7 +235,7 @@ class FTLStream:
raise ValueError("source id %i not found" % source)
def get_frames(self):
- ''' Returns all frames '''
+ ''' All frames '''
return self._frames
def get_frame(self, source, channel):
@@ -336,9 +246,19 @@ class FTLStream:
# raise an exception instead?
return None
+ def get_Q(self, source):
+ ''' Disparity to depth matrix in OpenCV format '''
+
+ calib = self.get_calibration(source)
+ Q = np.identity(4, dtype=np.float64)
+ Q[0,3] = calib.cx
+ Q[1,3] = calib.cy
+ Q[2,2] = 0.0
+ Q[2,3] = calib.fx
+ Q[3,2] = -1 / calib.baseline
+ Q[3,3] = calib.doff
+ return Q
+
def get_sources(self):
- ''' Get list of sources
-
- todo: Is there a better way?
- '''
+ ''' Get list of sources '''
return list(self._calibration.keys())
\ No newline at end of file
diff --git a/python/ftl/ftltypes.py b/python/ftl/ftltypes.py
new file mode 100644
index 0000000000000000000000000000000000000000..b72f5606c15fcbcb63f30d90f54d0d59dd25fa2a
--- /dev/null
+++ b/python/ftl/ftltypes.py
@@ -0,0 +1,70 @@
+
+from collections import namedtuple
+from enum import IntEnum
+
+# components/rgbd-sources/include/ftl/rgbd/camera.hpp
+Camera = namedtuple("Camera", ["fx", "fy", "cx", "cy", "width", "height",
+ "min_depth", "max_depth", "baseline", "doff"])
+
+# components/codecs/include/ftl/codecs/packet.hpp
+Packet = namedtuple("Packet", ["codec", "definition", "block_total",
+ "block_number", "flags", "data"])
+
+StreamPacket = namedtuple("StreamPacket", ["timestamp", "streamID",
+ "chanel_count", "channel"])
+
+# components/codecs/include/ftl/codecs/channels.hpp
+class Channel(IntEnum):
+ None_ = -1
+ Colour = 0
+ Left = 0
+ Depth = 1
+ Right = 2
+ Colour2 = 2
+ Disparity = 3
+ Depth2 = 3
+ Deviation = 4
+ Normals = 5
+ Points = 6
+ Confidence = 7
+ Contribution = 7
+ EnergyVector = 8
+ Flow = 9
+ Energy = 10
+ Mask = 11
+ Density = 12
+ LeftGray = 13
+ RightGray = 14
+ Overlay1 = 15
+
+ AudioLeft = 32
+ AudioRight = 33
+
+ Configuration = 64
+ Calibration = 65
+ Pose = 66
+ Data = 67
+
+# components/codecs/include/ftl/codecs/bitrates.hpp
+class codec_t(IntEnum):
+ JPG = 0
+ PNG = 1
+ H264 = 2
+ HEVC = 3
+ WAV = 4
+ JSON = 100
+ CALIBRATION = 101
+ POSE = 102
+ RAW = 103
+
+definition_t = {
+ 0 : (7680, 4320),
+ 1 : (2160, 3840),
+ 2 : (1080, 1920),
+ 3 : (720, 1280),
+ 4 : (576, 1024),
+ 5 : (480, 854),
+ 6 : (360, 640),
+ 7 : (0, 0),
+ 8 : (2056, 1852)
+}
diff --git a/python/ftl/libde265.py b/python/ftl/libde265.py
index 5cab9f0cc5deaea6ac63c49cab7218073be17970..afa663640191b2b2ec64e4b5ab04c85ad5ea575a 100644
--- a/python/ftl/libde265.py
+++ b/python/ftl/libde265.py
@@ -29,11 +29,15 @@ import numpy as np
import os
+'''
# default number of worker threads for decoder: half of os.cpu_count()
_threads = os.cpu_count() // 2
if _threads is None:
_threads = 1
+'''
+
+_threads = 1
# error codes copied from header (de265.h)
diff --git a/python/ftl/misc.py b/python/ftl/misc.py
new file mode 100644
index 0000000000000000000000000000000000000000..791dccf141d8129a03b1221f524fef03d0d96b42
--- /dev/null
+++ b/python/ftl/misc.py
@@ -0,0 +1,92 @@
+
+def disparity_to_depth(disparity, camera):
+ ''' Calculate depth map from disparity map '''
+ return (camera.fx * camera.baseline) / (disparity - camera.doff)
+
+from enum import IntEnum
+
+# components/codecs/include/ftl/codecs/hevc.hpp
+class NALType(IntEnum):
+ CODED_SLICE_TRAIL_N = 0
+ CODED_SLICE_TRAIL_R = 1
+
+ CODED_SLICE_TSA_N = 2
+ CODED_SLICE_TSA_R = 3
+
+ CODED_SLICE_STSA_N = 4
+ CODED_SLICE_STSA_R = 5
+
+ CODED_SLICE_RADL_N = 6
+ CODED_SLICE_RADL_R = 7
+
+ CODED_SLICE_RASL_N = 8
+ CODED_SLICE_RASL_R = 9
+
+ RESERVED_VCL_N10 = 10
+ RESERVED_VCL_R11 = 11
+ RESERVED_VCL_N12 = 12
+ RESERVED_VCL_R13 = 13
+ RESERVED_VCL_N14 = 14
+ RESERVED_VCL_R15 = 15
+
+ CODED_SLICE_BLA_W_LP = 16
+ CODED_SLICE_BLA_W_RADL = 17
+ CODED_SLICE_BLA_N_LP = 18
+ CODED_SLICE_IDR_W_RADL = 19
+ CODED_SLICE_IDR_N_LP = 20
+ CODED_SLICE_CRA = 21
+ RESERVED_IRAP_VCL22 = 22
+ RESERVED_IRAP_VCL23 = 23
+
+ RESERVED_VCL24 = 24
+ RESERVED_VCL25 = 25
+ RESERVED_VCL26 = 26
+ RESERVED_VCL27 = 27
+ RESERVED_VCL28 = 28
+ RESERVED_VCL29 = 29
+ RESERVED_VCL30 = 30
+ RESERVED_VCL31 = 31
+
+ VPS = 32
+ SPS = 33
+ PPS = 34
+ ACCESS_UNIT_DELIMITER = 35
+ EOS = 36
+ EOB = 37
+ FILLER_DATA = 38
+ PREFIX_SEI = 39
+ SUFFIX_SEI = 40
+
+ RESERVED_NVCL41 = 41
+ RESERVED_NVCL42 = 42
+ RESERVED_NVCL43 = 43
+ RESERVED_NVCL44 = 44
+ RESERVED_NVCL45 = 45
+ RESERVED_NVCL46 = 46
+ RESERVED_NVCL47 = 47
+ UNSPECIFIED_48 = 48
+ UNSPECIFIED_49 = 49
+ UNSPECIFIED_50 = 50
+ UNSPECIFIED_51 = 51
+ UNSPECIFIED_52 = 52
+ UNSPECIFIED_53 = 53
+ UNSPECIFIED_54 = 54
+ UNSPECIFIED_55 = 55
+ UNSPECIFIED_56 = 56
+ UNSPECIFIED_57 = 57
+ UNSPECIFIED_58 = 58
+ UNSPECIFIED_59 = 59
+ UNSPECIFIED_60 = 60
+ UNSPECIFIED_61 = 61
+ UNSPECIFIED_62 = 62
+ UNSPECIFIED_63 = 63
+ INVALID = 64
+
+def get_NAL_type(data):
+ if not isinstance(data, bytes):
+ raise ValueError("expected bytes")
+
+ return NALType((data[4] >> 1) & 0x3f)
+
+def is_iframe(data):
+ return get_NAL_type(data) == NALType.VPS