diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp index b5e70e898f1c9299a7aa909261ca3324738d2bf2..dea1827d6d089d7a9a55b8730b0481a389db68b1 100644 --- a/applications/gui/src/src_window.cpp +++ b/applications/gui/src/src_window.cpp @@ -133,7 +133,7 @@ SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl) 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"); @@ -147,7 +147,7 @@ SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl) this->src_->parameters() )) { LOG(ERROR) << "Snapshot failed"; - } + }*/ } catch(std::runtime_error) { LOG(ERROR) << "Snapshot failed (file error)"; diff --git a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp index 9643b1312b59ba068e300bc304a01c3f62d5d955..edec6150217a6655021b94d49ef36d6018d87374 100644 --- a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp +++ b/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 ¶ms); + bool addCameraParams(const std::string &name, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera ¶ms); + 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; diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp index 5599364c6508184c57651d7c46ca54951ac0d4ab..b45ea4f93d7097c57ea5e06eb1c8c9b9530a61b8 100644 --- a/components/rgbd-sources/src/snapshot.cpp +++ b/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 ¶ms) { +bool SnapshotWriter::addCameraParams(const string &name, const Matrix4d &pose, const Camera ¶ms) { bool retval = true; - retval &= addMat(name + "-RGB", rgb); - retval &= addMat(name + "-D", depth); retval &= addEigenMatrix4d(name + "-POSE", pose); nlohmann::json j; @@ -138,6 +135,58 @@ 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; + retval &= addMat(name + "-RGB", rgb); + retval &= addMat(name + "-D", depth); + return retval; +} + +SnapshotStreamWriter::SnapshotStreamWriter(const string &filename, int delay) : + writer_(filename), delay_(delay) {} + +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(ms + "-" + std::to_string(i), rgb[i], depth[i]); + } + + std::this_thread::sleep_for(std::chrono::microseconds(delay_)); + } + + run_ = false; + finished_ = true; +} + +void SnapshotStreamWriter::start() { + if (run_ || finished_) return; + run_ = true; + thread_ = std::thread([this] { run(); }); +} + + +void SnapshotStreamWriter::stop() { + run_ = false; + thread_.join(); +} + + SnapshotReader::SnapshotReader(const string &filename) { archive_ = archive_read_new();