diff --git a/applications/groupview/src/main.cpp b/applications/groupview/src/main.cpp
index eadda40ce19c586401a8a6c7464d461d9bc2b110..ed163daa525609ba131ff916e50b63ce92c6fcdf 100644
--- a/applications/groupview/src/main.cpp
+++ b/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;
diff --git a/applications/gui/src/media_panel.cpp b/applications/gui/src/media_panel.cpp
index 44bb92ec099b5d961142ec4fc1fed95e200c7bd2..2ee06a69d09690e497bb8f88283a947b466aae46 100644
--- a/applications/gui/src/media_panel.cpp
+++ b/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)";
diff --git a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
index edec6150217a6655021b94d49ef36d6018d87374..f9bb39756362c344877df441a445a07f3e5eab37 100644
--- a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
+++ b/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 ¶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");
+ void addSource(const std::string &id, const ftl::rgbd::Camera ¶ms, const Eigen::Matrix4d &extrinsic);
+ void addSource(const std::string &id, const std::vector<double> ¶ms, 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> ¶ms);
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 ¶ms);
- 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_;
};
-
};
};
diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp
index 20202302693721ce8f136ecdd3db8cd227f42db0..372c6124d2e530685923823712e043309ec48b7a 100644
--- a/components/rgbd-sources/src/snapshot.cpp
+++ b/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 ¶meters) {
+ 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> ¶ms) {
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> ¶ms, 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 ¶ms) {
+void SnapshotWriter::addSource(const std::string &id, const ftl::rgbd::Camera ¶ms, 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 ¶ms) {
- 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> ¶ms = 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 ¶ms = 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;
+}
diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp
index aa5268549bbf32de9a311a93e39aeed5e6a13555..7eb5bc6740540a55e310d63560e028b18b4148fc 100644
--- a/components/rgbd-sources/src/snapshot_source.cpp
+++ b/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;
}
diff --git a/components/rgbd-sources/src/snapshot_source.hpp b/components/rgbd-sources/src/snapshot_source.hpp
index eb4e58594323b7f1167976b13d8d46c6b96c8fd0..41bfe690cc69cadc2dc43316c004f327d3f895f2 100644
--- a/components/rgbd-sources/src/snapshot_source.hpp
+++ b/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_;
};
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 17e343a00b9523404f8a5b48ff85d15c0a0d9f9a..c52c4100c11acd626bc7d7412cb301793351b727 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/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;
diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp
index 1c80e8292ff27e3c2652e7453dff59a1acea7bda..1bbb336aaad3a6c25f3e515b89c7e6142da5b852 100644
--- a/components/rgbd-sources/test/source_unit.cpp
+++ b/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";
}