Add recorder app

CMakeLists.txt

@@ -228,6 +228,7 @@ add_subdirectory(components/control/cpp)
 add_subdirectory(applications/calibration)
 add_subdirectory(applications/groupview)
 add_subdirectory(applications/player)
+add_subdirectory(applications/recorder)
 
 if (HAVE_AVFORMAT)
 	add_subdirectory(applications/ftl2mkv)

applications/player/src/main.cpp

@@ -59,7 +59,6 @@ int main(int argc, char **argv) {
 
 	ftl::timer::add(ftl::timer::kTimerMain, [&current_stream,&current_channel,&r](int64_t ts) {
 		bool res = r.read(ts, [&current_stream,&current_channel,&r](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
-			if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel)) return;
 			if (spkt.streamID == current_stream) {
 
 				if (pkt.codec == codec_t::POSE) {
@@ -74,6 +73,8 @@ int main(int argc, char **argv) {
 					return;
 				}
 				
+				if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel)) return;
+
 				//LOG(INFO) << "Reading packet: (" << (int)spkt.streamID << "," << (int)spkt.channel << ") " << (int)pkt.codec << ", " << (int)pkt.definition;
 
 				cv::Mat frame(cv::Size(ftl::codecs::getWidth(pkt.definition),ftl::codecs::getHeight(pkt.definition)), (spkt.channel == Channel::Depth) ? CV_32F : CV_8UC3);
@@ -92,12 +93,17 @@ int main(int argc, char **argv) {
 					double time = (double)(spkt.timestamp - r.getStartTime()) / 1000.0;
 					cv::putText(frame, std::string("Time: ") + std::to_string(time) + std::string("s"), cv::Point(10,20), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,255));
 					cv::imshow("Player", frame);
+				} else {
+					frame.create(cv::Size(600,600), CV_8UC3);
+					cv::imshow("Player", frame);
 				}
 				int key = cv::waitKey(1);
 				if (key >= 48 && key <= 57) {
 					current_stream = key - 48;
 				} else if (key == 'd') {
 					current_channel = (current_channel == 0) ? 1 : 0;
+				} else if (key == 'r') {
+					current_channel = (current_channel == 0) ? 2 : 0;
 				} else if (key == 27) {
 					ftl::timer::stop(false);
 				}

applications/recorder/CMakeLists.txt

+set(RECSRC
+    src/main.cpp
+    src/registration.cpp
+)
+
+add_executable(ftl-record ${RECSRC})
+
+target_include_directories(ftl-record PUBLIC
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+	$<INSTALL_INTERFACE:include>
+	PRIVATE src)
+
+#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(ftl-record ftlcommon ftlrgbd Threads::Threads ${OpenCV_LIBS} ftlctrl ftlnet)
+
+

applications/recorder/src/main.cpp

+/*
+ * Copyright 2019 Nicolas Pope. All rights reserved.
+ *
+ * See LICENSE.
+ */
+
+#define LOGURU_WITH_STREAMS 1
+#include <loguru.hpp>
+#include <ftl/config.h>
+#include <ftl/configuration.hpp>
+#include <ftl/rgbd.hpp>
+#include <ftl/rgbd/virtual.hpp>
+#include <ftl/rgbd/streamer.hpp>
+#include <ftl/slave.hpp>
+#include <ftl/rgbd/group.hpp>
+#include <ftl/threads.hpp>
+#include <ftl/codecs/writer.hpp>
+
+
+#include <fstream>
+#include <string>
+#include <vector>
+#include <thread>
+#include <chrono>
+
+#include <opencv2/opencv.hpp>
+#include <ftl/net/universe.hpp>
+
+#include "registration.hpp"
+
+#include <cuda_profiler_api.h>
+
+#ifdef WIN32
+#pragma comment(lib, "Rpcrt4.lib")
+#endif
+
+using ftl::net::Universe;
+using std::string;
+using std::vector;
+using ftl::rgbd::Source;
+using ftl::config::json_t;
+using ftl::codecs::Channel;
+
+using json = nlohmann::json;
+using std::this_thread::sleep_for;
+using std::chrono::milliseconds;
+//using std::mutex;
+//using std::unique_lock;
+
+//using cv::Mat;
+
+using ftl::registration::loadTransformations;
+using ftl::registration::saveTransformations;
+
+static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
+  Eigen::Affine3d rx =
+      Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
+  Eigen::Affine3d ry =
+      Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
+  Eigen::Affine3d rz =
+      Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
+  return rz * rx * ry;
+}
+
+static void writeSourceProperties(ftl::codecs::Writer &writer, int id, ftl::rgbd::Source *src) {
+	ftl::codecs::StreamPacket spkt;
+	ftl::codecs::Packet pkt;
+
+	spkt.timestamp = 0;
+	spkt.streamID = id;
+	spkt.channel = Channel::Calibration;
+	spkt.channel_count = 1;
+	pkt.codec = ftl::codecs::codec_t::CALIBRATION;
+	pkt.definition = ftl::codecs::definition_t::Any;
+	pkt.block_number = 0;
+	pkt.block_total = 1;
+	pkt.flags = 0;
+	pkt.data = std::move(std::vector<uint8_t>((uint8_t*)&src->parameters(), (uint8_t*)&src->parameters() + sizeof(ftl::rgbd::Camera)));
+
+	writer.write(spkt, pkt);
+
+	spkt.channel = Channel::Pose;
+	pkt.codec = ftl::codecs::codec_t::POSE;
+	pkt.definition = ftl::codecs::definition_t::Any;
+	pkt.block_number = 0;
+	pkt.block_total = 1;
+	pkt.flags = 0;
+	pkt.data = std::move(std::vector<uint8_t>((uint8_t*)src->getPose().data(), (uint8_t*)src->getPose().data() + 4*4*sizeof(double)));
+
+	writer.write(spkt, pkt);
+}
+
+static void run(ftl::Configurable *root) {
+	Universe *net = ftl::create<Universe>(root, "net");
+	ftl::ctrl::Slave slave(net, root);
+
+	// Controls
+	auto *controls = ftl::create<ftl::Configurable>(root, "controls");
+	
+	net->start();
+	net->waitConnections();
+	
+	// Create a vector of all input RGB-Depth sources
+	auto sources = ftl::createArray<Source>(root, "sources", net);
+
+	if (sources.size() == 0) {
+		LOG(ERROR) << "No sources configured!";
+		return;
+	}
+
+	// Create scene transform, intended for axis aligning the walls and floor
+	Eigen::Matrix4d transform;
+	if (root->getConfig()["transform"].is_object()) {
+		auto &c = root->getConfig()["transform"];
+		float rx = c.value("pitch", 0.0f);
+		float ry = c.value("yaw", 0.0f);
+		float rz = c.value("roll", 0.0f);
+		float x = c.value("x", 0.0f);
+		float y = c.value("y", 0.0f);
+		float z = c.value("z", 0.0f);
+
+		Eigen::Affine3d r = create_rotation_matrix(rx, ry, rz);
+		Eigen::Translation3d trans(Eigen::Vector3d(x,y,z));
+		Eigen::Affine3d t(trans);
+		transform = t.matrix() * r.matrix();
+		LOG(INFO) << "Set transform: " << transform;
+	} else {
+		transform.setIdentity();
+	}
+
+	// Must find pose for each source...
+	if (sources.size() > 1) {
+		std::map<std::string, Eigen::Matrix4d> transformations;
+
+		if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
+			LOG(INFO) << "Loaded camera trasformations from file";
+		}
+		else {
+			LOG(ERROR) << "Error loading camera transformations from file";
+		}
+
+		for (auto &input : sources) {
+			string uri = input->getURI();
+			auto T = transformations.find(uri);
+			if (T == transformations.end()) {
+				LOG(ERROR) << "Camera pose for " + uri + " not found in transformations";
+				//LOG(WARNING) << "Using only first configured source";
+				// TODO: use target source if configured and found
+				//sources = { sources[0] };
+				//sources[0]->setPose(Eigen::Matrix4d::Identity());
+				//break;
+				input->setPose(transform * input->getPose());
+				continue;
+			}
+			input->setPose(transform * T->second);
+		}
+	}
+
+	ftl::rgbd::FrameSet scene_A;  // Output of align process
+	ftl::rgbd::FrameSet scene_B;  // Input of render process
+
+	ftl::rgbd::Streamer *stream = ftl::create<ftl::rgbd::Streamer>(root, "stream", net);
+	ftl::rgbd::Group *group = new ftl::rgbd::Group;
+
+	for (size_t i=0; i<sources.size(); i++) {
+		Source *in = sources[i];
+		in->setChannel(Channel::Right);
+		group->addSource(in);
+	}
+
+	// ---- Recording code -----------------------------------------------------
+
+	std::ofstream fileout;
+	ftl::codecs::Writer writer(fileout);
+	auto recorder = [&writer,&group](ftl::rgbd::Source *src, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+		ftl::codecs::StreamPacket s = spkt;
+		// Patch stream ID to match order in group
+		s.streamID = group->streamID(src);
+
+
+        //LOG(INFO) << "Record packet: " << (int)s.streamID << "," << s.timestamp;
+		writer.write(s, pkt);
+	};
+    group->addRawCallback(std::function(recorder));
+
+	root->set("record", false);
+
+	// Allow stream recording
+	root->on("record", [&group,&fileout,&writer,&recorder](const ftl::config::Event &e) {
+		if (e.entity->value("record", false)) {
+			char timestamp[18];
+			std::time_t t=std::time(NULL);
+			std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
+			fileout.open(std::string(timestamp) + ".ftl");
+
+			writer.begin();
+            LOG(INFO) << "Writer begin";
+
+			// TODO: Write pose+calibration+config packets
+			auto sources = group->sources();
+			for (int i=0; i<sources.size(); ++i) {
+				writeSourceProperties(writer, i, sources[i]);
+			}
+		} else {
+			//group->removeRawCallback(recorder);
+            LOG(INFO) << "Writer end";
+			writer.end();
+			fileout.close();
+		}
+	});
+
+	// -------------------------------------------------------------------------
+
+	stream->setLatency(6);  // FIXME: This depends on source!?
+	stream->add(group);
+	stream->run();
+
+	bool busy = false;
+
+	group->setLatency(4);
+	group->setName("ReconGroup");
+	group->sync([](ftl::rgbd::FrameSet &fs) -> bool {
+		return true;
+	});
+
+	LOG(INFO) << "Shutting down...";
+	ftl::timer::stop();
+	slave.stop();
+	net->shutdown();
+	ftl::pool.stop();
+
+	cudaProfilerStop();
+
+	LOG(INFO) << "Deleting...";
+
+	delete stream;
+	delete net;
+	delete group;
+
+	ftl::config::cleanup();  // Remove any last configurable objects.
+	LOG(INFO) << "Done.";
+}
+
+int main(int argc, char **argv) {
+	run(ftl::configure(argc, argv, "reconstruction_default"));
+}

applications/recorder/src/registration.cpp

+#include "registration.hpp"
+#include <fstream>
+#define LOGURU_WITH_STREAMS 1
+#include <loguru.hpp>
+
+
+namespace ftl {
+namespace registration {
+
+using ftl::rgbd::Camera;
+using ftl::rgbd::Source;
+
+using std::string;
+using std::vector;
+using std::pair;
+using std::map;
+using std::optional;
+
+using cv::Mat;
+using Eigen::Matrix4f;
+using Eigen::Matrix4d;
+
+void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
+	for (auto it = json.begin(); it != json.end(); ++it) {
+		Eigen::Matrix4d m;
+		auto data = m.data();
+		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
+		transformations[it.key()] = m;
+	}
+}
+
+void to_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
+	for (auto &item : transformations) {
+		auto val = nlohmann::json::array();
+		for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
+		json[item.first] = val;
+	}
+}
+
+bool loadTransformations(const string &path, map<string, Matrix4d> &data) {
+	std::ifstream file(path);
+	if (!file.is_open()) {
+		LOG(ERROR) << "Error loading transformations from file " << path;
+		return false;
+	}
+	
+	nlohmann::json json_registration;
+	file >> json_registration;
+	from_json(json_registration, data);
+	return true;
+}
+
+bool saveTransformations(const string &path, map<string, Matrix4d> &data) {
+	nlohmann::json data_json;
+	to_json(data_json, data);
+	std::ofstream file(path);
+
+	if (!file.is_open()) {
+		LOG(ERROR) << "Error writing transformations to file " << path;
+		return false;
+	}
+
+	file << std::setw(4) << data_json;
+	return true;
+}
+
+
+} // namespace registration
+} // namespace ftl

applications/recorder/src/registration.hpp

+#ifndef _FTL_RECONSTRUCT_REGISTRATION_HPP_
+#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
+
+#include <ftl/config.h>
+#include <ftl/configurable.hpp>
+#include <ftl/rgbd.hpp>
+#include <opencv2/opencv.hpp>
+
+
+namespace ftl {
+namespace registration {
+
+void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+
+bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+
+}
+}
+
+#endif  // _FTL_RECONSTRUCT_REGISTRATION_HPP_
\ No newline at end of file

components/codecs/include/ftl/codecs/writer.hpp

@@ -23,6 +23,7 @@ class Writer {
 	std::ostream *stream_;
 	msgpack::sbuffer buffer_;
 	int64_t timestart_;
+	bool active_;
 };
 
 }

components/codecs/src/writer.cpp

 #include <ftl/codecs/writer.hpp>
 #include <ftl/timer.hpp>
+#include <loguru.hpp>
 
 #include <tuple>
 
 using ftl::codecs::Writer;
 
-Writer::Writer(std::ostream &s) : stream_(&s) {}
+Writer::Writer(std::ostream &s) : stream_(&s), active_(false) {}
 
 Writer::~Writer() {
 
@@ -18,15 +19,17 @@ bool Writer::begin() {
 
 	// Capture current time to adjust timestamps
 	timestart_ = ftl::timer::get_time();
-
+	active_ = true;
 	return true;
 }
 
 bool Writer::end() {
+	active_ = false;
 	return true;
 }
 
 bool Writer::write(const ftl::codecs::StreamPacket &s, const ftl::codecs::Packet &p) {
+	if (!active_) return false;
 	ftl::codecs::StreamPacket s2 = s;
 	// Adjust timestamp relative to start of file.
 	s2.timestamp -= timestart_;

components/rgbd-sources/src/source.cpp

@@ -347,8 +347,10 @@ void Source::addRawCallback(const std::function<void(ftl::rgbd::Source*, const f
 void Source::removeRawCallback(const std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)> &f) {
 	UNIQUE_LOCK(mutex_,lk);
 	for (auto i=rawcallbacks_.begin(); i!=rawcallbacks_.end(); ++i) {
-		if (i->target<void(*)(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)>() == f.target<void(*)(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)>()) {
+		const auto targ = (*i).target<void(*)(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &, const ftl::codecs::Packet &)>();
+		if (targ && targ == f.target<void(*)(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &, const ftl::codecs::Packet &)>()) {
 			rawcallbacks_.erase(i);
+			LOG(INFO) << "Removing RAW callback";
 			return;
 		}
 	}