From e823755c13bce4383d2b41810978c70789264932 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Tue, 3 Mar 2020 09:06:34 +0200
Subject: [PATCH] Fix clipping on recon and send frameset data chans
---
components/operators/src/clipping.cpp | 11 +++---
.../include/ftl/rgbd/frameset.hpp | 6 ++++
components/rgbd-sources/src/frameset.cpp | 35 +++++++++++++------
components/streams/src/receiver.cpp | 17 +++++++--
components/streams/src/sender.cpp | 19 ++++++++++
.../structures/include/ftl/data/frameset.hpp | 2 ++
6 files changed, 73 insertions(+), 17 deletions(-)
diff --git a/components/operators/src/clipping.cpp b/components/operators/src/clipping.cpp
index e6380fffc..97a28296e 100644
--- a/components/operators/src/clipping.cpp
+++ b/components/operators/src/clipping.cpp
@@ -61,14 +61,17 @@ bool ClipScene::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStr
in.create(Channel::Shapes3D, shapes);
for (size_t i=0; i<in.frames.size(); ++i) {
+ if (!in.hasFrame(i)) continue;
auto &f = in.frames[i];
//auto *s = in.sources[i];
- auto pose = MatrixConversion::toCUDA(f.getPose().cast<float>());
+ if (f.hasChannel(Channel::Depth)) {
+ auto pose = MatrixConversion::toCUDA(f.getPose().cast<float>());
- auto sclip = clip;
- sclip.origin = sclip.origin.getInverse() * pose;
- if (!no_clip) ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream);
+ auto sclip = clip;
+ sclip.origin = sclip.origin.getInverse() * pose;
+ if (!no_clip) ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream);
+ }
}
return true;
diff --git a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
index 2f267b55b..1c4f7da20 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
@@ -122,6 +122,11 @@ class Builder : public Generator {
*/
ftl::rgbd::Frame &get(int64_t timestamp, size_t ix);
+ /**
+ * Get the entire frameset for a given timestamp.
+ */
+ ftl::rgbd::FrameSet *get(int64_t timestamp);
+
/**
* Mark a frame as completed.
*/
@@ -171,6 +176,7 @@ class Builder : public Generator {
/* Find a frameset with given latency in frames. */
ftl::rgbd::FrameSet *_getFrameset();
+ ftl::rgbd::FrameSet *_get(int64_t timestamp);
/* Search for a matching frameset. */
ftl::rgbd::FrameSet *_findFrameset(int64_t ts);
diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp
index 4ee8a89c7..0ed610f79 100644
--- a/components/rgbd-sources/src/frameset.cpp
+++ b/components/rgbd-sources/src/frameset.cpp
@@ -87,21 +87,15 @@ Builder::~Builder() {
}
}
-ftl::rgbd::Frame &Builder::get(int64_t timestamp, size_t ix) {
- if (timestamp <= 0 || ix >= kMaxFramesInSet) throw FTL_Error("Invalid frame timestamp or index");
+ftl::rgbd::FrameSet *Builder::get(int64_t timestamp) {
+ if (timestamp <= 0) throw FTL_Error("Invalid frame timestamp");
UNIQUE_LOCK(mutex_, lk);
- //LOG(INFO) << "BUILDER PUSH: " << timestamp << ", " << ix << ", " << size_;
-
- // Size is determined by largest frame index received... note that size
- // cannot therefore reduce.
- if (ix >= size_) {
- size_ = ix+1;
- states_.resize(size_);
- }
- //states_[ix] = frame.origin();
+ return _get(timestamp);
+}
+ftl::rgbd::FrameSet *Builder::_get(int64_t timestamp) {
if (timestamp <= last_frame_) {
throw FTL_Error("Frameset already completed: " << timestamp);
}
@@ -119,6 +113,25 @@ ftl::rgbd::Frame &Builder::get(int64_t timestamp, size_t ix) {
if (fs->test(ftl::data::FSFlag::STALE)) {
throw FTL_Error("Frameset already completed");
}
+ return fs;
+}
+
+ftl::rgbd::Frame &Builder::get(int64_t timestamp, size_t ix) {
+ if (timestamp <= 0 || ix >= kMaxFramesInSet) throw FTL_Error("Invalid frame timestamp or index");
+
+ UNIQUE_LOCK(mutex_, lk);
+
+ //LOG(INFO) << "BUILDER PUSH: " << timestamp << ", " << ix << ", " << size_;
+
+ // Size is determined by largest frame index received... note that size
+ // cannot therefore reduce.
+ if (ix >= size_) {
+ size_ = ix+1;
+ states_.resize(size_);
+ }
+ //states_[ix] = frame.origin();
+
+ auto *fs = _get(timestamp);
if (ix >= fs->frames.size()) {
throw FTL_Error("Frame index out-of-bounds");
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index 23c4ce867..7e6cacbf2 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -133,8 +133,15 @@ void Receiver::_processState(const StreamPacket &spkt, const Packet &pkt) {
void Receiver::_processData(const StreamPacket &spkt, const Packet &pkt) {
//InternalVideoStates &frame = _getVideoFrame(spkt);
- auto &frame = builder_[spkt.streamID].get(spkt.timestamp, spkt.frame_number);
- frame.createRawData(spkt.channel, pkt.data);
+ if (spkt.frameNumber() == 255) {
+ auto *fs = builder_[spkt.streamID].get(spkt.timestamp);
+ if (fs) {
+ fs->createRawData(spkt.channel, pkt.data);
+ }
+ } else {
+ auto &frame = builder_[spkt.streamID].get(spkt.timestamp, spkt.frame_number);
+ frame.createRawData(spkt.channel, pkt.data);
+ }
}
void Receiver::_processAudio(const StreamPacket &spkt, const Packet &pkt) {
@@ -354,6 +361,12 @@ void Receiver::setStream(ftl::stream::Stream *s) {
//if (spkt.frameSetID() > 0) LOG(INFO) << "Frameset " << spkt.frameSetID() << " received: " << (int)spkt.channel;
if (spkt.frameSetID() >= ftl::stream::kMaxStreams) return;
+ // Frameset level data channels
+ if (spkt.frameNumber() == 255 && pkt.data.size() > 0) {
+ _processData(spkt,pkt);
+ return;
+ }
+
// Too many frames, so ignore.
if (spkt.frameNumber() >= value("max_frames",32)) return;
diff --git a/components/streams/src/sender.cpp b/components/streams/src/sender.cpp
index 742945dad..b0a5684b6 100644
--- a/components/streams/src/sender.cpp
+++ b/components/streams/src/sender.cpp
@@ -139,6 +139,25 @@ void Sender::post(ftl::rgbd::FrameSet &fs) {
FTL_Profile("SenderPost", 0.02);
+ // Send any frameset data channels
+ for (auto c : fs.getDataChannels()) {
+ StreamPacket spkt;
+ spkt.version = 4;
+ spkt.timestamp = fs.timestamp;
+ spkt.streamID = 0; //fs.id;
+ spkt.frame_number = 255;
+ spkt.channel = c;
+
+ ftl::codecs::Packet pkt;
+ pkt.codec = ftl::codecs::codec_t::MSGPACK;
+ pkt.definition = ftl::codecs::definition_t::Any;
+ pkt.frame_count = 1;
+ pkt.flags = 0;
+ pkt.bitrate = 0;
+ pkt.data = fs.getRawData(c);
+ stream_->post(spkt, pkt);
+ }
+
for (size_t i=0; i<fs.frames.size(); ++i) {
const auto &frame = fs.frames[i];
diff --git a/components/structures/include/ftl/data/frameset.hpp b/components/structures/include/ftl/data/frameset.hpp
index 263f86ff4..de5d6f68f 100644
--- a/components/structures/include/ftl/data/frameset.hpp
+++ b/components/structures/include/ftl/data/frameset.hpp
@@ -106,6 +106,8 @@ class FrameSet {
data_channels_.clear();
}
+ ftl::codecs::Channels<2048> getDataChannels() const { return data_channels_; }
+
private:
std::unordered_map<int, std::vector<unsigned char>> data_;
ftl::codecs::Channels<2048> data_channels_;
--
GitLab