diff --git a/CMakeLists.txt b/CMakeLists.txt index 512d8b801426d59314a6baca5ddd7e2e7f13d342..e4f87adaa78881aaebb3d523ead2bbe4a5db30fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,11 @@ find_package( URIParser REQUIRED ) find_package( MsgPack REQUIRED ) find_package( Eigen3 REQUIRED ) +find_package( LibArchive ) +if (LibArchive_FOUND) + set(HAVE_LIBARCHIVE true) +endif() + if (WITH_FIXSTARS) find_package( LibSGM ) if (LibSGM_FOUND) diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 637255efd1109cb04112c6e867c0fcf88ac8db2b..fe828144030c81c66e754bae65c0bde1fcb20951 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/components/rgbd-sources/CMakeLists.txt @@ -12,6 +12,13 @@ set(RGBDSRC src/algorithms/opencv_bm.cpp ) +if (LIBARCHIVE_FOUND) + list(APPEND RGBDSRC + "src/snapshot.cpp" + "src/snapshot_source.cpp" + ) +endif (LIBARCHIVE_FOUND) + if (LIBSGM_FOUND) list(APPEND RGBDSRC "src/algorithms/fixstars_sgm.cpp") endif (LIBSGM_FOUND) @@ -44,6 +51,6 @@ set_property(TARGET ftlrgbd PROPERTY CUDA_SEPARABLE_COMPILATION OFF) endif() #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include) -target_link_libraries(ftlrgbd ftlcommon Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen glog::glog ftlnet) +target_link_libraries(ftlrgbd ftlcommon Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen glog::glog ftlnet ${LibArchive_LIBRARIES}) diff --git a/components/rgbd-sources/include/ftl/rgbd.hpp b/components/rgbd-sources/include/ftl/rgbd.hpp index c85524e9c344fe9e885f707804d814d737fb93e3..774ebfe67bdc96d5ef245b9a0cfa6bc6f5e462d7 100644 --- a/components/rgbd-sources/include/ftl/rgbd.hpp +++ b/components/rgbd-sources/include/ftl/rgbd.hpp @@ -8,4 +8,8 @@ #include <ftl/stereovideo_source.hpp> #include <ftl/net_source.hpp> +#ifdef HAVE_LIBARCHIVE +#include <ftl/snapshot_source.hpp> +#endif // HAVE_LIBARCHIVE + #endif // _FTL_RGBD_HPP_ diff --git a/components/rgbd-sources/include/ftl/snapshot.hpp b/components/rgbd-sources/include/ftl/snapshot.hpp new file mode 100644 index 0000000000000000000000000000000000000000..c4791900097ef87c90c41ba2e2578ee71b1cfe42 --- /dev/null +++ b/components/rgbd-sources/include/ftl/snapshot.hpp @@ -0,0 +1,69 @@ +#pragma once +#ifndef _FTL_RGBD_SNAPSHOT_HPP_ +#define _FTL_RGBD_SNAPSHOT_HPP_ + +#include <glog/logging.h> + +#include <opencv2/opencv.hpp> + +#include <Eigen/Eigen> +#include <opencv2/core/eigen.hpp> + +#include <ftl/camera_params.hpp> + +#include <archive.h> +#include <archive_entry.h> + +namespace ftl { +namespace rgbd { + +// FIXME: NOT thread safe + +class SnapshotWriter { +public: + SnapshotWriter(const std::string &filename); + ~SnapshotWriter(); + + bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4f &pose, const ftl::rgbd::CameraParameters ¶ms); + bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format="tiff"); + bool addEigenMatrix4f(const std::string &name, const Eigen::Matrix4f &m, const std::string &format="pfm"); + bool addFile(const std::string &name, const std::vector<uchar> &buf); + bool addFile(const std::string &name, const uchar *buf, const size_t len); + +private: + struct archive *archive_; + struct archive_entry *entry_; +}; + +struct SnapshotEntry { + cv::Mat rgb; + cv::Mat depth; + Eigen::Matrix4f pose; + ftl::rgbd::CameraParameters params; + uint status; + SnapshotEntry() : status(1+2+4+8) {}; +}; + +class SnapshotReader { +public: + SnapshotReader(const std::string &filename); + ~SnapshotReader(); + + bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4f &pose, ftl::rgbd::CameraParameters ¶ms); + std::vector<std::string> getIds(); + +private: + SnapshotEntry& getEntry(const std::string &id); + bool readEntry(std::vector<uchar> &data); + bool readArchive(); + + std::map<std::string, SnapshotEntry> data_; + struct archive *archive_; + struct archive_entry *entry_; +}; + + +}; +}; + +#endif // _FTL_RGBD_SNAPSHOT_HPP_ diff --git a/components/rgbd-sources/include/ftl/snapshot_source.hpp b/components/rgbd-sources/include/ftl/snapshot_source.hpp new file mode 100644 index 0000000000000000000000000000000000000000..3de8064def2e4cb2488f9b7e4531847265a59932 --- /dev/null +++ b/components/rgbd-sources/include/ftl/snapshot_source.hpp @@ -0,0 +1,23 @@ +#pragma once +#ifndef _FTL_RGBD_SNAPSHOT_SOURCE_HPP_ +#define _FTL_RGBD_SNAPSHOT_SOURCE_HPP_ + +#include <glog/logging.h> + +#include "ftl/rgbd_source.hpp" +#include "ftl/snapshot.hpp" + +namespace ftl { +namespace rgbd { + +class SnapshotSource : public RGBDSource { + public: + SnapshotSource(nlohmann::json &config, ftl::rgbd::SnapshotReader &reader, const std::string &id); + ~SnapshotSource() {}; + void grab() override {}; +}; + +}; +}; + +#endif // _FTL_RGBD_SNAPSHOT_SOURCE_HPP_ diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5142612b9d58b01ac0be7fb3172021e2de663d91 --- /dev/null +++ b/components/rgbd-sources/src/snapshot.cpp @@ -0,0 +1,276 @@ +#include <ftl/snapshot.hpp> + +#include <nlohmann/json.hpp> + +using namespace ftl::rgbd; + +using cv::Mat; +using Eigen::Matrix4f; + +using cv::imencode; +using cv::imdecode; + +using std::string; +using std::vector; + +using Eigen::Matrix4f; + +// TODO: move to camera_params +using ftl::rgbd::CameraParameters; + +void to_json(nlohmann::json& j, const CameraParameters &p) { + j = nlohmann::json{ + {"fx", p.fx}, + {"fy", p.fy}, + {"cx", p.cx}, + {"cy", p.cy}, + {"width", p.width}, + {"height", p.height}, + {"minDepth", p.minDepth}, + {"maxDepth", p.maxDepth} + }; +} + +void from_json(const nlohmann::json& j, CameraParameters &p) { + j.at("fx").get_to(p.fx); + j.at("fy").get_to(p.fy); + j.at("cx").get_to(p.cx); + j.at("cy").get_to(p.cy); + j.at("width").get_to(p.width); + j.at("height").get_to(p.height); + j.at("minDepth").get_to(p.minDepth); + j.at("maxDepth").get_to(p.maxDepth); +} +// + +SnapshotWriter::SnapshotWriter(const string &filename) { + archive_ = archive_write_new(); + if (!archive_) goto error3; + entry_ = archive_entry_new(); + if (!entry_) goto error2; + + if (archive_write_set_format_pax_restricted(archive_) != ARCHIVE_OK) + goto error1; + + // todo make compression optional (or remove it) + if (archive_write_add_filter_gzip(archive_) != ARCHIVE_OK) + goto error1; + if (archive_write_open_filename(archive_, filename.c_str()) != ARCHIVE_OK) + goto error1; + + return; + + error1: + archive_entry_free(entry_); + error2: + LOG(ERROR) << archive_error_string(archive_); + archive_write_free(archive_); + error3: + // throw exception; otherwise destructor might be called + throw std::runtime_error("SnapshotWriter failed"); +} + +SnapshotWriter::~SnapshotWriter() { + archive_entry_free(entry_); + archive_write_close(archive_); + archive_write_free(archive_); +} + +bool SnapshotWriter::addFile(const string &name, const uchar *buf, const size_t len) { + archive_entry_clear(entry_); + archive_entry_set_pathname(entry_, name.c_str()); + archive_entry_set_size(entry_, len); + archive_entry_set_filetype(entry_, AE_IFREG); + archive_entry_set_perm(entry_, 0644); + + size_t l = len; + if (archive_write_header(archive_, entry_) != ARCHIVE_OK) goto error; + + while (true) { + ssize_t ret_w = archive_write_data(archive_, buf, l); + if (ret_w == 0) { break; } + if (ret_w < 0) { goto error; } + else { + l -= ret_w; + buf = buf + ret_w; + } + } + return true; + + error: + LOG(ERROR) << archive_error_string(archive_); + return false; +} + +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) { + 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::addEigenMatrix4f(const string &name, const Matrix4f &m, const string &format) { + Mat tmp; + cv::eigen2cv(m, tmp); + return addMat(name, tmp, format); +} + +bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth, + const Matrix4f &pose, const CameraParameters ¶ms) { + bool retval = true; + retval &= addMat(name + "-RGB", rgb); + retval &= addMat(name + "-D", depth); + retval &= addEigenMatrix4f(name + "-POSE", pose); + + 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; +} + + +SnapshotReader::SnapshotReader(const string &filename) { + archive_ = archive_read_new(); + if (!archive_) goto error2; + archive_read_support_format_all(archive_); + archive_read_support_filter_all(archive_); + + if (archive_read_open_filename(archive_, filename.c_str(), 4096) != ARCHIVE_OK) + goto error1; + + readArchive(); + return; + + error1: + LOG(ERROR) << archive_error_string(archive_); + archive_read_free(archive_); + error2: + // throw exception; otherwise destructor might be called + throw std::runtime_error("SnapshotReader failed"); +} + +SnapshotReader::~SnapshotReader() { + archive_read_free(archive_); +} + +bool SnapshotReader::readEntry(vector<uchar> &data) { + if (!archive_entry_size_is_set(entry_)) { + LOG(ERROR) << "entry size unknown"; + return false; + } + + size_t size = archive_entry_size(entry_); + size_t size_read = 0; + data.resize(size); + uchar *buf = data.data(); + + while(true) { + ssize_t size_read_new = archive_read_data(archive_, buf + size_read, size - size_read); + if (size_read_new < 0) return false; + if (size_read_new == 0) return true; + size_read += size_read_new; + } +} + +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); + + // TODO: verify that input is valid + // TODO: check that earlier results are not overwritten (status) + + 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.status &= ~(1 << 1); + } + else if (path.rfind("-POSE.pfm") != string::npos) { + if (!readEntry(data)) continue; + Mat m_ = cv::imdecode(Mat(data), 0); + if ((m_.rows != 4) || (m_.cols != 4)) continue; + cv::Matx44f 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 (retval != ARCHIVE_EOF) { + LOG(ERROR) << archive_error_string(archive_); + 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); + } + return res; +} + +bool SnapshotReader::getCameraRGBD(const string &id, Mat &rgb, Mat &depth, + Matrix4f &pose, CameraParameters ¶ms) { + if (data_.find(id) == data_.end()) { + LOG(ERROR) << "entry not found: " << id; + return false; + } + + SnapshotEntry item = getEntry(id); + + if (item.status != 0) { + LOG(ERROR) << "entry incomplete: " << id; + } + + rgb = item.rgb; + depth = item.depth; + params = item.params; + pose = item.pose; + return true; +} \ No newline at end of file diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp new file mode 100644 index 0000000000000000000000000000000000000000..40751e58a523f5b2db8fe3d1788ab456ff1ef7b8 --- /dev/null +++ b/components/rgbd-sources/src/snapshot_source.cpp @@ -0,0 +1,15 @@ +#include "ftl/snapshot_source.hpp" + +#include <opencv2/opencv.hpp> +#include <Eigen/Eigen> +#include <opencv2/core/eigen.hpp> + +using namespace ftl::rgbd; + +using std::string; + +SnapshotSource::SnapshotSource(nlohmann::json &config, SnapshotReader &reader, const string &id) : RGBDSource(config) { + Eigen::Matrix4f pose; + reader.getCameraRGBD(id, rgb_, depth_, pose, params_); + setPose(pose); +}