Feature/Snapshot Video

applications/groupview/src/main.cpp

@@ -169,32 +169,35 @@ void modeFrame(ftl::Configurable *root, int frames=1) {
 	}
 
 	std::atomic<bool> grab = false;
+	std::atomic<bool> video = false;
+	
+	std::vector<cv::Mat> rgb(sources.size());
+	std::vector<cv::Mat> depth(sources.size());
+	std::mutex m;
 
 	group.sync([&grab](const ftl::rgbd::FrameSet &fs) {
 		LOG(INFO) << "Complete set: " << fs.timestamp;
 		if (!ftl::running) { return false; }
-		if (grab) {
-			grab = false;
 
 #ifdef HAVE_LIBARCHIVE
+		if (grab) {
+			grab = false;
 			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");
 
 			for (size_t i=0; i<fs.sources.size(); ++i) {
-				writer.addCameraParams(std::string("camera")+std::to_string(i), fs.sources[i]->getPose(), fs.sources[i]->parameters());
+				writer.addSource(fs.sources[i]->getURI(), fs.sources[i]->parameters(), fs.sources[i]->getPose());
 				LOG(INFO) << "SAVE: " << fs.channel1[i].cols << ", " << fs.channel2[i].type();
-				writer.addCameraRGBD(std::string("camera")+std::to_string(i), fs.channel1[i], fs.channel2[i]);
+				writer.addRGBD(i, fs.channel1[i], fs.channel2[i]);
 			}
-#endif  // HAVE_LIBARCHIVE
 		}
+#endif  // HAVE_LIBARCHIVE
 		return true;
 	});
 
 	cv::Mat show;
-	vector<cv::Mat> rgb(sources.size());
-	vector<cv::Mat> depth(sources.size());
 
 	while (ftl::running) {
 		for (auto s : sources) s->grab(30);
@@ -229,10 +232,18 @@ void modeVideo(ftl::Configurable *root) {
 	vector<cv::Mat> rgb(sources.size());
 	vector<cv::Mat> depth(sources.size());
 
-	int count = 0;
-	int remaining = 0;
+#ifdef HAVE_LIBARCHIVE
+	char timestamp[18];
+	std::time_t t=std::time(NULL);
+	std::strftime(timestamp, sizeof(timestamp), "%F-%H%M%S", std::localtime(&t));
+	ftl::rgbd::SnapshotWriter writer = ftl::rgbd::SnapshotWriter(std::string(timestamp) + ".tar.gz");
+
+	for (size_t i = 0; i < sources.size(); i++) {
+		writer.addSource(sources[i]->getURI(), sources[i]->parameters(), sources[i]->getPose());
+	}
+#endif // HAVE_LIBARCHIVE
 
-	Mat depth_out;
+	bool save = false;
 
 	while (ftl::running) {
 		for (auto s : sources) s->grab(30);
@@ -241,24 +252,24 @@ void modeVideo(ftl::Configurable *root) {
 			while(rgb[i].empty() || depth[i].empty());
 		}
 
-		if (remaining > 0) {
+#ifdef HAVE_LIBARCHIVE
+		if (save) {
 			for (size_t i = 0; i < sources.size(); i++) {
-				depth[i].convertTo(depth_out, CV_16UC1, 1000.0f);
-				cv::imwrite(path + "rgb-" + std::to_string(i) + "-" + std::to_string(count) + ".jpg", rgb[i]);
-				cv::imwrite(path + "depth-" + std::to_string(i) + "-" + std::to_string(count) + ".png", depth_out);
+				writer.addRGBD(i, rgb[i], depth[i]);
 			}
-			count++;
-			remaining--;
-			LOG(INFO) << "remaining: " << remaining;
 		}
+#endif // HAVE_LIBARCHIVE
 
 		stack(rgb, show);
 		cv::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
 		cv::imshow("Cameras", show);
 
 		auto key = cv::waitKey(20);
-		if (key == 'g' && remaining == 0) {
-			remaining = 250;
+		if (key == 'r') {
+			save = true;
+		}
+		if (key == 's') {
+			save = false;
 		}
 		if (key == 27) break;
 	}
@@ -274,7 +285,8 @@ int main(int argc, char **argv) {
 		LOG(INFO) << "Video mode";
 		modeVideo(root);
 	} else {
-		modeFrame(root);
+		modeVideo(root);
+		//modeFrame(root);
 	}
 
 	ftl::running = false;

applications/gui/src/media_panel.cpp

@@ -14,89 +14,86 @@
 using ftl::gui::MediaPanel;
 
 MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""), screen_(screen) {
-    using namespace nanogui;
+	using namespace nanogui;
 
-    paused_ = false;
-    writer_ = nullptr;
+	paused_ = false;
+	writer_ = nullptr;
 
-    setLayout(new BoxLayout(Orientation::Horizontal,
+	setLayout(new BoxLayout(Orientation::Horizontal,
 									Alignment::Middle, 5, 10));
 
-    auto size = Vector2i(400, 60);
-    //setFixedSize(size);
-    setPosition(Vector2i(screen->width() / 2 - size[0]/2, screen->height() - 30 - size[1]));
+	auto size = Vector2i(400, 60);
+	//setFixedSize(size);
+	setPosition(Vector2i(screen->width() / 2 - size[0]/2, screen->height() - 30 - size[1]));
 
-    auto button = new Button(this, "", ENTYPO_ICON_EDIT);
+	auto button = new Button(this, "", ENTYPO_ICON_EDIT);
 	button->setTooltip("Edit camera properties");
-    button->setCallback([this]() {
-        auto *cam = screen_->activeCamera();
-        if (cam) cam->showPoseWindow();
-    });
+	button->setCallback([this]() {
+		auto *cam = screen_->activeCamera();
+		if (cam) cam->showPoseWindow();
+	});
 
-    button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
-    button->setFlags(Button::ToggleButton);
-    button->setChangeCallback([this,button](bool state) {
-        if (state){
-            auto *cam = screen_->activeCamera();
+	button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
+	button->setFlags(Button::ToggleButton);
+	button->setChangeCallback([this,button](bool state) {
+		if (state){
+			auto *cam = screen_->activeCamera();
 
-            button->setTextColor(nanogui::Color(1.0f,0.1f,0.1f,1.0f));
-            char timestamp[18];
+			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
-    });
+			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
+	});
 
 	button = new Button(this, "", ENTYPO_ICON_CONTROLLER_STOP);
-    button->setCallback([this]() {
-        screen_->setActiveCamera(nullptr);
-    });
+	button->setCallback([this]() {
+		screen_->setActiveCamera(nullptr);
+	});
 
-    button = new Button(this, "", ENTYPO_ICON_CONTROLLER_PAUS);
-    button->setCallback([this,button]() {
-        paused_ = !paused_;
-        screen_->control()->pause();
-        if (paused_) {
-            button->setIcon(ENTYPO_ICON_CONTROLLER_PLAY);
-        } else {
-            button->setIcon(ENTYPO_ICON_CONTROLLER_PAUS);
-        }
-    });
+	button = new Button(this, "", ENTYPO_ICON_CONTROLLER_PAUS);
+	button->setCallback([this,button]() {
+		paused_ = !paused_;
+		screen_->control()->pause();
+		if (paused_) {
+			button->setIcon(ENTYPO_ICON_CONTROLLER_PLAY);
+		} else {
+			button->setIcon(ENTYPO_ICON_CONTROLLER_PAUS);
+		}
+	});
 
-    //button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
+	//button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
 
  #ifdef HAVE_LIBARCHIVE
 	auto button_snapshot = new Button(this, "", ENTYPO_ICON_IMAGES);
-    button_snapshot->setTooltip("Screen capture");
+	button_snapshot->setTooltip("Screen capture");
 	button_snapshot->setCallback([this] {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (!cam) return;
-    
-		try {
-			char timestamp[18];
+	ftl::gui::Camera *cam = screen_->activeCamera();
+	if (!cam) return;
+
+	try {
+		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");
 			cv::Mat rgb, depth;
 			cam->source()->getFrames(rgb, depth);
-			if (!writer.addCameraParams("0", cam->source()->getPose(), cam->source()->parameters()) || !writer.addCameraRGBD(
-					"0", // TODO
-					rgb,
-					depth
-				)) {
-				LOG(ERROR) << "Snapshot failed";
-			}
+			writer.addSource(	cam->source()->getURI(),
+								cam->source()->parameters(),
+								cam->source()->getPose());
+			writer.addRGBD(0, rgb, depth);
 		}
 		catch(std::runtime_error) {
 			LOG(ERROR) << "Snapshot failed (file error)";

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

@@ -27,16 +27,26 @@ public:
 	explicit SnapshotWriter(const std::string &filename);
 	~SnapshotWriter();
 	
-	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");
+	void addSource(const std::string &id, const ftl::rgbd::Camera &params, const Eigen::Matrix4d &extrinsic);
+	void addSource(const std::string &id, const std::vector<double> &params, const cv::Mat &extrinsic);
+	bool addRGBD(size_t source, const cv::Mat &rgb, const cv::Mat &depth, uint64_t time=0);
+
+	bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format, const std::vector<int> &params);
 	bool addFile(const std::string &name, const std::vector<uchar> &buf);
 	bool addFile(const std::string &name, const uchar *buf, const size_t len);
+	
+	void writeIndex();
 
 private:
-	struct archive *archive_;
-	struct archive_entry *entry_;
+	std::vector<std::string> sources_;
+	std::vector<std::vector<double>> params_;
+	std::vector<cv::Mat> extrinsic_;
+	std::vector<size_t> frame_idx_;
+	std::vector<std::vector<std::string>> fname_rgb_;
+	std::vector<std::vector<std::string>> fname_depth_;
+
+	struct archive *archive_ = nullptr;
+	struct archive_entry *entry_ = nullptr;
 };
 
 class SnapshotStreamWriter {
@@ -59,35 +69,47 @@ private:
 	void run();
 };
 
-struct SnapshotEntry {
-	long t;
-	cv::Mat rgb;
-	cv::Mat depth;
-	Eigen::Matrix4d pose;
-	ftl::rgbd::Camera params;
-	uint status;
-	SnapshotEntry() : status(1+2+4+8) {};
+class Snapshot {
+public:
+	size_t getSourcesCount();
+	size_t getFramesCount();
+	
+	std::string getSourceURI(size_t camera);
+	ftl::rgbd::Camera getParameters(size_t camera);
+	void getPose(size_t camera, cv::Mat &out);
+	void getPose(size_t camera, Eigen::Matrix4d &out);
+
+	void getLeftRGB(size_t camera, size_t frame, cv::Mat &data);
+	void getLeftDepth(size_t camera, size_t frame, cv::Mat &data);
+
+	size_t n_frames;
+	size_t n_cameras;
+
+	std::vector<std::string> sources;
+	std::vector<ftl::rgbd::Camera> parameters;
+	std::vector<cv::Mat> extrinsic;
+	std::vector<std::vector<cv::Mat>> rgb_left;
+	std::vector<std::vector<cv::Mat>> depth_left;
 };
 
 class SnapshotReader {
 public:
 	explicit SnapshotReader(const std::string &filename);
 	~SnapshotReader();
-	
-	bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4d &pose, ftl::rgbd::Camera &params);
-	std::vector<std::string> getIds();
+
+	Snapshot readArchive();
 
 private:
-	SnapshotEntry& getEntry(const std::string &id);
 	bool readEntry(std::vector<uchar> &data);
-	bool readArchive();
 
-	std::map<std::string, SnapshotEntry> data_;
+	bool getDepth(const std::string &name, cv::Mat &data);
+	bool getRGB(const std::string &name, cv::Mat &data);
+
+	std::map<std::string, std::vector<uchar>> files_;
 	struct archive *archive_;
 	struct archive_entry *entry_;
 };
 
-
 };
 };
 

components/rgbd-sources/src/snapshot.cpp

@@ -13,6 +13,8 @@ using cv::imdecode;
 using std::string;
 using std::vector;
 
+using cv::FileStorage;
+
 // TODO: move to camera_params
 using ftl::rgbd::Camera;
 
@@ -39,6 +41,12 @@ void from_json(const nlohmann::json& j, Camera &p) {
 	j.at("minDepth").get_to(p.minDepth);
 	j.at("maxDepth").get_to(p.maxDepth);
 }
+/*
+Mat getCameraMatrix(const ftl::rgbd::Camera &parameters) {
+	Mat m = (cv::Mat_<double>(3,3) << parameters.fx, 0.0, -parameters.cx, 0.0, parameters.fy, -parameters.cy, 0.0, 0.0, 1.0);
+	return m;
+}
+*/
 //
 
 SnapshotWriter::SnapshotWriter(const string &filename) {
@@ -69,9 +77,7 @@ SnapshotWriter::SnapshotWriter(const string &filename) {
 }
 
 SnapshotWriter::~SnapshotWriter() {
-	archive_entry_free(entry_);
-	archive_write_close(archive_);
-	archive_write_free(archive_);
+	if (archive_) writeIndex();
 }
 
 bool SnapshotWriter::addFile(const string &name, const uchar *buf, const size_t len) {
@@ -104,44 +110,83 @@ bool SnapshotWriter::addFile(const string &name, const vector<uchar> &buf) {
 	return addFile(name, buf.data(), buf.size());
 }
 
-bool SnapshotWriter::addMat(const string &name, const Mat &mat, const std::string &format) {
+bool SnapshotWriter::addMat(const string &name, const Mat &mat, const std::string &format, const vector<int> &params) {
 	if (mat.rows == 0 || mat.cols == 0) {
 		LOG(ERROR) << "empty mat";
 		return false;
 	}
 
 	vector<uchar> buf;
-	vector<int> params;
 	bool retval = true;
 	retval &= imencode("." + format, mat, buf, params);
 	retval &= addFile(name + "." + format, buf);
 	return retval;
 }
 
-bool SnapshotWriter::addEigenMatrix4d(const string &name, const Matrix4d &m, const string &format) {
-	Mat tmp;
-	cv::eigen2cv(m, tmp);
-	return addMat(name, tmp, format);
+void SnapshotWriter::addSource(const std::string &id, const vector<double> &params, const cv::Mat &extrinsic) {
+	frame_idx_.push_back(0);
+	sources_.push_back(id);
+	params_.push_back(params);
+	extrinsic_.push_back(extrinsic);
+	fname_rgb_.emplace_back();
+	fname_depth_.emplace_back();
 }
 
-bool SnapshotWriter::addCameraParams(const string &name, const Matrix4d &pose, const Camera &params) {
+void SnapshotWriter::addSource(const std::string &id, const ftl::rgbd::Camera &params, const Eigen::Matrix4d &extrinsic) {
+	vector<double> params_vec;
+	Mat extrinsic_cv;
+	cv::eigen2cv(extrinsic, extrinsic_cv);
+	params_vec.push_back(params.fx);
+	params_vec.push_back(params.fy);
+	params_vec.push_back(params.cx);
+	params_vec.push_back(params.cy);
+	params_vec.push_back(params.width);
+	params_vec.push_back(params.height);
+	params_vec.push_back(params.minDepth);
+	params_vec.push_back(params.maxDepth);
+	params_vec.push_back(params.baseline);
+	params_vec.push_back(params.doffs);
+	addSource(id, params_vec, extrinsic_cv);
+}
+
+
+bool SnapshotWriter::addRGBD(size_t source, const cv::Mat &rgb, const cv::Mat &depth, uint64_t time) {
+	// TODO: png option
+	if (time != 0) { LOG(WARNING) << "time parameter not used (not implemented)"; }
+
 	bool retval = true;
-	retval &= addEigenMatrix4d(name + "-POSE", pose);
+	string fname = std::to_string(source) + "/" + std::to_string(frame_idx_[source]++);
+	
+	fname_rgb_[source].push_back("RGB" + fname + ".jpg");
+	retval &= addMat("RGB" + fname, rgb, "jpg", {});
+
+	fname_depth_[source].push_back("DEPTH" + fname + ".tiff");
+	retval &= addMat("DEPTH" + fname, depth, "tiff", {});
 
-	nlohmann::json j;
-	to_json(j, params);
-	string str_params = j.dump();
-	retval &= addFile(name + "-PARAMS.json", (uchar*) str_params.c_str(), str_params.size());
 	return retval;
 }
 
-bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth) {
-	bool retval = true;
-	cv::Mat tdepth;
-	depth.convertTo(tdepth, CV_16UC1, 1000.0f);
-	retval &= addMat(name + "-RGB", rgb, "jpg");
-	retval &= addMat(name + "-D", tdepth, "png");
-	return retval;
+void SnapshotWriter::writeIndex() {
+	FileStorage fs(".yml", FileStorage::WRITE + FileStorage::MEMORY);
+
+	vector<string> channels = {"time", "rgb_left", "depth_left"};
+
+	fs << "sources" << sources_;
+	fs << "params" <<params_;
+	fs << "extrinsic" << extrinsic_;
+	fs << "channels" << channels;
+	
+	fs << "rgb_left" << fname_rgb_;
+	fs << "depth_left" << fname_depth_;
+
+	string buf = fs.releaseAndGetString();
+	addFile("index.yml", (uchar*) buf.c_str(), buf.length());
+
+	archive_entry_free(entry_);
+	archive_write_close(archive_);
+	archive_write_free(archive_);
+	archive_ = nullptr;
+	entry_ = nullptr;
 }
 
 SnapshotStreamWriter::SnapshotStreamWriter(const string &filename, int delay) : 
@@ -150,11 +195,11 @@ SnapshotStreamWriter::SnapshotStreamWriter(const string &filename, int delay) :
 	}
 
 SnapshotStreamWriter::~SnapshotStreamWriter() {
-
+	
 }
 
 void SnapshotStreamWriter::addSource(ftl::rgbd::Source *src) {
-	writer_.addCameraParams(std::to_string(sources_.size()), src->getPose(), src->parameters());
+	writer_.addSource(src->getURI(), src->parameters(), src->getPose());
 	sources_.push_back(src);
 }
 
@@ -169,12 +214,19 @@ void SnapshotStreamWriter::run() {
 		auto duration = t_now.time_since_epoch();
 		auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
 
+		bool good = true;
 		for(size_t i = 0; i < sources_.size(); ++i) {
 			sources_[i]->getFrames(rgb[i], depth[i]);
+			good &= !rgb[i].empty() && !depth[i].empty();
+		}
+
+		if (!good) {
+			LOG(WARNING) << "Missing frames";
+			continue;
 		}
 
 		for(size_t i = 0; i < sources_.size(); ++i) {
-			writer_.addCameraRGBD(std::to_string(ms) + "-" + std::to_string(i), rgb[i], depth[i]);
+			writer_.addRGBD(i, rgb[i], depth[i]);
 		}
 
 		std::this_thread::sleep_until(t_wakeup);
@@ -197,10 +249,25 @@ void SnapshotStreamWriter::stop() {
 	if (wasrunning) thread_.join();
 }
 
-
+size_t Snapshot::getSourcesCount() { return sources.size(); }
+size_t Snapshot::getFramesCount() { return depth_left[0].size(); }
+	
+string Snapshot::getSourceURI(size_t camera) { return sources[camera]; }
+ftl::rgbd::Camera Snapshot::getParameters(size_t camera) { return parameters[camera]; }
+void Snapshot::getPose(size_t camera, cv::Mat &out) { out = extrinsic[camera]; }
+void Snapshot::getPose(size_t camera, Eigen::Matrix4d &out) {
+	Mat mat;
+	getPose(camera, mat);
+	cv::cv2eigen(mat, out);
+}
+void Snapshot::getLeftRGB(size_t camera, size_t frame, cv::Mat &data) { data = rgb_left[camera][frame]; }
+void Snapshot::getLeftDepth(size_t camera, size_t frame, cv::Mat &data) { data = depth_left[camera][frame]; }
 
 SnapshotReader::SnapshotReader(const string &filename) {
 	archive_ = archive_read_new();
+	int retval = ARCHIVE_OK;
+	string msg;
+
 	if (!archive_) goto error2;
 	archive_read_support_format_all(archive_);
 	archive_read_support_filter_all(archive_);
@@ -208,12 +275,22 @@ SnapshotReader::SnapshotReader(const string &filename) {
 	if (archive_read_open_filename(archive_, filename.c_str(), 4096) != ARCHIVE_OK)
 		goto error1;
 	
-	readArchive();
+	while((retval = archive_read_next_header(archive_, &entry_)) == ARCHIVE_OK) {
+		string path = string(archive_entry_pathname(entry_));
+		vector<uchar> data;
+		
+		if (readEntry(data)) { files_[path] = data; }
+	}
+
+	if (retval != ARCHIVE_EOF) { goto error1; }
+
 	return;
 	
 	error1:
-	LOG(ERROR) << archive_error_string(archive_);
+	msg = archive_error_string(archive_);
 	archive_read_free(archive_);
+	throw std::runtime_error(msg);
+
 	error2:
 	// throw exception; otherwise destructor might be called
 	throw std::runtime_error("SnapshotReader failed");
@@ -242,94 +319,183 @@ bool SnapshotReader::readEntry(vector<uchar> &data) {
 	}
 }
 
-SnapshotEntry& SnapshotReader::getEntry(const string &id) {
-	/*if (data_.find(id) == data_.end()) {
-		data_.emplace(id, SnapshotEntry{});
-	}*/
-	return data_[id];
-}
-
-/* read all entries to data_ */
-bool SnapshotReader::readArchive() {
-	int retval = ARCHIVE_OK;
-	vector<uchar> data;
-
-	while((retval = archive_read_next_header(archive_, &entry_)) == ARCHIVE_OK) {
-		string path = string(archive_entry_pathname(entry_));
-		if (path.rfind("-") == string::npos) {
-			LOG(WARNING) << "unrecognized file " << path;
-			continue;
-		}
-		string id = path.substr(0, path.find("-"));
-
-		SnapshotEntry &snapshot = getEntry(id);
+bool SnapshotReader::getDepth(const std::string &name, cv::Mat &data) {
+	if (files_.find(name) == files_.end()) {
+		LOG(ERROR) << name << " not found in archive";
+		return false;
+	}
 
-		// TODO: verify that input is valid
-		// TODO: check that earlier results are not overwritten (status)
+	const vector<uchar> &data_raw = files_[name];
+	const string ext = name.substr(name.find_last_of(".") + 1);
 
-		if (path.rfind("-RGB.") != string::npos) {
-			if (!readEntry(data)) continue;
-			snapshot.rgb = cv::imdecode(data, cv::IMREAD_COLOR);
-			snapshot.status &= ~1;
-		}
-		else if (path.rfind("-D.") != string::npos) {
-			if (!readEntry(data)) continue;
-			snapshot.depth = cv::imdecode(data, cv::IMREAD_ANYDEPTH);
-			snapshot.depth.convertTo(snapshot.depth, CV_32FC1, 1.0f / 1000.0f);
-			snapshot.status &= ~(1 << 1);
-		}
-		else if (path.rfind("-POSE.pfm") != string::npos) {
-			if (!readEntry(data)) continue;
-			Mat m_ = cv::imdecode(Mat(data), cv::IMREAD_ANYDEPTH);
-			if ((m_.rows != 4) || (m_.cols != 4)) continue;
-			cv::Matx44d pose_(m_);
-			cv::cv2eigen(pose_, snapshot.pose);
-			snapshot.status &= ~(1 << 2);
-		}
-		else if (path.rfind("-PARAMS.json") != string::npos) {
-			if (!readEntry(data)) continue;
-			nlohmann::json j = nlohmann::json::parse(string((const char*) data.data(), data.size()));
-			from_json(j, snapshot.params);
-			snapshot.status &= ~(1 << 3);
-		}
-		else {
-			LOG(WARNING) << "unknown file " << path;
-		}
+	if (ext == "tiff") {
+		data = cv::imdecode(data_raw, cv::IMREAD_ANYDEPTH);
 	}
-	
-	if (retval != ARCHIVE_EOF) {
-		LOG(ERROR) << archive_error_string(archive_);
+	else if (ext == "png") {
+		data = cv::imdecode(data_raw, cv::IMREAD_ANYDEPTH);
+		data.convertTo(data, CV_32FC1, 1.0f / 1000.0f);
+	}
+	else {
+		LOG(ERROR) << "Unsupported file extension for depth image: " << ext;
+		return false;
+	}
+
+	if (data.empty()) {
+		LOG(ERROR) << "Error decoding file: " << name;
 		return false;
 	}
 	
 	return true;
 }
 
-vector<string> SnapshotReader::getIds() {
-	vector<string> res;
-	res.reserve(data_.size());
-	for(auto itr = data_.begin(); itr != data_.end(); ++itr) {
-		res.push_back(itr->first);
+bool SnapshotReader::getRGB(const std::string &name, cv::Mat &data) {
+	if (files_.find(name) == files_.end()) {
+		LOG(ERROR) << name << " not found in archive";
+		return false;
 	}
-	return res;
-}
 
-bool SnapshotReader::getCameraRGBD(const string &id, Mat &rgb, Mat &depth,
-							 Matrix4d &pose, Camera &params) {
-	if (data_.find(id) == data_.end()) {
-		LOG(ERROR) << "entry not found: " << id;
+	const vector<uchar> &data_raw = files_[name];
+	const string ext = name.substr(name.find_last_of(".") + 1);
+	
+	if (!(ext == "png" || ext == "jpg")) { 
+		LOG(ERROR) << "Unsupported file extension for depth image: " << ext;
 		return false;
 	}
 
-	SnapshotEntry item = getEntry(id);
+	data = cv::imdecode(data_raw, cv::IMREAD_COLOR);
 
-	if (item.status != 0) {
-		LOG(ERROR) << "entry incomplete: " << id;
+	if (data.empty()) {
+		LOG(ERROR) << "Error decoding file: " << name;
+		return false;
 	}
-
-	rgb = item.rgb;
-	depth = item.depth;
-	params = item.params;
-	pose = item.pose;
+	
 	return true;
-}
\ No newline at end of file
+}
+
+Snapshot SnapshotReader::readArchive() {
+	Snapshot result;
+	
+	if (files_.find("index.yml") != files_.end()) {
+		LOG(INFO) << "Using new format snapshot archive";
+		string input;
+		{
+			vector<uchar> data = files_["index.yml"]; 
+			input = string((char*) data.data(), data.size());
+		}
+		FileStorage fs(input, FileStorage::READ | FileStorage::MEMORY);
+		
+		vector<string> &sources = result.sources;
+		vector<ftl::rgbd::Camera> &params = result.parameters;
+		vector<Mat> &extrinsic = result.extrinsic;
+		
+		vector<vector<Mat>> &rgb_left = result.rgb_left;
+		vector<vector<Mat>> &depth_left = result.depth_left;
+
+		vector<string> channels;
+
+		fs["sources"] >> sources;
+		fs["extrinsic"] >> extrinsic;
+		fs["channels"] >> channels;
+
+		cv::FileNode fn;
+		fn = fs["params"];
+		for (cv::FileNodeIterator it = fn.begin(); it != fn.end(); it++) {
+			vector<double> p;
+			*it >> p;
+
+			ftl::rgbd::Camera camera;
+			camera.fx = p[0];
+			camera.fy = p[1];
+			camera.cx = p[2];
+			camera.cy = p[3];
+			camera.width = p[4];
+			camera.height = p[5];
+			camera.minDepth = p[6];
+			camera.maxDepth = p[7];
+			camera.baseline = p[8];
+			camera.doffs = p[9];
+			params.push_back(camera);
+		}
+
+		vector<string> files;
+		for (auto const &channel : channels) {
+			files.clear();
+
+			if (channel == "time") {
+				//fs["time"] >> times;
+			}
+			else if (channel == "rgb_left") {
+				fn = fs["rgb_left"];
+				files.clear();
+				for (cv::FileNodeIterator it = fn.begin(); it != fn.end(); it++) {
+					*it >> files;
+					auto &images = rgb_left.emplace_back();
+					for (const string& file : files) {
+						Mat &img = images.emplace_back();
+						getRGB(file, img);
+					}
+				}
+			}
+			else if (channel == "depth_left") {
+				fn = fs["depth_left"];
+				files.clear();
+				for (cv::FileNodeIterator it = fn.begin(); it != fn.end(); it++) {
+					*it >> files;
+					auto &images = depth_left.emplace_back();
+					for (const string& file : files) {
+						Mat &img = images.emplace_back();
+						getDepth(file, img);
+					}
+				}
+			}
+			else {
+				LOG(ERROR) << "Unsupported channel: " << channel;
+			}
+		}
+
+		fs.release();
+	}
+	else {
+		LOG(INFO) << "Using old format snapshot archive";
+
+		result.n_cameras = 1;
+		result.n_frames = 1;
+		Mat &rgb = result.rgb_left.emplace_back().emplace_back();
+		Mat &depth = result.depth_left.emplace_back().emplace_back();
+		Mat &pose = result.extrinsic.emplace_back();
+		Camera &params = result.parameters.emplace_back();
+
+		for (auto const& [path, data] : files_) {
+			if (path.rfind("-") == string::npos) {
+				LOG(WARNING) << "unrecognized file " << path;
+				continue;
+			}
+			string id = path.substr(0, path.find("-"));
+
+			// TODO: verify that input is valid
+			// TODO: check that earlier results are not overwritten (status)
+			
+			if (path.rfind("-RGB.") != string::npos) {
+				getRGB(path, rgb);
+			}
+			else if (path.rfind("-D.") != string::npos) {
+				getDepth(path, depth);
+			}
+			else if (path.rfind("-POSE.pfm") != string::npos) {
+				Mat m_ = cv::imdecode(Mat(data), cv::IMREAD_ANYDEPTH);
+				if ((m_.rows != 4) || (m_.cols != 4)) continue;
+				cv::Matx44d pose_(m_);
+				pose = m_;
+			}
+			else if (path.rfind("-PARAMS.json") != string::npos) {
+				nlohmann::json j = nlohmann::json::parse(string((const char*) data.data(), data.size()));
+				from_json(j, params);
+			}
+			else {
+				LOG(WARNING) << "unknown file " << path;
+			}
+		}
+	}
+
+	return result;
+}

components/rgbd-sources/src/snapshot_source.cpp

@@ -13,22 +13,21 @@ using ftl::rgbd::detail::SnapshotSource;
 using std::string;
 using std::vector;
 
-SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader, const string &id) : detail::Source(host) {
-    Eigen::Matrix4d pose;
-    reader.getCameraRGBD(id, snap_rgb_, snap_depth_, pose, params_);
+SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, Snapshot &snapshot, const string &id) : detail::Source(host) {
+	snapshot_ = snapshot;
+	camera_idx_ = std::atoi(id.c_str());
+	frame_idx_ = 0;
 
-	rgb_ = snap_rgb_;
-	depth_ = snap_depth_;
-
-	if (rgb_.empty()) LOG(ERROR) << "Did not load snapshot rgb - " << id;
-	if (depth_.empty()) LOG(ERROR) << "Did not load snapshot depth - " << id;
-	if (params_.width != rgb_.cols) LOG(ERROR) << "Camera parameters corrupt for " << id;
+	Eigen::Matrix4d pose;
+	snapshot.getPose(camera_idx_, pose);
+	params_ = snapshot.getParameters(camera_idx_);
 
+	/*
 	ftl::rgbd::colourCorrection(rgb_, host->value("gamma", 1.0f), host->value("temperature", 6500));
-
 	host->on("gamma", [this,host](const ftl::config::Event&) {
 		ftl::rgbd::colourCorrection(rgb_, host->value("gamma", 1.0f), host->value("temperature", 6500));
 	});
+	*/
 
 	// Add calibration to config object
 	host_->getConfig()["focal"] = params_.fx;
@@ -51,11 +50,17 @@ SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader,
 
 	LOG(INFO) << "POSE = " << pose;
 
-    host->setPose(pose);
+	host->setPose(pose);
 }
 
 bool SnapshotSource::compute(int n, int b) {
+	snapshot_.getLeftRGB(camera_idx_, frame_idx_, snap_rgb_);
+	snapshot_.getLeftDepth(camera_idx_, frame_idx_, snap_depth_);
+
 	snap_rgb_.copyTo(rgb_);
 	snap_depth_.copyTo(depth_);
+
+	frame_idx_ = (frame_idx_ + 1) % snapshot_.getFramesCount();
+
 	return true;
 }

components/rgbd-sources/src/snapshot_source.hpp

@@ -14,7 +14,7 @@ namespace detail {
 class SnapshotSource : public detail::Source {
 	public:
 	SnapshotSource(ftl::rgbd::Source *);
-	SnapshotSource(ftl::rgbd::Source *, ftl::rgbd::SnapshotReader &reader, const std::string &id);
+	SnapshotSource(ftl::rgbd::Source *, ftl::rgbd::Snapshot &snapshot, const std::string &id);
 	~SnapshotSource() {};
 
 	bool compute(int n, int b);
@@ -22,6 +22,11 @@ class SnapshotSource : public detail::Source {
 
 	//void reset();
 	private:
+	size_t frame_idx_;
+	size_t camera_idx_;
+	
+	ftl::rgbd::Snapshot snapshot_;
+
 	cv::Mat snap_rgb_;
 	cv::Mat snap_depth_;
 };

components/rgbd-sources/src/source.cpp

@@ -120,7 +120,8 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) {
 		} else if (ext == "tar" || ext == "gz") {
 #ifdef HAVE_LIBARCHIVE
 			ftl::rgbd::SnapshotReader reader(path);
-			return new ftl::rgbd::detail::SnapshotSource(this, reader, value("index", std::string("0")));  // TODO: Use URI fragment
+			auto snapshot = reader.readArchive();
+			return new ftl::rgbd::detail::SnapshotSource(this, snapshot, value("index", std::string("0")));  // TODO: Use URI fragment
 #else
 			LOG(ERROR) << "Cannot read snapshots, libarchive not installed";
 			return nullptr;

components/rgbd-sources/test/source_unit.cpp

@@ -10,9 +10,12 @@ static std::string last_type = "";
 namespace ftl {
 namespace rgbd {
 
+class Snapshot {};
+
 class SnapshotReader {
 	public:
 	SnapshotReader(const std::string &) {}
+	Snapshot readArchive() { return Snapshot(); };
 };
 
 namespace detail {
@@ -55,7 +58,7 @@ class NetSource : public ftl::rgbd::detail::Source {
 
 class SnapshotSource : public ftl::rgbd::detail::Source {
 	public:
-	SnapshotSource(ftl::rgbd::Source *host, ftl::rgbd::SnapshotReader &r, const std::string &) : ftl::rgbd::detail::Source(host) {
+	SnapshotSource(ftl::rgbd::Source *host, ftl::rgbd::Snapshot &r, const std::string &) : ftl::rgbd::detail::Source(host) {
 		last_type = "snapshot";
 	}