Merge branch 'feature/snapshot' into 'master'

Snapshot stream recording

See merge request nicolas.pope/ftl!54

applications/gui/src/media_panel.cpp

@@ -17,6 +17,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
     using namespace nanogui;
 
     paused_ = false;
+    writer_ = nullptr;
 
     setLayout(new BoxLayout(Orientation::Horizontal,
 									Alignment::Middle, 5, 10));
@@ -54,8 +55,24 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
     button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
     button->setFlags(Button::ToggleButton);
     button->setChangeCallback([this,button](bool state) {
-        if (state) button->setTextColor(nanogui::Color(1.0f,0.1f,0.1f,1.0f));
-        else button->setTextColor(nanogui::Color(1.0f,1.0f,1.0f,1.0f));
+        if (state){
+            auto *cam = screen_->activeCamera();
+
+            button->setTextColor(nanogui::Color(1.0f,0.1f,0.1f,1.0f));
+            char timestamp[18];
+			std::time_t t=std::time(NULL);
+			std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
+			writer_ = new ftl::rgbd::SnapshotStreamWriter(std::string(timestamp) + ".tar.gz", 1000 / 25);
+            writer_->addSource(cam->source());
+            writer_->start();
+        } else {
+            button->setTextColor(nanogui::Color(1.0f,1.0f,1.0f,1.0f));
+            if (writer_) {
+                writer_->stop();
+                delete writer_;
+                writer_ = nullptr;
+            }
+        }
         //if (state) ... start
         //else ... stop
     });
@@ -92,12 +109,10 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
 			auto writer = ftl::rgbd::SnapshotWriter(std::string(timestamp) + ".tar.gz");
 			cv::Mat rgb, depth;
 			cam->source()->getFrames(rgb, depth);
-			if (!writer.addCameraRGBD(
+			if (!writer.addCameraParams("0", cam->source()->getPose(), cam->source()->parameters()) || !writer.addCameraRGBD(
 					"0", // TODO
 					rgb,
-					depth,
-					cam->source()->getPose(),
-					cam->source()->parameters()
+					depth
 				)) {
 				LOG(ERROR) << "Snapshot failed";
 			}

applications/gui/src/media_panel.hpp

@@ -4,6 +4,11 @@
 #include <nanogui/window.h>
 
 namespace ftl {
+
+namespace rgbd {
+class SnapshotStreamWriter;
+}
+
 namespace gui {
 
 class Screen;
@@ -16,6 +21,7 @@ class MediaPanel : public nanogui::Window {
     private:
     ftl::gui::Screen *screen_;
     bool paused_;
+    ftl::rgbd::SnapshotStreamWriter *writer_;
 };
 
 }

applications/gui/src/src_window.cpp

@@ -88,7 +88,7 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
 	auto button_snapshot = new Button(this, "Snapshot", ENTYPO_ICON_IMAGES);
 	button_snapshot->setCallback([this] {
 		try {
-			char timestamp[18];
+			/*char timestamp[18];
 			std::time_t t=std::time(NULL);
 			std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
 			auto writer = ftl::rgbd::SnapshotWriter(std::string(timestamp) + ".tar.gz");

components/rgbd-sources/include/ftl/rgbd/snapshot.hpp

@@ -3,14 +3,17 @@
 #define _FTL_RGBD_SNAPSHOT_HPP_
 
 #include <loguru.hpp>
+#include <thread>
 
 #include <opencv2/opencv.hpp>
 
 #include <Eigen/Eigen>
 #include <opencv2/core/eigen.hpp>
 
+#include <ftl/rgbd/source.hpp>
 #include <ftl/rgbd/camera.hpp>
 
+#include <atomic>
 #include <archive.h>
 #include <archive_entry.h>
 
@@ -24,7 +27,8 @@ public:
 	explicit SnapshotWriter(const std::string &filename);
 	~SnapshotWriter();
 	
-	bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera &params);
+	bool addCameraParams(const std::string &name, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera &params);
+	bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth);
 	bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format="tiff");
 	bool addEigenMatrix4d(const std::string &name, const Eigen::Matrix4d &m, const std::string &format="pfm");
 	bool addFile(const std::string &name, const std::vector<uchar> &buf);
@@ -35,7 +39,28 @@ private:
 	struct archive_entry *entry_;
 };
 
+class SnapshotStreamWriter {
+public:
+	SnapshotStreamWriter(const std::string &filename, int delay);
+	~SnapshotStreamWriter();
+	void addSource(ftl::rgbd::Source* src);
+	void start();
+	void stop();
+
+private:
+	std::atomic<bool> run_;
+	bool finished_;
+	int delay_;
+
+	std::vector<ftl::rgbd::Source*> sources_;
+	SnapshotWriter writer_;
+	std::thread thread_;
+
+	void run();
+};
+
 struct SnapshotEntry {
+	long t;
 	cv::Mat rgb;
 	cv::Mat depth;
 	Eigen::Matrix4d pose;

components/rgbd-sources/src/snapshot.cpp

@@ -124,11 +124,8 @@ bool SnapshotWriter::addEigenMatrix4d(const string &name, const Matrix4d &m, con
 	return addMat(name, tmp, format);
 }
 
-bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth,
-							 const Matrix4d &pose, const Camera &params) {
+bool SnapshotWriter::addCameraParams(const string &name, const Matrix4d &pose, const Camera &params) {
 	bool retval = true;
-	retval &= addMat(name + "-RGB", rgb);
-	retval &= addMat(name + "-D", depth);
 	retval &= addEigenMatrix4d(name + "-POSE", pose);
 
 	nlohmann::json j;
@@ -138,6 +135,65 @@ bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat
 	return retval;
 }
 
+bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth) {
+	bool retval = true;
+	cv::Mat tdepth;
+	depth.convertTo(tdepth, CV_16SC1, 16.0f*10.0f);
+	retval &= addMat(name + "-RGB", rgb, "jpg");
+	retval &= addMat(name + "-D", tdepth, "png");
+	return retval;
+}
+
+SnapshotStreamWriter::SnapshotStreamWriter(const string &filename, int delay) : 
+		run_(false), finished_(false), writer_(filename), delay_(delay) {}
+
+SnapshotStreamWriter::~SnapshotStreamWriter() {
+
+}
+
+void SnapshotStreamWriter::addSource(ftl::rgbd::Source *src) {
+	writer_.addCameraParams(std::to_string(sources_.size()), src->getPose(), src->parameters());
+	sources_.push_back(src);
+}
+
+void SnapshotStreamWriter::run() {
+	vector<Mat> rgb(sources_.size());
+	vector<Mat> depth(sources_.size());
+
+	while(run_) {
+		auto now = std::chrono::system_clock::now();
+		auto duration = now.time_since_epoch();
+		auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
+
+		for(size_t i = 0; i < sources_.size(); ++i) {
+			sources_[i]->getFrames(rgb[i], depth[i]);
+		}
+
+		for(size_t i = 0; i < sources_.size(); ++i) {
+			writer_.addCameraRGBD(std::to_string(ms) + "-" + std::to_string(i), rgb[i], depth[i]);
+		}
+
+		std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
+	}
+
+	run_ = false;
+	finished_ = true;
+}
+
+void SnapshotStreamWriter::start() {
+	if (run_ || finished_) return;
+	run_ = true;
+	thread_ = std::thread([this] { run(); });
+}
+
+
+void SnapshotStreamWriter::stop() {
+	bool wasrunning = run_;
+	run_ = false;
+	if (wasrunning) thread_.join();
+}
+
+
 
 SnapshotReader::SnapshotReader(const string &filename) {
 	archive_ = archive_read_new();