Blender plugin (export to .ftl)

SDK/Python/blender.py

+bl_info = {
+    "name": "FTL plugin",
+    "blender": (2, 80, 2),
+    "category": "Import-Export",
+}
+
+import bpy
+import numpy as np
+from mathutils import Matrix, Vector
+
+from ftl import Camera, FTLStreamWriter
+from ftl.types import Channel, Pipeline
+
+################################################################################
+# https://blender.stackexchange.com/a/120063
+# https://blender.stackexchange.com/questions/15102/what-is-blenders-camera-projection-matrix-model
+
+def get_sensor_size(sensor_fit, sensor_x, sensor_y):
+    if sensor_fit == 'VERTICAL':
+        return sensor_y
+    return sensor_x
+
+def get_sensor_fit(sensor_fit, size_x, size_y):
+    if sensor_fit == 'AUTO':
+        if size_x >= size_y:
+            return 'HORIZONTAL'
+        else:
+            return 'VERTICAL'
+    return sensor_fit
+
+
+def get_calibration_matrix_K_from_blender(camd):
+    if camd.type != 'PERSP':
+        raise ValueError('Non-perspective cameras not supported')
+
+    scene = bpy.context.scene
+    f_in_mm = camd.lens
+    scale = scene.render.resolution_percentage / 100
+    resolution_x_in_px = scale * scene.render.resolution_x
+    resolution_y_in_px = scale * scene.render.resolution_y
+    sensor_size_in_mm = get_sensor_size(camd.sensor_fit, camd.sensor_width, camd.sensor_height)
+    sensor_fit = get_sensor_fit(
+        camd.sensor_fit,
+        scene.render.pixel_aspect_x * resolution_x_in_px,
+        scene.render.pixel_aspect_y * resolution_y_in_px
+    )
+    pixel_aspect_ratio = scene.render.pixel_aspect_y / scene.render.pixel_aspect_x
+    if sensor_fit == 'HORIZONTAL':
+        view_fac_in_px = resolution_x_in_px
+    else:
+        view_fac_in_px = pixel_aspect_ratio * resolution_y_in_px
+    pixel_size_mm_per_px = sensor_size_in_mm / f_in_mm / view_fac_in_px
+    s_u = 1 / pixel_size_mm_per_px
+    s_v = 1 / pixel_size_mm_per_px / pixel_aspect_ratio
+
+    # Parameters of intrinsic calibration matrix K
+    u_0 = resolution_x_in_px / 2 - camd.shift_x * view_fac_in_px
+    v_0 = resolution_y_in_px / 2 + camd.shift_y * view_fac_in_px / pixel_aspect_ratio
+    skew = 0 # only use rectangular pixels
+
+    K = Matrix(
+        ((s_u, skew, u_0),
+        (   0,  s_v, v_0),
+        (   0,    0,   1)))
+    return K
+
+# Returns camera rotation and translation matrices from Blender.
+#
+# There are 3 coordinate systems involved:
+#    1. The World coordinates: "world"
+#       - right-handed
+#    2. The Blender camera coordinates: "bcam"
+#       - x is horizontal
+#       - y is up
+#       - right-handed: negative z look-at direction
+#    3. The desired computer vision camera coordinates: "cv"
+#       - x is horizontal
+#       - y is down (to align to the actual pixel coordinates
+#         used in digital images)
+#       - right-handed: positive z look-at direction
+def get_3x4_RT_matrix_from_blender(cam):
+    # bcam stands for blender camera
+    R_bcam2cv = Matrix(
+        ((1, 0,  0),
+        (0, -1, 0),
+        (0, 0, -1)))
+
+    # Transpose since the rotation is object rotation,
+    # and we want coordinate rotation
+    # R_world2bcam = cam.rotation_euler.to_matrix().transposed()
+    # T_world2bcam = -1*R_world2bcam * location
+    #
+    # Use matrix_world instead to account for all constraints
+    location, rotation = cam.matrix_world.decompose()[0:2]
+    R_world2bcam = rotation.to_matrix().transposed()
+
+    # Convert camera location to translation vector used in coordinate changes
+    # T_world2bcam = -1*R_world2bcam*cam.location
+    # Use location from matrix_world to account for constraints:
+    T_world2bcam = -1*R_world2bcam @ location
+
+    # Build the coordinate transform matrix from world to computer vision camera
+    R_world2cv = R_bcam2cv@R_world2bcam
+    T_world2cv = R_bcam2cv@T_world2bcam
+
+    # put into 3x4 matrix
+    RT = Matrix((
+        R_world2cv[0][:] + (T_world2cv[0],),
+        R_world2cv[1][:] + (T_world2cv[1],),
+        R_world2cv[2][:] + (T_world2cv[2],)
+        ))
+
+    return RT
+
+def get_ftl_calibration_from_blender(camd, d_min=0.0, d_max=np.inf, baseline=0.0, doff=0.0):
+
+    K = get_calibration_matrix_K_from_blender(camd)
+    scene = bpy.context.scene
+    scale = scene.render.resolution_percentage / 100
+    resolution_x_in_px = scale * scene.render.resolution_x
+    resolution_y_in_px = scale * scene.render.resolution_y
+
+    ftlcam = Camera(K[0][0], K[1][1], -K[0][2], -K[1][2],
+                    int(resolution_x_in_px), int(resolution_y_in_px),
+                    d_min, d_max, baseline, doff)
+
+    return ftlcam
+
+################################################################################
+
+import typing
+
+class StereoImage(typing.NamedTuple):
+    intrinsics: Camera
+    pose: np.array
+    imL: np.array
+    depthL: np.array
+    imR: np.array
+    depthR: np.array
+
+def render():
+    """ render active camera (image and depth) """
+
+    use_nodes = bpy.context.scene.use_nodes
+    bpy.context.scene.use_nodes = True
+    tree = bpy.context.scene.node_tree
+    links = tree.links
+
+    # possible issues with existing nodes of same type when accessing via bpy.data (?)
+    rl = tree.nodes.new('CompositorNodeRLayers')
+    v = tree.nodes.new('CompositorNodeViewer')
+    v.use_alpha = True
+
+    try:
+        links.new(rl.outputs['Image'], v.inputs['Image'])
+        # depth cannot be accessed in python; hack uses alpha to store z-values
+        links.new(rl.outputs['Depth'], v.inputs['Alpha'])
+
+        bpy.ops.render.render()
+
+        pixels = bpy.data.images['Viewer Node']
+        pix = np.array(pixels.pixels[:])
+
+        # sRGB conversion
+        #pix2 = np.zeros(pix.shape[:], dtype=np.float)
+        pix_srgb = np.copy(pix)
+        np.copyto(pix_srgb, 1.055*(pix**(1.0/2.4)) - 0.055, where=pix <= 1)
+        np.copyto(pix_srgb, pix * 12.92, where=pix <= 0.0031308)
+
+        # Clamp?
+        pix_srgb[pix_srgb > 1.0] = 1.0
+
+        im = pix_srgb.reshape((pixels.size[1], pixels.size[0], pixels.channels))[:,:,0:3]
+        depth = np.array(pixels.pixels[:]).reshape((pixels.size[1], pixels.size[0], pixels.channels))[:,:,3]
+
+        # set invalid depth values to 0.0
+        d_max = 65504.0
+        depth[depth >= d_max] = 0.0
+
+        return im, depth
+
+    finally:
+        tree.nodes.remove(v)
+        tree.nodes.remove(rl)
+        bpy.context.scene.use_nodes = use_nodes
+
+def render_stereo(camera, baseline=0.15):
+    camera_old = bpy.context.scene.camera
+    try:
+        bpy.context.scene.camera = camera
+        imL, depthL = render()
+
+        location_old = camera.location.copy()
+        try:
+            camera.location = (camera.matrix_world @ Vector((baseline, 0.0, 0.0, 1.0)))[0:3]
+            imR, depthR = render()
+
+        finally:
+            camera.location = location_old
+
+    finally:
+        bpy.context.scene.camera = camera_old
+
+    d_max = max(np.max(depthL), np.max(depthR))
+    pose = np.identity(4,dtype=np.float32)
+    pose[0:3,0:4] = get_3x4_RT_matrix_from_blender(camera)
+    pose = np.linalg.inv(pose.T)
+
+    ftlcamera = get_ftl_calibration_from_blender(camera.data, baseline=baseline, d_max=d_max)
+
+    return StereoImage(ftlcamera, np.array(pose), imL, depthL, imR, depthR)
+
+################################################################################
+
+class FTL_Options(bpy.types.PropertyGroup):
+
+    file_path : bpy.props.StringProperty(name="File",
+                                         description="Output file",
+                                         default="./blender.ftl",
+                                         maxlen=1024,
+                                         subtype="FILE_PATH")
+
+    baseline : bpy.props.FloatProperty(name="Baseline",
+                                       description="Distance between cameras (x-direction relative to camera)",
+                                       default=0.15)
+
+    use_sgm : bpy.props.BoolProperty(name="Use SGM",
+                                     description="Calculate disparity using SGM",
+                                     default=False)
+
+
+    use_ground_truth : bpy.props.BoolProperty(name="Save as ground truth",
+                                              description="Save depth in Ground Truth instead of Depth channel.",
+                                              default=True)
+
+    mask_occlusions : bpy.props.BoolProperty(name="Mask occlusions",
+                                             description="Right camera depth is used to mask occluded pixels.",
+                                             default=True)
+
+    cameras : bpy.props.EnumProperty(
+        name = "Cameras",
+        description = "Cameras for rendering",
+        items = [
+            ("ACTIVE", "Active camera", "Only use active camera"),
+            ("SELECTED", "Selected cameras", "Use all selected cameras"),
+            ("ALL", "All cameras", "Use all available cameras")
+        ],
+        default = "ACTIVE"
+    )
+
+import ftl
+class FTL_OT_Operator(bpy.types.Operator):
+    bl_idname = "scene.ftl_operator"
+    bl_label = "FTL Operator"
+
+    def execute(self, context):
+        options = context.scene.ftl_options
+        writer = FTLStreamWriter(options.file_path)
+
+        if options.use_sgm:
+            writer.enable_pipeline(Pipeline.DEPTH)
+
+        cameras = []
+
+        if options.cameras == 'ACTIVE':
+            cameras.append(bpy.context.scene.camera)
+
+        elif options.cameras == 'SELECTED':
+            for obj in context.selected_objects:
+                if obj.type != 'CAMERA':
+                    continue
+                cameras.append(obj)
+
+        elif options.cameras == 'ALL':
+            for obj in context.scene.objects:
+                if obj.type != 'CAMERA':
+                    continue
+                cameras.append(obj)
+
+        depth_channel = Channel.Depth if not options.use_ground_truth else Channel.GroundTruth
+        baseline = options.baseline
+
+        for i, camera in enumerate(cameras):
+            res = render_stereo(camera, baseline)
+            writer.write(i, Channel.Calibration, res.intrinsics)
+            writer.write(i, Channel.Pose, res.pose)
+            writer.write(i, Channel.Left, res.imL)
+            writer.write(i, Channel.Right, res.imR)
+            writer.write(i, depth_channel, res.depthL)
+
+            if options.mask_occlusions:
+                writer.mask_occlusion(i, depth_channel, res.depthR)
+
+        print("saved to %s" % options.file_path)
+        return {'FINISHED'}
+
+class FTL_PT_Panel(bpy.types.Panel):
+    bl_label = "FTL Export"
+    bl_idname = "FTL_PT_ftl"
+    bl_space_type = 'PROPERTIES'
+    bl_region_type = 'WINDOW'
+    bl_context = "output"
+
+    def draw(self, context):
+        layout = self.layout
+
+        ftl_options = context.scene.ftl_options
+
+        row = layout.row()
+        row.prop(ftl_options, "file_path")
+
+        row = layout.row()
+        row.prop(ftl_options, "baseline")
+
+        row = layout.row()
+        row.prop(ftl_options, "cameras")
+
+        row = layout.row()
+        row.prop(ftl_options, "use_sgm")
+
+        row = layout.row()
+        row.prop(ftl_options, "use_ground_truth")
+
+        row = layout.row()
+        row.prop(ftl_options, "mask_occlusions")
+
+        row = layout.row()
+        row.operator("scene.ftl_operator", text="Generate")
+
+classes = (FTL_Options,
+           FTL_OT_Operator,
+           FTL_PT_Panel)
+
+def register():
+    for cls in classes:
+        bpy.utils.register_class(cls)
+    bpy.types.Scene.ftl_options = bpy.props.PointerProperty(type=FTL_Options)
+
+def unregister():
+    for cls in classes:
+        bpy.utils.unregister_class(cls)
+    del bpy.types.Scene.ftl_options
+
+if __name__ == "__main__":
+    register()

SDK/Python/ftl/__init__.py

+from . streamwriter import FTLStreamWriter
+from . types import Camera
+import types
\ No newline at end of file

SDK/Python/ftl/streamwriter.py

+from . types import Channel, is_float_channel, Camera, Pipeline
+import numpy as np
+from enum import IntEnum
+
+import ctypes
+
+import os.path
+
+_paths = [
+    ".",
+    "/usr/lib",
+    "/usr/local/lib",
+]
+
+class _imageformat_t(IntEnum):
+	FLOAT     = 0
+	BGRA      = 1
+	RGBA      = 2
+	RGB       = 3
+	BGR       = 4
+	RGB_FLOAT = 5
+
+_libpath = None
+for p in _paths:
+    p = os.path.join(p, "libftl-dev.so")
+    if os.path.exists(p):
+        _libpath = p
+        break
+
+if _libpath is None:
+    raise FileNotFoundError("libftl-dev.so not found")
+
+_c_api = ctypes.CDLL(_libpath)
+
+_c_api.ftlCreateWriteStream.restype = ctypes.c_void_p
+_c_api.ftlCreateWriteStream.argtypes = [ctypes.c_char_p]
+
+_c_api.ftlIntrinsicsWriteLeft.restype = ctypes.c_int
+_c_api.ftlIntrinsicsWriteLeft.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float]
+
+_c_api.ftlIntrinsicsWriteRight.restype = ctypes.c_int
+_c_api.ftlIntrinsicsWriteRight.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float, ctypes.c_float]
+
+_c_api.ftlImageWrite.restype = ctypes.c_int
+_c_api.ftlImageWrite.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_void_p]
+
+_c_api.ftlPoseWrite.restype = ctypes.c_int
+_c_api.ftlPoseWrite.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_void_p]
+
+_c_api.ftlRemoveOcclusion.restype = ctypes.c_int
+_c_api.ftlRemoveOcclusion.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_void_p]
+
+_c_api.ftlMaskOcclusion.restype = ctypes.c_int
+_c_api.ftlMaskOcclusion.argtypes = [ctypes.c_void_p, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_void_p]
+
+_c_api.ftlEnablePipeline.restype = ctypes.c_int
+_c_api.ftlEnablePipeline.argtypes = [ctypes.c_void_p, ctypes.c_int]
+
+_c_api.ftlDisablePipeline.restype = ctypes.c_int
+_c_api.ftlDisablePipeline.argtypes = [ctypes.c_void_p, ctypes.c_int]
+
+_c_api.ftlSelect.restype = ctypes.c_int
+_c_api.ftlSelect.argtypes = [ctypes.c_void_p, ctypes.c_int]
+
+_c_api.ftlNextFrame.restype = ctypes.c_int
+_c_api.ftlNextFrame.argtypes = [ctypes.c_void_p]
+
+_c_api.ftlDestroyStream.restype = ctypes.c_int
+_c_api.ftlDestroyStream.argtypes = [ctypes.c_void_p]
+
+def _ftl_check(retval):
+    if retval != 0:
+        raise Exception("FTL api returned %i" % retval)
+
+class FTLStreamWriter:
+    def __init__(self, fname):
+        self._sources = {}
+        self._instance = _c_api.ftlCreateWriteStream(bytes(fname, "utf8"))
+
+        if self._instance is None:
+            raise Exception("Error: ftlCreateWriteStream")
+
+    def __del__(self):
+        if self._instance is not None:
+            _c_api.ftlDestroyStream(self._instance)
+
+    def _check_image(self, source, channel, data):
+        """ Check image is has correct number of channels and correct size
+             and convert to compatible datatype if necessary. Raises
+             TypeError/ValueError on failure.
+        """
+
+        if not type(data) is np.ndarray:
+            raise TypeError("Data must be in numpy array")
+
+        if source not in self._sources:
+            raise ValueError("Source must be configured before adding images")
+
+        if len(data.shape) not in [2, 3]:
+            raise ValueError("Unsupported array shape %s" % str(data.shape))
+
+        height, width = self._sources[source].height, self._sources[source].width
+        if data.shape[0] != height or data.shape[1] != width:
+            # TODO: this can happen (depth and color), requires support in C api
+            raise ValueError("Image size different than previously configured")
+
+        if is_float_channel(channel):
+            data = data.astype(np.float32)
+            ftl_dtype = _imageformat_t.FLOAT
+
+        else:
+            if len(data.shape) == 2:
+                raise ValueError("Expected multi-channel image for channel %s" % str(channel))
+
+            nchans = data.shape[2]
+            if data.dtype in [np.float32, np.float64]:
+                data = data.astype(np.float32)
+                ftl_dtype = _imageformat_t.RGB_FLOAT
+                if nchans != 3:
+                    raise ValueError("Unsupported number of channels: %i" % nchans)
+
+            elif data.dtype in [np.int8, np.uint8]:
+                if nchans == 3:
+                    ftl_dtype = _imageformat_t.RGB
+                elif nchans == 4:
+                    ftl_dtype = _imageformat_t.RGBA
+                else:
+                    raise ValueError("Unsupported number of channels: %i" % nchans)
+
+            else:
+                raise ValueError ("Unsupported numpy data type")
+
+        data = np.ascontiguousarray(data)
+        return data, ftl_dtype
+
+    def _write_image(self, source, channel, data):
+        """ Wrapper for ftlImageWrite """
+
+        data, ftl_dtype = self._check_image(source, channel, data)
+        _ftl_check(_c_api.ftlImageWrite(self._instance, ctypes.c_int(source),
+                                        channel, ftl_dtype, 0,
+                                        data.ctypes.data_as(ctypes.c_void_p)))
+
+    def _write_calibration(self, source, channel, camera):
+        """ Wrapper for ftlIntrinsicsWriteLeft and ftlIntrinsicsWriteRight """
+
+        # TODO: type checks for camera
+
+        func = _c_api.ftlIntrinsicsWriteLeft if channel == Channel.Calibration else _c_api.ftlIntrinsicsWriteRight
+
+        _ftl_check(func(self._instance, source, camera.width, camera.height,
+                        camera.fx, camera.cx, camera.cy, camera.baseline,
+                        camera.min_depth, camera.max_depth))
+
+        self._sources[source] = camera
+
+    def _write_pose(self, source, channel, pose):
+        """ Wrapper for ftlftlPoseWrite """
+
+        if type(pose) not in [np.ndarray, np.matrix]:
+            raise TypeError("Data must be in numpy array or matrix")
+
+        if len(pose.shape) != 2 or pose.shape[0] != 4 or pose.shape[1] != 4:
+            raise ValueError("Pose must be a 4x4 matrix")
+
+        pose = np.ascontiguousarray(pose.astype(np.float32).T)
+        _ftl_check(_c_api.ftlPoseWrite(self._instance, source, pose.ctypes.data_as(ctypes.c_void_p)))
+
+    def write(self, source, channel, data):
+        """ Write data to stream """
+
+        if channel in [Channel.Calibration, Channel.Calibration2]:
+            self._write_calibration(source, channel, data)
+        elif channel == Channel.Pose:
+            self._write_pose(source, channel, data)
+        else:
+            self._write_image(source, channel, data)
+
+    def enable_pipeline(self, t):
+        if t not in Pipeline:
+            raise ValueError("Unknown pipeline")
+
+        _ftl_check(_c_api.ftlEnablePipeline(self._instance, t))
+
+    def disable_pipeline(self, t):
+        if t not in Pipeline:
+            raise ValueError("Unknown pipeline")
+
+        _ftl_check(_c_api.ftlDisablePipeline(self._instance, t))
+
+    def mask_occlusion(self, source, channel, data):
+        data, ftl_dtype = self._check_image(source, channel, data)
+
+        if not is_float_channel(channel):
+            raise ValueError("Bad channel")
+
+        if len(data.shape) != 2:
+            raise ValueError("Wrong number of channels")
+
+        _ftl_check(_c_api.ftlMaskOcclusion(self._instance, source, channel, 0,
+                                           data.ctypes.data_as(ctypes.c_void_p)))
+
+    def next_frame(self):
+        _ftl_check(_c_api.ftlNextFrame(self._instance))

SDK/Python/ftl/types.py

+from typing import NamedTuple
+from enum import IntEnum
+
+class Pipeline(IntEnum):
+	DEPTH = 0
+	RECONSTRUCT = 1
+
+# components/rgbd-sources/include/ftl/rgbd/camera.hpp
+class Camera(NamedTuple):
+    fx : float
+    fy : float
+    cx : float
+    cy : float
+    width : int
+    height : int
+    min_depth : float
+    max_depth : float
+    baseline : float
+    doff : float
+
+# components/codecs/include/ftl/codecs/channels.hpp
+class Channel(IntEnum):
+    None_           = -1
+    Colour			= 0	# 8UC3 or 8UC4
+    Left			= 0
+    Depth			= 1	# 32S or 32F
+    Right			= 2	# 8UC3 or 8UC4
+    Colour2			= 2
+    Depth2			= 3
+    Deviation		= 4
+    Screen			= 4
+    Normals			= 5	# 16FC4
+    Weights			= 6	# short
+    Confidence		= 7	# 32F
+    Contribution	= 7	# 32F
+    EnergyVector	= 8	# 32FC4
+    Flow			= 9	# 32F
+    Energy			= 10	# 32F
+    Mask			= 11	# 32U
+    Density			= 12	# 32F
+    Support1		= 13	# 8UC4 (currently)
+    Support2		= 14	# 8UC4 (currently)
+    Segmentation	= 15	# 32S?
+    Normals2		= 16	# 16FC4
+    ColourHighRes	= 17	# 8UC3 or 8UC4
+    LeftHighRes		= 17	# 8UC3 or 8UC4
+    Disparity		= 18
+    Smoothing		= 19	# 32F
+    RightHighRes	= 20	# 8UC3 or 8UC4
+    Colour2HighRes	= 20
+    Overlay			= 21   # 8UC4
+    GroundTruth		= 22	# 32F
+
+    Audio			= 32
+    AudioMono		= 32
+    AudioStereo		= 33
+
+    Configuration	= 64	# JSON Data
+    Settings1		= 65
+    Calibration		= 65	# Camera Parameters Object
+    Pose			= 66	# Eigen::Matrix4d
+    Settings2		= 67
+    Calibration2	= 67	# Right camera parameters
+    Index           = 68
+    Control			= 69	# For stream and encoder control
+    Settings3		= 70
+
+    Data			= 2048	# Custom data any codec.
+    Faces			= 2049 # Data about detected faces
+    Transforms		= 2050	# Transformation matrices for framesets
+    Shapes3D		= 2051	# Labeled 3D shapes
+    Messages		= 2052	# Vector of Strings
+
+_float_channels = [
+    Channel.Depth,
+    Channel.Confidence,
+    Channel.Density,
+    Channel.Energy,
+    Channel.GroundTruth,
+	Channel.Flow
+]
+
+def is_float_channel(channel):
+    return channel in _float_channels