diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 929582d1844f211ef37bdfe8361605dc66e169c1..3ee32ab4a693c12c280af59c3d1328b7b8eaeb30 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -164,11 +164,12 @@ static void run(ftl::Configurable *root) { std::ofstream fileout; ftl::codecs::Writer writer(fileout); - auto recorder = [&writer](ftl::rgbd::Source *src, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) { - LOG(INFO) << "About to write"; - // TODO: Patch spkt with correct streamID - // TODO: Also adapt timestamp inside writer to be file relative - writer.write(spkt, pkt); + auto recorder = [&writer,&group](ftl::rgbd::Source *src, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) { + ftl::codecs::StreamPacket s = spkt; + + // Patch stream ID to match order in group + s.streamID = group.streamID(src); + writer.write(s, pkt); }; root->set("record", false); diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 2b056d009a73dd7ee82adaedbf03bb98194d636f..e064f91a259874b68879627e2fbb8ff44b24f2ca 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/components/rgbd-sources/CMakeLists.txt @@ -19,6 +19,7 @@ set(RGBDSRC src/abr.cpp src/offilter.cpp src/virtual.cpp + src/file_source.cpp ) if (HAVE_REALSENSE) diff --git a/components/rgbd-sources/include/ftl/rgbd/group.hpp b/components/rgbd-sources/include/ftl/rgbd/group.hpp index f32ada4f70df19e026358b02337ca3a6caea9785..fdd7ba7412a2f0d247fb545150cd428ae4519d9f 100644 --- a/components/rgbd-sources/include/ftl/rgbd/group.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/group.hpp @@ -99,6 +99,8 @@ class Group { void stop() {} + int streamID(const ftl::rgbd::Source *s) const; + private: std::vector<FrameSet> framesets_; std::vector<Source*> sources_; diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp index 4c27baf866fc8a80d30f37ec3b794df4e0368916..18413cb8941c56f09f6189895a098a517b276c7a 100644 --- a/components/rgbd-sources/include/ftl/rgbd/source.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp @@ -9,9 +9,11 @@ #include <ftl/uri.hpp> #include <ftl/rgbd/detail/source.hpp> #include <ftl/codecs/packet.hpp> +#include <ftl/codecs/reader.hpp> #include <opencv2/opencv.hpp> #include <Eigen/Eigen> #include <string> +#include <map> #include <ftl/cuda_common.hpp> #include <ftl/rgbd/frame.hpp> @@ -244,6 +246,10 @@ class Source : public ftl::Configurable { detail::Source *_createFileImpl(const ftl::URI &uri); detail::Source *_createNetImpl(const ftl::URI &uri); detail::Source *_createDeviceImpl(const ftl::URI &uri); + + static ftl::codecs::Reader *__createReader(const std::string &path); + + static std::map<std::string, ftl::codecs::Reader*> readers__; }; } diff --git a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp index 642add6dc0e39ab496fec560cd3a5b0d77dbfb53..c1a1e4b38a2cdcafea900c2b5728b0c75a9d9b4f 100644 --- a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp @@ -54,6 +54,7 @@ struct StreamSource { std::list<detail::StreamClient> clients; SHARED_MUTEX mutex; unsigned long long frame; + int id; ftl::codecs::Encoder *hq_encoder_c1 = nullptr; ftl::codecs::Encoder *hq_encoder_c2 = nullptr; diff --git a/components/rgbd-sources/src/file_source.cpp b/components/rgbd-sources/src/file_source.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a7fe61169c0cdb626341756eaa18d8e2f0126a2d --- /dev/null +++ b/components/rgbd-sources/src/file_source.cpp @@ -0,0 +1,31 @@ +#include "file_source.hpp" + +using ftl::rgbd::detail::FileSource; + +FileSource::FileSource(ftl::rgbd::Source *s, ftl::codecs::Reader *r, int sid) : ftl::rgbd::detail::Source(s) { + reader_ = r; + r->onPacket(sid, [this](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) { + LOG(INFO) << "PACKET RECEIVED " << spkt.streamID; + }); +} + +FileSource::~FileSource() { + +} + +bool FileSource::capture(int64_t ts) { + reader_->read(ts); + return true; +} + +bool FileSource::retrieve() { + return true; +} + +bool FileSource::compute(int n, int b) { + return true; +} + +bool FileSource::isReady() { + return true; +} diff --git a/components/rgbd-sources/src/file_source.hpp b/components/rgbd-sources/src/file_source.hpp new file mode 100644 index 0000000000000000000000000000000000000000..f69ec8321e381eb749484beed2c34db5cbba2d74 --- /dev/null +++ b/components/rgbd-sources/src/file_source.hpp @@ -0,0 +1,33 @@ +#pragma once +#ifndef _FTL_RGBD_FILE_SOURCE_HPP_ +#define _FTL_RGBD_FILE_SOURCE_HPP_ + +#include <loguru.hpp> + +#include <ftl/rgbd/source.hpp> +#include <ftl/codecs/reader.hpp> + +namespace ftl { +namespace rgbd { +namespace detail { + +class FileSource : public detail::Source { + public: + FileSource(ftl::rgbd::Source *, ftl::codecs::Reader *, int sid); + ~FileSource(); + + bool capture(int64_t ts); + bool retrieve(); + bool compute(int n, int b); + bool isReady(); + + //void reset(); + private: + ftl::codecs::Reader *reader_; +}; + +} +} +} + +#endif // _FTL_RGBD_FILE_SOURCE_HPP_ diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp index 6b684940d101892a9b7ecd8312878e05b2c2230e..4a85c3d7cee7a9f897d0f9fdb87ceba1fa86e671 100644 --- a/components/rgbd-sources/src/group.cpp +++ b/components/rgbd-sources/src/group.cpp @@ -145,6 +145,13 @@ void Group::_computeJob(ftl::rgbd::Source *src) { } } +int Group::streamID(const ftl::rgbd::Source *s) const { + for (int i=0; i<sources_.size(); ++i) { + if (sources_[i] == s) return i; + } + return -1; +} + void Group::sync(std::function<bool(ftl::rgbd::FrameSet &)> cb) { if (latency_ == 0) { callback_ = cb; diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp index 416defb9a667c9b47890e15d38e1182965994b4e..8712f48d84498c0075aea717a0a2582d50ec4a70 100644 --- a/components/rgbd-sources/src/net.cpp +++ b/components/rgbd-sources/src/net.cpp @@ -246,7 +246,7 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk host_->notifyRaw(spkt, pkt); const ftl::rgbd::Channel chan = host_->getChannel(); - int rchan = spkt.channel & 0x1; + int rchan = spkt.channel; // & 0x1; NetFrame &frame = queue_.getFrame(spkt.timestamp, cv::Size(params_.width, params_.height), CV_8UC3, (isFloatChannel(chan) ? CV_32FC1 : CV_8UC3)); @@ -280,7 +280,8 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk // Calculate how many packets to expect for this frame if (frame.chunk_total == 0) { // Getting a second channel first means expect double packets - frame.chunk_total = pkt.block_total * ((spkt.channel >> 1) + 1); + // FIXME: Assumes each packet has same number of blocks! + frame.chunk_total = pkt.block_total * spkt.channel_count; } ++frame.chunk_count; diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index dcab5bd24adc81f4cdb9c80e20ce0279c29b7af4..db2dfa10848a6c56affc205092cb960bb3424a79 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/components/rgbd-sources/src/source.cpp @@ -12,6 +12,8 @@ #include "snapshot_source.hpp" #endif +#include "file_source.hpp" + #ifdef HAVE_REALSENSE #include "realsense_source.hpp" using ftl::rgbd::detail::RealsenseSource; @@ -26,6 +28,9 @@ using ftl::rgbd::detail::ImageSource; using ftl::rgbd::detail::MiddleburySource; using ftl::rgbd::capability_t; using ftl::rgbd::Channel; +using ftl::rgbd::detail::FileSource; + +std::map<std::string, ftl::codecs::Reader*> Source::readers__; Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) { impl_ = nullptr; @@ -115,8 +120,8 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) { string ext = path.substr(eix+1); if (ext == "ftl") { - //auto *reader = _createReader(path); - //return new PlayerSource(player, std::stoi(uri.getFragment())); + ftl::codecs::Reader *reader = __createReader(path); + return new FileSource(this, reader, std::stoi(uri.getFragment())); } else if (ext == "png" || ext == "jpg") { return new ImageSource(this, path); } else if (ext == "mp4") { @@ -140,6 +145,21 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) { return nullptr; } +ftl::codecs::Reader *Source::__createReader(const std::string &path) { + if (readers__.find(path) != readers__.end()) { + return readers__[path]; + } + + std::ifstream *file = new std::ifstream; + file->open(path); + + // FIXME: This is a memory leak, must delete ifstream somewhere. + + auto *r = new ftl::codecs::Reader(*file); + readers__[path] = r; + return r; +} + ftl::rgbd::detail::Source *Source::_createNetImpl(const ftl::URI &uri) { return new NetSource(this); } diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp index 5d8235a76139a6e1fe94ee5b612fdf128255059f..462d49946281521130fe4ea940312e8811576c07 100644 --- a/components/rgbd-sources/src/streamer.cpp +++ b/components/rgbd-sources/src/streamer.cpp @@ -176,7 +176,8 @@ void Streamer::add(Source *src) { void Streamer::add(ftl::rgbd::Group *grp) { auto srcs = grp->sources(); - for (auto src : srcs) { + for (int i=0; i<srcs.size(); ++i) { + auto &src = srcs[i]; { UNIQUE_LOCK(mutex_,ulk); if (sources_.find(src->getID()) != sources_.end()) return; @@ -189,6 +190,7 @@ void Streamer::add(ftl::rgbd::Group *grp) { s->clientCount = 0; s->hq_count = 0; s->lq_count = 0; + s->id = i; sources_[src->getID()] = s; //group_.addSource(src); @@ -528,8 +530,10 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) { void Streamer::_transmitPacket(StreamSource *src, const ftl::codecs::Packet &pkt, int chan, bool hasChan2, Quality q) { ftl::codecs::StreamPacket spkt = { frame_no_, - 0, - static_cast<uint8_t>((chan & 0x1) | ((hasChan2) ? 0x2 : 0x0)) + src->id, + (hasChan2) ? 2 : 1, + chan + //static_cast<uint8_t>((chan & 0x1) | ((hasChan2) ? 0x2 : 0x0)) }; _transmitPacket(src, spkt, pkt, q); diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp index dca38be2ffb6e06b8be416c8de71a7a799c77867..ca66273a99f75f58d118d16237f558017911ee20 100644 --- a/components/rgbd-sources/test/source_unit.cpp +++ b/components/rgbd-sources/test/source_unit.cpp @@ -74,6 +74,18 @@ class SnapshotSource : public ftl::rgbd::detail::Source { bool isReady() { return true; }; }; +class FileSource : public ftl::rgbd::detail::Source { + public: + FileSource(ftl::rgbd::Source *host, ftl::codecs::Reader *r, int) : ftl::rgbd::detail::Source(host) { + last_type = "filesource"; + } + + bool capture(int64_t ts) { return true; } + bool retrieve() { return true; } + bool compute(int n, int b) { return true; }; + bool isReady() { return true; }; +}; + class RealsenseSource : public ftl::rgbd::detail::Source { public: explicit RealsenseSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) { @@ -112,6 +124,7 @@ class MiddleburySource : public ftl::rgbd::detail::Source { #define _FTL_RGBD_IMAGE_HPP_ #define _FTL_RGBD_REALSENSE_HPP_ #define _FTL_RGBD_MIDDLEBURY_SOURCE_HPP_ +#define _FTL_RGBD_FILE_SOURCE_HPP_ #include "../src/source.cpp"