From 92565996b36d32c8c20eab4fe81f7e4ea9e446b4 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Sat, 14 Mar 2020 15:46:11 +0200
Subject: [PATCH] C SDK for writing FTL files
---
CMakeLists.txt | 11 +-
SDK/C/CMakeLists.txt | 22 ++
SDK/C/examples/image_write/CMakeLists.txt | 2 +
SDK/C/examples/image_write/main.cpp | 48 +++
SDK/C/examples/video_write/CMakeLists.txt | 2 +
SDK/C/examples/video_write/main.cpp | 42 +++
SDK/C/include/ftl/ftl.h | 195 +++++++++++++
SDK/C/src/common.cpp | 3 +
SDK/C/src/streams.cpp | 276 ++++++++++++++++++
components/streams/src/filestream.cpp | 2 +
.../structures/include/ftl/data/frame.hpp | 3 +
11 files changed, 603 insertions(+), 3 deletions(-)
create mode 100644 SDK/C/CMakeLists.txt
create mode 100644 SDK/C/examples/image_write/CMakeLists.txt
create mode 100644 SDK/C/examples/image_write/main.cpp
create mode 100644 SDK/C/examples/video_write/CMakeLists.txt
create mode 100644 SDK/C/examples/video_write/main.cpp
create mode 100644 SDK/C/include/ftl/ftl.h
create mode 100644 SDK/C/src/common.cpp
create mode 100644 SDK/C/src/streams.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 338ed2cb5..376b06b36 100644
--- a/CMakeLists.txt
+++ b/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()
diff --git a/SDK/C/CMakeLists.txt b/SDK/C/CMakeLists.txt
new file mode 100644
index 000000000..e34a99214
--- /dev/null
+++ b/SDK/C/CMakeLists.txt
@@ -0,0 +1,22 @@
+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)
diff --git a/SDK/C/examples/image_write/CMakeLists.txt b/SDK/C/examples/image_write/CMakeLists.txt
new file mode 100644
index 000000000..9415ced85
--- /dev/null
+++ b/SDK/C/examples/image_write/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_executable(image_write main.cpp)
+target_link_libraries(image_write ftl-dev)
diff --git a/SDK/C/examples/image_write/main.cpp b/SDK/C/examples/image_write/main.cpp
new file mode 100644
index 000000000..4cb9bd425
--- /dev/null
+++ b/SDK/C/examples/image_write/main.cpp
@@ -0,0 +1,48 @@
+#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;
+}
diff --git a/SDK/C/examples/video_write/CMakeLists.txt b/SDK/C/examples/video_write/CMakeLists.txt
new file mode 100644
index 000000000..b0048d762
--- /dev/null
+++ b/SDK/C/examples/video_write/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_executable(video_write main.cpp)
+target_link_libraries(video_write ftl-dev)
diff --git a/SDK/C/examples/video_write/main.cpp b/SDK/C/examples/video_write/main.cpp
new file mode 100644
index 000000000..94cd6fa72
--- /dev/null
+++ b/SDK/C/examples/video_write/main.cpp
@@ -0,0 +1,42 @@
+#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;
+}
diff --git a/SDK/C/include/ftl/ftl.h b/SDK/C/include/ftl/ftl.h
new file mode 100644
index 000000000..33ff215da
--- /dev/null
+++ b/SDK/C/include/ftl/ftl.h
@@ -0,0 +1,195 @@
+#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
diff --git a/SDK/C/src/common.cpp b/SDK/C/src/common.cpp
new file mode 100644
index 000000000..6e1e252f4
--- /dev/null
+++ b/SDK/C/src/common.cpp
@@ -0,0 +1,3 @@
+#include <ftl/ftl.h>
+
+
diff --git a/SDK/C/src/streams.cpp b/SDK/C/src/streams.cpp
new file mode 100644
index 000000000..e9dc6ff07
--- /dev/null
+++ b/SDK/C/src/streams.cpp
@@ -0,0 +1,276 @@
+#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;
+}
diff --git a/components/streams/src/filestream.cpp b/components/streams/src/filestream.cpp
index 9766298b1..412594814 100644
--- a/components/streams/src/filestream.cpp
+++ b/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;
diff --git a/components/structures/include/ftl/data/frame.hpp b/components/structures/include/ftl/data/frame.hpp
index 637621169..c304e4e97 100644
--- a/components/structures/include/ftl/data/frame.hpp
+++ b/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>
--
GitLab