Merge branch 'feature/cwrapper' into 'master'

C SDK for writing FTL files

See merge request nicolas.pope/ftl!289

CMakeLists.txt

@@ -4,7 +4,7 @@ include (CheckIncludeFileCXX)
 include (CheckFunctionExists)
 include(CheckLanguage)
 
-project (ftl.utu.fi)
+project (ftl.utu.fi VERSION 0.0.4)
 
 include(GNUInstallDirs)
 include(CTest)
@@ -263,7 +263,7 @@ endif()
 check_language(CUDA)
 if (CUDA_TOOLKIT_ROOT_DIR)
 enable_language(CUDA)
-set(CMAKE_CUDA_FLAGS "")
+set(CMAKE_CUDA_FLAGS "-Xcompiler -fPIC")
 set(CMAKE_CUDA_FLAGS_DEBUG "--gpu-architecture=compute_61 -g -DDEBUG -D_DEBUG")
 set(CMAKE_CUDA_FLAGS_RELEASE "--gpu-architecture=compute_61")
 set(HAVE_CUDA TRUE)
@@ -352,7 +352,7 @@ if (WIN32) # TODO(nick) Should do based upon compiler (VS)
 	set(OS_LIBS "")
 else()
 	add_definitions(-DUNIX)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -msse3 -Werror -Wall")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -fPIC -msse3 -Werror -Wall")
 	set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG -pg")
 	set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mfpmath=sse")
 	set(OS_LIBS "dl")
@@ -378,6 +378,11 @@ add_subdirectory(components/calibration)
 #add_subdirectory(applications/merger)
 add_subdirectory(applications/tools)
 
+# SDK only compiles on linux currently
+if (NOT WIN32)
+	add_subdirectory(SDK/C)
+endif()
+
 if (HAVE_AVFORMAT)
 	add_subdirectory(applications/ftl2mkv)
 endif()

SDK/C/CMakeLists.txt

+set(SDKSRC
+	src/common.cpp
+	src/streams.cpp
+)
+
+add_library(ftl-dev SHARED ${SDKSRC})
+set_target_properties(ftl-dev PROPERTIES VERSION ${PROJECT_VERSION})
+set_target_properties(ftl-dev PROPERTIES SOVERSION 0)
+
+target_include_directories(ftl-dev PUBLIC
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+	$<INSTALL_INTERFACE:include>
+	PRIVATE src)
+
+target_link_libraries(ftl-dev ftlcommon ftlrgbd ftlstreams Threads::Threads ${OpenCV_LIBS} ftlnet)
+
+install(TARGETS ftl-dev
+    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+	PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+	
+add_subdirectory(examples/image_write)
+add_subdirectory(examples/video_write)

SDK/C/examples/image_write/CMakeLists.txt

+add_executable(image_write main.cpp)
+target_link_libraries(image_write ftl-dev)

SDK/C/examples/image_write/main.cpp

+#include <ftl/ftl.h>
+#include <opencv2/core/mat.hpp>
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
+#include <Eigen/Eigen>
+
+static void ftlCheck(ftlError_t err) {
+	if (err != FTLERROR_OK) {
+		LOG(ERROR) << "FTL Stream Error: " << err;
+		exit(-1);
+	}
+}
+
+int main(int argc, char **argv) {
+	ftlStream_t s = ftlCreateWriteStream("./out.ftl");
+	if (!s) ftlCheck(ftlGetLastStreamError(s));
+
+	// Two test images, red and green
+	cv::Mat test_image1(720, 1280, CV_8UC4, cv::Scalar(0,0,255,255));
+	cv::Mat test_image2(720, 1280, CV_8UC4, cv::Scalar(0,255,0,255));
+
+	// Two test depth maps
+	cv::Mat test_depth1(720, 1280, CV_32F, cv::Scalar(3.0f));
+	cv::Mat test_depth2(720, 1280, CV_32F, cv::Scalar(2.0f));
+
+	// Write red image
+	ftlCheck(ftlIntrinsicsWriteLeft(s, 0, 1280, 720, 300.0f, -1280.0f/2.0f, -720.0f/2.0f, 0.1f, 0.1f, 8.0f));
+	ftlCheck(ftlImageWrite(s, 0, FTLCHANNEL_Colour, FTLIMAGE_BGRA, test_image1.step, test_image1.data));
+
+	// Write green image
+	ftlCheck(ftlIntrinsicsWriteLeft(s, 1, 1280, 720, 300.0f, -1280.0f/2.0f, -720.0f/2.0f, 0.1f, 0.1f, 8.0f));
+	ftlCheck(ftlImageWrite(s, 1, FTLCHANNEL_Colour, FTLIMAGE_BGRA, test_image2.step, test_image2.data));
+
+	// Write depth images
+	ftlCheck(ftlImageWrite(s, 0, FTLCHANNEL_Depth, FTLIMAGE_FLOAT, test_depth1.step, test_depth1.data));
+	ftlCheck(ftlImageWrite(s, 1, FTLCHANNEL_Depth, FTLIMAGE_FLOAT, test_depth2.step, test_depth2.data));
+
+	// Set pose for second source
+	Eigen::Translation3f trans(1.0f, 0.5f, 0.0f);
+	Eigen::Affine3f t(trans);
+	Eigen::Matrix4f viewPose = t.matrix();
+	ftlCheck(ftlPoseWrite(s, 1, viewPose.data()));
+
+	ftlCheck(ftlDestroyStream(s));
+
+	return 0;
+}

SDK/C/examples/video_write/CMakeLists.txt

+add_executable(video_write main.cpp)
+target_link_libraries(video_write ftl-dev)

SDK/C/examples/video_write/main.cpp

+#include <ftl/ftl.h>
+#include <opencv2/core/mat.hpp>
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
+#include <Eigen/Eigen>
+
+static void ftlCheck(ftlError_t err) {
+	if (err != FTLERROR_OK) {
+		LOG(ERROR) << "FTL Stream Error: " << err;
+		exit(-1);
+	}
+}
+
+int main(int argc, char **argv) {
+	ftlStream_t s = ftlCreateWriteStream("./out.ftl");
+	if (!s) ftlCheck(ftlGetLastStreamError(s));
+
+	ftlCheck(ftlSetFrameRate(s, 20.0f));
+
+	// Two test frames, red and green
+	cv::Mat test_image1(720, 1280, CV_8UC4, cv::Scalar(0,0,255,255));
+	cv::Mat test_image2(720, 1280, CV_8UC4, cv::Scalar(0,255,0,255));
+
+	ftlCheck(ftlIntrinsicsWriteLeft(s, 0, 1280, 720, 300.0f, -1280.0f/2.0f, -720.0f/2.0f, 0.1f, 0.1f, 8.0f));
+
+	// Write a number of frames, alternating images
+	for (int i=0; i<100; ++i) {
+		if (i&1) {
+			ftlCheck(ftlImageWrite(s, 0, FTLCHANNEL_Colour, FTLIMAGE_BGRA, test_image2.step, test_image2.data));
+		} else {
+			ftlCheck(ftlImageWrite(s, 0, FTLCHANNEL_Colour, FTLIMAGE_BGRA, test_image1.step, test_image1.data));
+		}
+
+		ftlCheck(ftlNextFrame(s));
+	}
+	
+
+	ftlCheck(ftlDestroyStream(s));
+
+	return 0;
+}

SDK/C/include/ftl/ftl.h

+#ifndef _FTL_SDK_HPP_
+#define _FTL_SDK_HPP_
+
+#ifdef __cplusplus
+#include <cstdint>
+#else
+#include <stdint.h>
+#endif
+
+struct FTLStream;
+typedef FTLStream* ftlStream_t;
+
+
+enum ftlError_t {
+	FTLERROR_OK,
+	FTLERROR_UNKNOWN,
+	FTLERROR_STREAM_READONLY,
+	FTLERROR_STREAM_WRITEONLY,
+	FTLERROR_STREAM_NO_FILE,
+	FTLERROR_STREAM_LISTEN_FAILED,
+	FTLERROR_STREAM_FILE_CREATE_FAILED,
+	FTLERROR_STREAM_NET_CREATE_FAILED,
+	FTLERROR_STREAM_INVALID_STREAM,
+	FTLERROR_STREAM_INVALID_PARAMETER,
+	FTLERROR_STREAM_ENCODE_FAILED,
+	FTLERROR_STREAM_DECODE_FAILED,
+	FTLERROR_STREAM_BAD_CHANNEL,
+	FTLERROR_STREAM_BAD_TIMESTAMP,
+	FTLERROR_STREAM_BAD_URI,
+	FTLERROR_STREAM_BAD_IMAGE_TYPE,
+	FTLERROR_STREAM_BAD_DATA,
+	FTLERROR_STREAM_BAD_IMAGE_SIZE,
+	FTLERROR_STREAM_NO_INTRINSICS,
+	FTLERROR_STREAM_NO_DATA,
+	FTLERROR_STREAM_DUPLICATE
+};
+
+enum ftlChannel_t {
+	FTLCHANNEL_None				= -1,
+	FTLCHANNEL_Colour			= 0,	// 8UC3 or 8UC4
+	FTLCHANNEL_Left				= 0,
+	FTLCHANNEL_Depth			= 1,	// 32S or 32F
+	FTLCHANNEL_Right			= 2,	// 8UC3 or 8UC4
+	FTLCHANNEL_Colour2			= 2,
+	FTLCHANNEL_Depth2			= 3,
+	FTLCHANNEL_Deviation		= 4,
+	FTLCHANNEL_Screen			= 4,
+	FTLCHANNEL_Normals			= 5,	// 16FC4
+	FTLCHANNEL_Weights			= 6,	// short
+	FTLCHANNEL_Confidence		= 7,	// 32F
+	FTLCHANNEL_Contribution		= 7,	// 32F
+	FTLCHANNEL_EnergyVector		= 8,	// 32FC4
+	FTLCHANNEL_Flow				= 9,	// 32F
+	FTLCHANNEL_Energy			= 10,	// 32F
+	FTLCHANNEL_Mask				= 11,	// 32U
+	FTLCHANNEL_Density			= 12,	// 32F
+	FTLCHANNEL_Support1			= 13,	// 8UC4 (currently)
+	FTLCHANNEL_Support2			= 14,	// 8UC4 (currently)
+	FTLCHANNEL_Segmentation		= 15,	// 32S?
+	FTLCHANNEL_Normals2			= 16,	// 16FC4
+	FTLCHANNEL_ColourHighRes	= 17,	// 8UC3 or 8UC4
+	FTLCHANNEL_LeftHighRes		= 17,	// 8UC3 or 8UC4
+	FTLCHANNEL_Disparity		= 18,
+	FTLCHANNEL_Smoothing		= 19,	// 32F
+	FTLCHANNEL_RightHighRes		= 20,	// 8UC3 or 8UC4
+	FTLCHANNEL_Colour2HighRes	= 20,
+	FTLCHANNEL_Overlay			= 21,   // 8UC4
+	FTLCHANNEL_GroundTruth		= 22,	// 32F
+
+	FTLCHANNEL_Audio			= 32,
+	FTLCHANNEL_AudioMono		= 32,
+	FTLCHANNEL_AudioStereo		= 33,
+
+	FTLCHANNEL_Configuration	= 64,	// JSON Data
+	FTLCHANNEL_Settings1		= 65,
+	FTLCHANNEL_Calibration		= 65,	// Camera Parameters Object
+	FTLCHANNEL_Pose				= 66,	// Eigen::Matrix4d
+	FTLCHANNEL_Settings2		= 67,
+	FTLCHANNEL_Calibration2		= 67,	// Right camera parameters
+	FTLCHANNEL_Index           	= 68,
+	FTLCHANNEL_Control			= 69,	// For stream and encoder control
+	FTLCHANNEL_Settings3		= 70,
+
+	FTLCHANNEL_Data				= 2048,	// Custom data, any codec.
+	FTLCHANNEL_Faces			= 2049, // Data about detected faces
+	FTLCHANNEL_Transforms		= 2050,	// Transformation matrices for framesets
+	FTLCHANNEL_Shapes3D			= 2051,	// Labeled 3D shapes
+	FTLCHANNEL_Messages			= 2052	// Vector of Strings
+};
+
+enum ftlImageFormat_t {
+	FTLIMAGE_FLOAT,
+	FTLIMAGE_BGRA,
+	FTLIMAGE_RGBA,
+	FTLIMAGE_RGB,
+	FTLIMAGE_BGR
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Get the last error for a particular stream. Or use NULL stream to get errors
+ * when creating streams.
+ */
+ftlError_t ftlGetLastStreamError(ftlStream_t stream);
+
+// ==== FTL Stream API =========================================================
+
+/**
+ * Create a new file or net stream from a URI. This is for writing or sending
+ * data and makes a write only stream.
+ */
+ftlStream_t ftlCreateWriteStream(const char *uri);
+
+/**
+ * Open an existing file or network stream from the URI. These streams are
+ * read only.
+ */
+ftlStream_t ftlCreateReadStream(const char *uri);
+
+/**
+ * Write raw image data to a frame. The width and height of
+ * the image contained in `data` are contained within the intrinsics data which
+ * must be written to a stream before calling `ftlImageWrite`. The `pitch`
+ * argument determines the number of bytes per row of the image, if set to 0
+ * the pitch is assumed to be width multiplied by size of pixel in bytes. Note
+ * that `ftlNextFrame` must be called before calling this again for the same
+ * source and channel.
+ * 
+ * @param stream As created with `ftlCreateWriteStream`
+ * @param sourceId Unique consecutive ID for a camera or source
+ * @param channel The image channel
+ * @param type The image format
+ * @param pitch Bytes per row, in case different from width x sizeof(type)
+ * @param data Raw image data, pitch x height bytes in size
+ * 
+ * @return FTLERROR_OK on success
+ */
+ftlError_t ftlImageWrite(
+	ftlStream_t stream,
+	int32_t sourceId,
+	ftlChannel_t channel,
+	ftlImageFormat_t type,
+	uint32_t pitch,
+	const void *data);
+
+/**
+ * Only for writing streams, this determines the timestamp interval between
+ * frames when `ftlNextFrame` is called. When reading an FTL file this is
+ * determined automatically. Default is 25 fps. Advised to call this only once
+ * before any call to `ftlNextFrame`.
+ * 
+ * @param stream As created with `ftlCreateWriteStream`
+ * @param fps Frames per second of video output
+ */
+ftlError_t ftlSetFrameRate(ftlStream_t stream, float fps);
+
+/**
+ * Move a write stream to the next frame. It is an error to call
+ * `ftlImageWrite` multiple times for the same source and channel without
+ * calling this function. This will also be used when reading FTL files.
+ */
+ftlError_t ftlNextFrame(ftlStream_t stream);
+
+/**
+ * Write of 4x4 transformation matrix into the stream for a given source. The
+ * `data` pointer must contain 16 float values packed and in Eigen::Matrix4f
+ * form.
+ */
+ftlError_t ftlPoseWrite(ftlStream_t stream, int32_t sourceId, const float *data);
+
+/**
+ * This should be called before any other function for a given `sourceId`.
+ * The width and height information here is used implicitely by other API calls.
+ */
+ftlError_t ftlIntrinsicsWriteLeft(ftlStream_t stream, int32_t sourceId, int32_t width, int32_t height, float f, float cx, float cy, float baseline, float minDepth, float maxDepth);
+
+/**
+ * Call this after the left intrinsics have been set.
+ */
+ftlError_t ftlIntrinsicsWriteRight(ftlStream_t stream, int32_t sourceId, int32_t width, int32_t height, float f, float cx, float cy, float baseline, float minDepth, float maxDepth);
+
+/**
+ * Close and destroy the stream, ensuring all read/write operations have
+ * completed. Network connections are terminated and files closed.
+ */
+ftlError_t ftlDestroyStream(ftlStream_t stream);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif

SDK/C/src/common.cpp

+#include <ftl/ftl.h>
+
+

SDK/C/src/streams.cpp

+#include <ftl/ftl.h>
+#include <ftl/uri.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/streams/sender.hpp>
+#include <ftl/streams/filestream.hpp>
+#include <ftl/streams/netstream.hpp>
+
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
+static ftlError_t last_error = FTLERROR_OK;
+static ftl::Configurable *root = nullptr;
+
+struct FTLStream {
+	bool readonly;
+	ftl::stream::Sender *sender;
+	ftl::stream::Stream *stream;
+	ftlError_t last_error;
+	int64_t interval;
+	bool has_fresh_data;
+
+	std::vector<ftl::rgbd::FrameState> video_states;
+	ftl::rgbd::FrameSet video_fs;
+};
+
+ftlError_t ftlGetLastStreamError(ftlStream_t stream) {
+	return (stream == nullptr) ? last_error : stream->last_error;
+}
+
+static void createFileWriteStream(FTLStream *s, const ftl::URI &uri) {
+	if (!root) {
+		int argc = 1;
+		const char *argv[] = {"SDK",0};
+		root = ftl::configure(argc, const_cast<char**>(argv), "sdk_default");
+	}
+
+	auto *fs = ftl::create<ftl::stream::File>(root, "ftlfile");
+	fs->set("filename", uri.getPath());
+	fs->setMode(ftl::stream::File::Mode::Write);
+	s->stream = fs;
+};
+
+ftlStream_t ftlCreateWriteStream(const char *uri) {
+	std::string uristr(uri);
+	ftl::URI u(uristr);
+
+	if (!u.isValid()) {
+		last_error = FTLERROR_STREAM_BAD_URI;
+		return nullptr;
+	}
+
+	FTLStream *s = new FTLStream;
+	s->last_error = FTLERROR_OK;
+	s->stream = nullptr;
+	s->sender = nullptr;
+	s->video_fs.id = 0;
+	s->video_fs.count = 0;
+	s->video_fs.mask = 0;
+	s->interval = 40;
+	s->video_fs.frames.reserve(32);
+	s->video_states.resize(32);
+
+	switch (u.getScheme()) {
+		case ftl::URI::SCHEME_FILE	: createFileWriteStream(s, u); break;
+		default						: last_error = FTLERROR_STREAM_BAD_URI;
+									  return nullptr;
+	}
+
+	if (s->last_error == FTLERROR_OK) {
+		s->sender = ftl::create<ftl::stream::Sender>(root, "sender");
+		s->sender->setStream(s->stream);
+		if (!s->stream->begin()) {
+			last_error = FTLERROR_STREAM_FILE_CREATE_FAILED;
+			return nullptr;
+		}
+	}
+	last_error = FTLERROR_OK;
+
+	s->video_fs.timestamp = ftl::timer::get_time();
+
+	return s;
+}
+
+ftlError_t ftlImageWrite(
+	ftlStream_t stream,
+	int32_t sourceId,
+	ftlChannel_t channel,
+	ftlImageFormat_t type,
+	uint32_t pitch,
+	const void *data)
+{
+	if (!stream || !stream->stream)
+		return FTLERROR_STREAM_INVALID_STREAM;
+	if (sourceId < 0 || sourceId >= 32)
+		return FTLERROR_STREAM_INVALID_PARAMETER;
+	if (static_cast<int>(channel) < 0 || static_cast<int>(channel) > 32)
+		return FTLERROR_STREAM_BAD_CHANNEL;
+	if (!stream->video_fs.hasFrame(sourceId))
+		return FTLERROR_STREAM_NO_INTRINSICS;
+	if (!data) return FTLERROR_STREAM_NO_DATA;
+	if (stream->video_fs.hasChannel(static_cast<ftl::codecs::Channel>(channel)))
+		return FTLERROR_STREAM_DUPLICATE;
+
+	try {
+		auto &frame = stream->video_fs.frames[sourceId];
+		auto &img = frame.create<cv::cuda::GpuMat>(static_cast<ftl::codecs::Channel>(channel));
+		auto &intrin = frame.getLeft();
+
+		if (intrin.width == 0) {
+			return FTLERROR_STREAM_NO_INTRINSICS;
+		}
+
+		switch (type) {
+		case FTLIMAGE_FLOAT		: img.upload(cv::Mat(intrin.height, intrin.width, CV_32F, const_cast<void*>(data), pitch)); break;
+		case FTLIMAGE_BGRA		: img.upload(cv::Mat(intrin.height, intrin.width, CV_8UC4, const_cast<void*>(data), pitch)); break;
+		case FTLIMAGE_RGBA		:
+		case FTLIMAGE_BGR		:
+		case FTLIMAGE_RGB		:
+		default					: return FTLERROR_STREAM_BAD_IMAGE_TYPE;
+		}
+
+		if (img.empty()) return FTLERROR_STREAM_NO_DATA;
+
+		ftl::codecs::Channels<0> channels;
+		if (stream->stream->size() > static_cast<unsigned int>(stream->video_fs.id)) channels = stream->stream->selected(stream->video_fs.id);
+		channels += static_cast<ftl::codecs::Channel>(channel);
+		stream->stream->select(stream->video_fs.id, channels, true);
+
+	} catch (const std::exception &e) {
+		return FTLERROR_UNKNOWN;
+	}
+
+	stream->has_fresh_data = true;
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlIntrinsicsWriteLeft(ftlStream_t stream, int32_t sourceId, int32_t width, int32_t height, float f, float cx, float cy, float baseline, float minDepth, float maxDepth) {
+	if (!stream || !stream->stream)
+		return FTLERROR_STREAM_INVALID_STREAM;
+	if (sourceId < 0 || sourceId >= 32)
+		return FTLERROR_STREAM_INVALID_PARAMETER;
+
+	while (stream->video_fs.frames.size() <= static_cast<unsigned int>(sourceId)) {
+		stream->video_fs.frames.emplace_back();
+	}
+
+	if (stream->video_fs.hasFrame(sourceId)) {
+		return FTLERROR_STREAM_DUPLICATE;
+	}
+
+	ftl::rgbd::Camera cam;
+	cam.fx = f;
+	cam.fy = f;
+	cam.cx = cx;
+	cam.cy = cy;
+	cam.width = width;
+	cam.height = height;
+	cam.minDepth = minDepth;
+	cam.maxDepth = maxDepth;
+	cam.baseline = baseline;
+	cam.doffs = 0.0f;
+	stream->video_fs.mask |= 1 << sourceId;
+	stream->video_fs.count++;
+	if (!stream->video_fs.frames[sourceId].origin()) {
+		stream->video_fs.frames[sourceId].setOrigin(&stream->video_states[sourceId]);
+	}
+	stream->video_fs.frames[sourceId].setLeft(cam);
+	stream->has_fresh_data = true;
+
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlIntrinsicsWriteRight(ftlStream_t stream, int32_t sourceId, int32_t width, int32_t height, float f, float cx, float cy, float baseline, float minDepth, float maxDepth) {
+	if (!stream || !stream->stream)
+		return FTLERROR_STREAM_INVALID_STREAM;
+	if (sourceId < 0 || sourceId >= 32)
+		return FTLERROR_STREAM_INVALID_PARAMETER;
+	if (!stream->video_fs.hasFrame(sourceId))
+		return FTLERROR_STREAM_NO_INTRINSICS;
+
+	ftl::rgbd::Camera cam;
+	cam.fx = f;
+	cam.fy = f;
+	cam.cx = cx;
+	cam.cy = cy;
+	cam.width = width;
+	cam.height = height;
+	cam.minDepth = minDepth;
+	cam.maxDepth = maxDepth;
+	cam.baseline = baseline;
+	cam.doffs = 0.0f;
+	stream->video_fs.frames[sourceId].setRight(cam);
+	stream->has_fresh_data = true;
+
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlPoseWrite(ftlStream_t stream, int32_t sourceId, const float *data) {
+	if (!stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (!stream->stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (sourceId < 0 || sourceId >= 32)
+		return FTLERROR_STREAM_INVALID_PARAMETER;
+	if (!stream->video_fs.hasFrame(sourceId))
+		return FTLERROR_STREAM_NO_INTRINSICS;
+	if (!data) return FTLERROR_STREAM_NO_DATA;
+
+	Eigen::Matrix4f pose;
+	for (int i=0; i<16; ++i) pose.data()[i] = data[i];
+
+	auto &frame = stream->video_fs.frames[sourceId];
+	frame.setPose(pose.cast<double>());
+
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlSetFrameRate(ftlStream_t stream, float fps) {
+	if (!stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (!stream->stream) return FTLERROR_STREAM_INVALID_STREAM;
+
+	stream->interval = int64_t(1000.0f / fps);
+
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlNextFrame(ftlStream_t stream) {
+	if (!stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (!stream->stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (!stream->has_fresh_data) return FTLERROR_STREAM_NO_DATA;
+
+	try {
+		stream->sender->post(stream->video_fs);
+	} catch (const std::exception &e) {
+		return FTLERROR_STREAM_ENCODE_FAILED;
+	}
+
+	// Reset the frameset.
+	for (size_t i=0; i<stream->video_fs.frames.size(); ++i) {
+		if (!stream->video_fs.hasFrame(i)) continue;
+
+		auto &f = stream->video_fs.frames[i];
+		f.reset();
+		f.setOrigin(&stream->video_states[i]);
+	}
+
+	// FIXME: These should be reset each time
+	//stream->video_fs.count = 0;
+	//stream->video_fs.mask = 0;
+	stream->video_fs.timestamp += stream->interval;
+	stream->has_fresh_data = false;
+	return FTLERROR_OK;
+}
+
+ftlError_t ftlDestroyStream(ftlStream_t stream) {
+	if (!stream) return FTLERROR_STREAM_INVALID_STREAM;
+	if (!stream->stream) return FTLERROR_STREAM_INVALID_STREAM;
+
+	ftlError_t err = FTLERROR_OK;
+
+	if (stream->has_fresh_data) {
+		try {
+			stream->sender->post(stream->video_fs);
+		} catch (const std::exception &e) {
+			err = FTLERROR_STREAM_ENCODE_FAILED;
+		}
+	}
+
+	if (!stream->stream->end()) {
+		err = FTLERROR_STREAM_FILE_CREATE_FAILED;
+	}
+	if (stream->sender) delete stream->sender;
+	delete stream->stream;
+	stream->sender = nullptr;
+	stream->stream = nullptr;
+	delete stream;
+	return err;
+}

components/streams/src/filestream.cpp

@@ -91,6 +91,8 @@ bool File::post(const ftl::codecs::StreamPacket &s, const ftl::codecs::Packet &p
 		return false;
 	}
 
+	LOG(INFO) << "WRITE: " << s.timestamp << " " << (int)s.channel << " " << p.data.size();
+
 	// Don't write dummy packets to files.
 	if (p.data.size() == 0) return true;
 

components/structures/include/ftl/data/frame.hpp

@@ -525,6 +525,7 @@ void ftl::data::Frame<BASE,N,STATE,DATA>::setPose(const Eigen::Matrix4d &pose, b
 		if (mark) origin_->setPose(pose);
 		else origin_->getPose() = pose;
 	}
+	state_.setPose(pose);
 }
 
 template <int BASE, int N, typename STATE, typename DATA>
@@ -535,11 +536,13 @@ void ftl::data::Frame<BASE,N,STATE,DATA>::patchPose(const Eigen::Matrix4d &pose)
 template <int BASE, int N, typename STATE, typename DATA>
 void ftl::data::Frame<BASE,N,STATE,DATA>::setLeft(const typename STATE::Settings &c) {
 	if (origin_) origin_->setLeft(c);
+	state_.setLeft(c);
 }
 
 template <int BASE, int N, typename STATE, typename DATA>
 void ftl::data::Frame<BASE,N,STATE,DATA>::setRight(const typename STATE::Settings &c) {
 	if (origin_) origin_->setRight(c);
+	state_.setRight(c);
 }
 
 template <int BASE, int N, typename STATE, typename DATA>