diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0874ce89a2e7a83211ac765c4027997b440195a0..0f866eda014e6b6353824435cbacbc500fe2c683 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/applications/player/src/main.cpp b/applications/player/src/main.cpp
index cc0f5039a702f64af43004fc8895d447cf8f5a28..60d2793c1c4b0484dbb226bff12deebfb6e1fc2e 100644
--- a/applications/player/src/main.cpp
+++ b/applications/player/src/main.cpp
@@ -59,7 +59,6 @@ int main(int argc, char **argv) {
ftl::timer::add(ftl::timer::kTimerMain, [¤t_stream,¤t_channel,&r](int64_t ts) {
bool res = r.read(ts, [¤t_stream,¤t_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) {
@@ -73,6 +72,8 @@ int main(int argc, char **argv) {
LOG(INFO) << "Have calibration: " << camera->fx;
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;
@@ -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);
}
diff --git a/applications/recorder/CMakeLists.txt b/applications/recorder/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0d7b668c8e9f7dafe519a25f24499ff9c70d6074
--- /dev/null
+++ b/applications/recorder/CMakeLists.txt
@@ -0,0 +1,16 @@
+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)
+
+
diff --git a/applications/recorder/src/main.cpp b/applications/recorder/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6f8926a5afa5d02112ca943c6a48af89a17c5fa
--- /dev/null
+++ b/applications/recorder/src/main.cpp
@@ -0,0 +1,246 @@
+/*
+ * 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"));
+}
diff --git a/applications/recorder/src/registration.cpp b/applications/recorder/src/registration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a86f2b3d1c6c5ca8c172a303ad979bf741de67de
--- /dev/null
+++ b/applications/recorder/src/registration.cpp
@@ -0,0 +1,69 @@
+#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
diff --git a/applications/recorder/src/registration.hpp b/applications/recorder/src/registration.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1af4de8a5e3cbc8b64b291e4e115165e197cf28a
--- /dev/null
+++ b/applications/recorder/src/registration.hpp
@@ -0,0 +1,22 @@
+#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
diff --git a/components/codecs/include/ftl/codecs/writer.hpp b/components/codecs/include/ftl/codecs/writer.hpp
index 3befecf7cdff226f4b91dba599b82a5e66c0217d..044624ba837f17c83e4a22a16787ec86edd4ad67 100644
--- a/components/codecs/include/ftl/codecs/writer.hpp
+++ b/components/codecs/include/ftl/codecs/writer.hpp
@@ -23,6 +23,7 @@ class Writer {
std::ostream *stream_;
msgpack::sbuffer buffer_;
int64_t timestart_;
+ bool active_;
};
}
diff --git a/components/codecs/src/writer.cpp b/components/codecs/src/writer.cpp
index 2c19a01fd260e1549b01daef10c5600ea8c4b5a3..6561549f6f2e681b7678b328f4357762eae468a1 100644
--- a/components/codecs/src/writer.cpp
+++ b/components/codecs/src/writer.cpp
@@ -1,11 +1,12 @@
#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_;
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 4482448cfe05bbd08c22082861f4eeadf1daafc8..7e2cc10ed8971cf2c0b29d2a07b345121f92cb9e 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/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;
}
}