From 3054bca849d9c48b8dd674941a11597ba43d1029 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Sat, 26 Oct 2019 17:53:23 +0300
Subject: [PATCH] Fix old file loading and add config support
---
.../codecs/include/ftl/codecs/reader.hpp | 1 +
components/codecs/src/reader.cpp | 3 +-
components/rgbd-sources/src/source.cpp | 1 +
.../src/sources/ftlfile/file_source.cpp | 86 +++++++++++++++----
.../src/sources/ftlfile/file_source.hpp | 2 +
.../rgbd-sources/src/sources/net/net.cpp | 10 ++-
components/rgbd-sources/src/streamer.cpp | 5 +-
7 files changed, 89 insertions(+), 19 deletions(-)
diff --git a/components/codecs/include/ftl/codecs/reader.hpp b/components/codecs/include/ftl/codecs/reader.hpp
index 69e19f0a5..c3ea0adc6 100644
--- a/components/codecs/include/ftl/codecs/reader.hpp
+++ b/components/codecs/include/ftl/codecs/reader.hpp
@@ -41,6 +41,7 @@ class Reader {
bool end();
inline int64_t getStartTime() const { return timestart_; };
+ inline int version() const { return version_; }
private:
std::istream *stream_;
diff --git a/components/codecs/src/reader.cpp b/components/codecs/src/reader.cpp
index 3727bdd38..97a74acd1 100644
--- a/components/codecs/src/reader.cpp
+++ b/components/codecs/src/reader.cpp
@@ -28,6 +28,7 @@ bool Reader::begin() {
}
version_ = h.version;
+ LOG(INFO) << "FTL format version " << version_;
// Capture current time to adjust timestamps
timestart_ = (ftl::timer::get_time() / ftl::timer::getInterval()) * ftl::timer::getInterval();
@@ -89,7 +90,7 @@ bool Reader::read(int64_t ts, const std::function<void(const ftl::codecs::Stream
get<0>(data).timestamp += timestart_;
// Fix to clear flags for version 2.
- if (version_ == 2) {
+ if (version_ <= 2) {
get<1>(data).flags = 0;
}
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 8e402bf33..4c7485460 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -271,6 +271,7 @@ void Source::removeRawCallback(const std::function<void(ftl::rgbd::Source*, cons
void Source::notifyRaw(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
SHARED_LOCK(mutex_,lk);
+
for (auto &i : rawcallbacks_) {
i(this, spkt, pkt);
}
diff --git a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
index d8ebdb399..76cd435e7 100644
--- a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
+++ b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
@@ -33,25 +33,42 @@ FileSource::FileSource(ftl::rgbd::Source *s, ftl::rgbd::Player *r, int sid) : ft
r->onPacket(sid, [this](const ftl::codecs::StreamPacket &spkt, ftl::codecs::Packet &pkt) {
host_->notifyRaw(spkt, pkt);
+
+ // Should config items be parsed here?
+ if (spkt.channel == Channel::Configuration) {
+ std::tuple<std::string, std::string> cfg;
+ auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size());
+ unpacked.get().convert(cfg);
+
+ LOG(INFO) << "Config Received: " << std::get<1>(cfg);
+ }
+ else if (spkt.channel == Channel::Calibration) {
+ _processCalibration(pkt);
+ return;
+ } else if (spkt.channel == Channel::Pose) {
+ _processPose(pkt);
+ return;
+ }
+
+ // FIXME: For bad and old FTL files
if (pkt.codec == codec_t::POSE) {
- Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data());
- host_->setPose(p);
+ _processPose(pkt);
+ return;
} else if (pkt.codec == codec_t::CALIBRATION) {
- ftl::rgbd::Camera *camera = (ftl::rgbd::Camera*)pkt.data.data();
- LOG(INFO) << "Have calibration: " << camera->fx;
- params_ = *camera;
- has_calibration_ = true;
- } else {
- if (pkt.codec == codec_t::HEVC) {
- if (ftl::codecs::hevc::isIFrame(pkt.data)) _removeChannel(spkt.channel);
- }
- cache_[cache_write_].emplace_back();
- auto &c = cache_[cache_write_].back();
-
- // TODO: Attempt to avoid this copy operation
- c.spkt = spkt;
- c.pkt = pkt;
+ _processCalibration(pkt);
+ return;
+ }
+
+
+ if (pkt.codec == codec_t::HEVC) {
+ if (ftl::codecs::hevc::isIFrame(pkt.data)) _removeChannel(spkt.channel);
}
+ cache_[cache_write_].emplace_back();
+ auto &c = cache_[cache_write_].back();
+
+ // TODO: Attempt to avoid this copy operation
+ c.spkt = spkt;
+ c.pkt = pkt;
});
}
@@ -59,6 +76,38 @@ FileSource::~FileSource() {
}
+void FileSource::_processPose(ftl::codecs::Packet &pkt) {
+ LOG(INFO) << "Got POSE channel";
+ if (pkt.codec == codec_t::POSE) {
+ Eigen::Matrix4d p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data());
+ host_->setPose(p);
+ } else if (pkt.codec == codec_t::MSGPACK) {
+
+ }
+}
+
+void FileSource::_processCalibration(ftl::codecs::Packet &pkt) {
+ if (pkt.codec == codec_t::CALIBRATION) {
+ ftl::rgbd::Camera *camera = (ftl::rgbd::Camera*)pkt.data.data();
+ params_ = *camera;
+ has_calibration_ = true;
+ } else if (pkt.codec == codec_t::MSGPACK) {
+ std::tuple<ftl::rgbd::Camera, ftl::codecs::Channel, ftl::rgbd::capability_t> params;
+ auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size());
+ unpacked.get().convert(params);
+
+ if (std::get<1>(params) == Channel::Left) {
+ params_ = std::get<0>(params);
+ capabilities_ = std::get<2>(params);
+ has_calibration_ = true;
+
+ LOG(INFO) << "Got Calibration channel: " << params_.width << "x" << params_.height;
+ } else {
+ //params_right_ = std::get<0>(params);
+ }
+ }
+}
+
void FileSource::_removeChannel(ftl::codecs::Channel channel) {
int c = 0;
for (auto i=cache_[cache_write_].begin(); i != cache_[cache_write_].end(); ++i) {
@@ -108,6 +157,11 @@ bool FileSource::compute(int n, int b) {
lastc++;
}
+ //LOG(INFO) << "DECODE FRAME: " << c.spkt.timestamp << "," << (int)c.spkt.channel << "," << (int)c.pkt.codec << "," << (int)c.pkt.definition;
+
+ // FIXME: This hack is for old and bad ftl files.
+ //if ((int)c.pkt.codec >= 100) continue;
+
if (c.spkt.channel == Channel::Colour) {
rgb_.create(cv::Size(ftl::codecs::getWidth(c.pkt.definition),ftl::codecs::getHeight(c.pkt.definition)), CV_8UC3);
} else {
diff --git a/components/rgbd-sources/src/sources/ftlfile/file_source.hpp b/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
index 80f3b368b..2d59b68cb 100644
--- a/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
+++ b/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
@@ -44,6 +44,8 @@ class FileSource : public detail::Source {
bool realtime_;
+ void _processCalibration(ftl::codecs::Packet &pkt);
+ void _processPose(ftl::codecs::Packet &pkt);
void _removeChannel(ftl::codecs::Channel channel);
void _createDecoder(int ix, const ftl::codecs::Packet &pkt);
};
diff --git a/components/rgbd-sources/src/sources/net/net.cpp b/components/rgbd-sources/src/sources/net/net.cpp
index 39227c5e6..1843203e1 100644
--- a/components/rgbd-sources/src/sources/net/net.cpp
+++ b/components/rgbd-sources/src/sources/net/net.cpp
@@ -289,7 +289,15 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk
const ftl::codecs::Channel rchan = spkt.channel;
const int channum = (rchan == Channel::Colour) ? 0 : 1;
- if (rchan == Channel::Calibration) {
+ // Should config items be parsed here?
+ if (rchan == Channel::Configuration) {
+ std::tuple<std::string, std::string> cfg;
+ auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size());
+ unpacked.get().convert(cfg);
+
+ LOG(INFO) << "Config Received: " << std::get<1>(cfg);
+ }
+ else if (rchan == Channel::Calibration) {
std::tuple<ftl::rgbd::Camera, ftl::codecs::Channel, ftl::rgbd::capability_t> params;
auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size());
unpacked.get().convert(params);
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 3eff9e24f..8ecda51b7 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -338,7 +338,10 @@ void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID
// Finally, inject calibration and config data
s->src->inject(Channel::Calibration, s->src->parameters(Channel::Left), Channel::Left, s->src->getCapabilities());
s->src->inject(Channel::Calibration, s->src->parameters(Channel::Right), Channel::Right, s->src->getCapabilities());
- //s->src->inject(Channel::Pose, s->src->getPose());
+ //s->src->inject(s->src->getPose());
+ //if (!(*s->src->get<nlohmann::json>("meta")).is_null()) {
+ s->src->inject(Channel::Configuration, "/original", s->src->getConfig().dump());
+ //}
}
void Streamer::remove(Source *) {
--
GitLab