From 538ca135060e5712ad8089327e098d5702645f54 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Tue, 7 Jan 2020 10:06:38 +0200
Subject: [PATCH] Implements #272 Disparity pipeline in reconstruct
---
applications/gui/src/camera.cpp | 29 +-
applications/gui/src/gltexture.cpp | 4 +-
applications/gui/src/screen.cpp | 1 +
applications/reconstruct/src/main.cpp | 27 +-
.../reconstruct/src/reconstruction.cpp | 95 ++---
.../reconstruct/src/reconstruction.hpp | 5 +-
applications/vision/src/main.cpp | 7 +-
.../codecs/include/ftl/codecs/channels.hpp | 7 +-
.../include/ftl/codecs/opencv_decoder.hpp | 2 +-
components/codecs/src/channels.cpp | 4 +-
components/codecs/src/nvpipe_decoder.cpp | 4 +-
components/codecs/src/opencv_decoder.cpp | 13 +-
components/codecs/test/opencv_codec_unit.cpp | 4 +-
components/operators/CMakeLists.txt | 3 +
.../include/ftl/operators/colours.hpp | 1 +
.../include/ftl/operators/disparity.hpp | 22 ++
components/operators/src/colours.cpp | 36 +-
components/operators/src/depth.cpp | 76 ++++
.../src/disparity/bilateral_filter.cpp | 28 +-
components/operators/src/disparity/cuda.hpp | 3 +
.../operators/src/disparity/disp2depth.cu | 29 ++
.../src/disparity/disparity_to_depth.cpp | 5 +-
.../operators/src/disparity/fixstars_sgm.cpp | 9 +-
.../opencv/disparity_bilateral_filter.cpp | 200 +++++++++++
.../opencv/disparity_bilateral_filter.cu | 220 ++++++++++++
.../opencv/disparity_bilateral_filter.hpp | 8 +
.../src/disparity/opencv/joint_bilateral.hpp | 12 +
components/operators/src/mask.cpp | 4 +
components/operators/src/normals.cpp | 6 +-
components/operators/src/operator.cpp | 17 +-
components/operators/src/segmentation.cpp | 6 +-
.../cpp/include/ftl/render/tri_render.hpp | 3 +
components/renderers/cpp/src/tri_render.cpp | 10 +
.../include/ftl/rgbd/detail/source.hpp | 3 +-
.../rgbd-sources/include/ftl/rgbd/frame.hpp | 20 ++
.../include/ftl/rgbd/frameset.hpp | 2 +
.../rgbd-sources/include/ftl/rgbd/group.hpp | 33 +-
.../rgbd-sources/include/ftl/rgbd/source.hpp | 12 +-
.../include/ftl/rgbd/streamer.hpp | 2 +
.../rgbd-sources/include/ftl/rgbd/virtual.hpp | 2 +-
components/rgbd-sources/src/frame.cpp | 89 ++++-
components/rgbd-sources/src/frameset.cpp | 8 +
components/rgbd-sources/src/group.cpp | 326 ++++++++++++------
components/rgbd-sources/src/source.cpp | 14 +-
.../src/sources/ftlfile/file_source.cpp | 89 +++--
.../src/sources/ftlfile/file_source.hpp | 5 +-
.../src/sources/ftlfile/player.cpp | 7 +-
.../rgbd-sources/src/sources/net/net.cpp | 45 ++-
.../rgbd-sources/src/sources/net/net.hpp | 7 +-
.../sources/realsense/realsense_source.cpp | 9 +-
.../src/sources/snapshot/snapshot_source.cpp | 8 +-
.../src/sources/stereovideo/stereovideo.cpp | 32 +-
.../src/sources/virtual/virtual.cpp | 17 +-
components/rgbd-sources/src/streamer.cpp | 117 +++++--
54 files changed, 1394 insertions(+), 353 deletions(-)
create mode 100644 components/operators/src/depth.cpp
create mode 100644 components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
create mode 100644 components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
create mode 100644 components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
create mode 100644 components/operators/src/disparity/opencv/joint_bilateral.hpp
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index fb5777740..3259bfa33 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -14,6 +14,7 @@ using ftl::gui::GLTexture;
using ftl::gui::PoseWindow;
using ftl::codecs::Channel;
using ftl::codecs::Channels;
+using cv::cuda::GpuMat;
// TODO(Nick) MOVE
class StatisticsImage {
@@ -156,20 +157,22 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : scr
posewin_->setTheme(screen->windowtheme);
posewin_->setVisible(false);
- src->setCallback([this](int64_t ts, cv::cuda::GpuMat &channel1, cv::cuda::GpuMat &channel2) {
+ src->setCallback([this](int64_t ts, ftl::rgbd::Frame &frame) {
UNIQUE_LOCK(mutex_, lk);
- im1_.create(channel1.size(), channel1.type());
- im2_.create(channel2.size(), channel2.type());
-
- //cv::swap(channel1, im1_);
- //cv::swap(channel2, im2_);
+ auto &channel1 = frame.get<GpuMat>(Channel::Colour);
+ im1_.create(channel1.size(), channel1.type());
channel1.download(im1_);
- channel2.download(im2_);
-
+
// OpenGL (0,0) bottom left
cv::flip(im1_, im1_, 0);
- cv::flip(im2_, im2_, 0);
+
+ if (channel_ != Channel::Colour && channel_ != Channel::None && frame.hasChannel(channel_)) {
+ auto &channel2 = frame.get<GpuMat>(channel_);
+ im2_.create(channel2.size(), channel2.type());
+ channel2.download(im2_);
+ cv::flip(im2_, im2_, 0);
+ }
});
}
@@ -330,6 +333,7 @@ static void visualizeDepthMap( const cv::Mat &depth, cv::Mat &out,
applyColorMap(out, out, cv::COLORMAP_JET);
out.setTo(cv::Scalar(255, 255, 255), mask);
+ cv::cvtColor(out,out, cv::COLOR_BGR2BGRA);
}
static void visualizeEnergy( const cv::Mat &depth, cv::Mat &out,
@@ -343,6 +347,7 @@ static void visualizeEnergy( const cv::Mat &depth, cv::Mat &out,
applyColorMap(out, out, cv::COLORMAP_JET);
//out.setTo(cv::Scalar(255, 255, 255), mask);
+ cv::cvtColor(out,out, cv::COLOR_BGR2BGRA);
}
static void drawEdges( const cv::Mat &in, cv::Mat &out,
@@ -353,8 +358,8 @@ static void drawEdges( const cv::Mat &in, cv::Mat &out,
cv::Laplacian(in, edges, 8, ksize);
cv::threshold(edges, edges, threshold, 255, threshold_type);
- cv::Mat edges_color(in.size(), CV_8UC3);
- cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC3);
+ cv::Mat edges_color(in.size(), CV_8UC4);
+ cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC4);
}
cv::Mat ftl::gui::Camera::visualizeActiveChannel() {
@@ -497,7 +502,7 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
//case Channel::Flow:
case Channel::ColourNormals:
case Channel::Right:
- if (im2_.rows == 0 || im2_.type() != CV_8UC3) { break; }
+ if (im2_.rows == 0 || im2_.type() != CV_8UC4) { break; }
texture2_.update(im2_);
break;
diff --git a/applications/gui/src/gltexture.cpp b/applications/gui/src/gltexture.cpp
index 8b36e848f..d12c19379 100644
--- a/applications/gui/src/gltexture.cpp
+++ b/applications/gui/src/gltexture.cpp
@@ -19,7 +19,7 @@ void GLTexture::update(cv::Mat &m) {
glGenTextures(1, &glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
//cv::Mat m(cv::Size(100,100), CV_8UC3);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, m.cols, m.rows, 0, GL_BGR, GL_UNSIGNED_BYTE, m.data);
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -27,7 +27,7 @@ void GLTexture::update(cv::Mat &m) {
} else {
glBindTexture(GL_TEXTURE_2D, glid_);
// TODO Allow for other formats
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, m.cols, m.rows, 0, GL_BGR, GL_UNSIGNED_BYTE, m.data);
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
}
auto err = glGetError();
if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;
diff --git a/applications/gui/src/screen.cpp b/applications/gui/src/screen.cpp
index 77c4e4318..aa2d5e36e 100644
--- a/applications/gui/src/screen.cpp
+++ b/applications/gui/src/screen.cpp
@@ -54,6 +54,7 @@ namespace {
in vec2 uv;
void main() {
color = blendAmount * texture(image1, uv) + (1.0 - blendAmount) * texture(image2, uv);
+ color.w = 1.0f;
})";
}
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 9cb5c0819..c3dc37e32 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -101,6 +101,8 @@ static void run(ftl::Configurable *root) {
sourcecounts.push_back(configuration_size);
}
+ ftl::codecs::Channels channels;
+
// Check paths for FTL files to load.
auto paths = (*root->get<nlohmann::json>("paths"));
for (auto &x : paths.items()) {
@@ -117,8 +119,9 @@ static void run(ftl::Configurable *root) {
reader.begin();
int max_stream = 0;
- reader.read(reader.getStartTime()+100, [&max_stream](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+ reader.read(reader.getStartTime()+100, [&max_stream,&channels](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
max_stream = max(max_stream, spkt.streamID);
+ if ((int)spkt.channel < 32) channels |= spkt.channel;
});
reader.end();
@@ -135,6 +138,8 @@ static void run(ftl::Configurable *root) {
}
}
+ if (channels.empty()) channels |= Channel::Depth;
+
// Create a vector of all input RGB-Depth sources
auto sources = ftl::createArray<Source>(root, "sources", net);
@@ -191,6 +196,11 @@ static void run(ftl::Configurable *root) {
std::string id = std::to_string(cumulative);
auto reconstr = ftl::create<ftl::Reconstruction>(root, id, id);
for (size_t i=cumulative; i<cumulative+c; i++) {
+ if (channels.has(Channel::Depth)) {
+ sources[i]->setChannel(Channel::Depth);
+ } else if (channels.has(Channel::Right)) {
+ sources[i]->setChannel(Channel::Right);
+ }
reconstr->addSource(sources[i]);
}
groups.push_back(reconstr);
@@ -202,10 +212,18 @@ static void run(ftl::Configurable *root) {
renderpipe->append<ftl::operators::FXAA>("antialiasing");
vs->onRender([vs, &groups, &renderpipe](ftl::rgbd::Frame &out) {
+ bool hasFrame = false;
for (auto &reconstr : groups) {
- reconstr->render(vs, out);
+ hasFrame = reconstr->render(vs, out) || hasFrame;
+ }
+
+ if (hasFrame) {
+ renderpipe->apply(out, out, vs, 0);
+ return true;
+ } else {
+ LOG(INFO) << "NO FRAME";
+ return false;
}
- renderpipe->apply(out, out, vs, 0);
});
stream->add(vs);
@@ -313,8 +331,7 @@ static void run(ftl::Configurable *root) {
});
// -------------------------------------------------------------------------
-
- stream->setLatency(6); // FIXME: This depends on source!?
+
//stream->add(group);
stream->run();
diff --git a/applications/reconstruct/src/reconstruction.cpp b/applications/reconstruct/src/reconstruction.cpp
index 8a868e56e..621693f0e 100644
--- a/applications/reconstruct/src/reconstruction.cpp
+++ b/applications/reconstruct/src/reconstruction.cpp
@@ -9,6 +9,7 @@
#include "ftl/operators/antialiasing.hpp"
#include "ftl/operators/mvmls.hpp"
#include "ftl/operators/clipping.hpp"
+#include <ftl/operators/disparity.hpp>
using ftl::Reconstruction;
using ftl::codecs::Channel;
@@ -24,14 +25,17 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
}
Reconstruction::Reconstruction(nlohmann::json &config, const std::string name) :
- ftl::Configurable(config), busy_(false), fs_render_(), fs_align_() {
+ ftl::Configurable(config), busy_(false), rbusy_(false), new_frame_(false), fs_render_(), fs_align_() {
group_ = new ftl::rgbd::Group;
- group_->setName("ReconGroup-" + name);
+ group_->setName("Reconstruction-" + name);
group_->setLatency(4);
renderer_ = ftl::create<ftl::render::Triangular>(this, "renderer", &fs_render_);
pipeline_ = ftl::config::create<ftl::operators::Graph>(this, "pre_filters");
+ pipeline_->append<ftl::operators::DepthChannel>("depth"); // Ensure there is a depth channel
+ pipeline_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter")->set("enabled", false);
+ pipeline_->append<ftl::operators::DisparityToDepth>("calculate_depth")->set("enabled", false);
pipeline_->append<ftl::operators::ClipScene>("clipping")->set("enabled", false);
pipeline_->append<ftl::operators::ColourChannels>("colour"); // Convert BGR to BGRA
//pipeline_->append<ftl::operators::HFSmoother>("hfnoise"); // Remove high-frequency noise
@@ -49,52 +53,46 @@ Reconstruction::Reconstruction(nlohmann::json &config, const std::string name) :
group_->sync([this](ftl::rgbd::FrameSet &fs) -> bool {
// TODO: pause
- if (busy_) {
- LOG(INFO) << "Group frameset dropped: " << fs.timestamp;
+ /*if (busy_) {
+ LOG(WARNING) << "Group frameset dropped: " << fs.timestamp;
return true;
}
- busy_ = true;
+ busy_ = true;*/
// Swap the entire frameset to allow rapid return
- fs.swapTo(fs_align_);
+ // FIXME: This gets stuck on a mutex
+ //fs.swapTo(fs_align_);
- ftl::pool.push([this](int id) {
- UNIQUE_LOCK(fs_align_.mtx, lk);
-
- /*rgb_.resize(fs_align_.frames.size());
- for (size_t i = 0; i < rgb_.size(); i++) {
- auto &depth = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Depth);
- auto &color = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Colour);
-
- if (depth.size() != color.size()) {
- std::swap(rgb_[i], color);
- cv::cuda::resize(rgb_[i], color, depth.size(), 0.0, 0.0, cv::INTER_LINEAR);
- }
- }*/
-
- pipeline_->apply(fs_align_, fs_align_, 0);
+ //ftl::pool.push([this](int id) {
+ //UNIQUE_LOCK(fs.mtx, lk);
+
+ pipeline_->apply(fs, fs, 0);
// TODO: To use second GPU, could do a download, swap, device change,
// then upload to other device. Or some direct device-2-device copy.
- /*
- for (size_t i = 0; i < rgb_.size(); i++) {
- auto &depth = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Depth);
- auto &color = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Colour);
- auto &tmp = rgb_[i];
-
- // TODO doesn't always work correctly if resolution changes
- if (!tmp.empty() && (depth.size() != tmp.size())) {
- std::swap(tmp, color);
- fs_align_.frames[i].resetTexture(ftl::codecs::Channel::Colour);
- fs_align_.frames[i].createTexture<uchar4>(ftl::codecs::Channel::Colour, true);
- }
- }*/
- fs_align_.swapTo(fs_render_);
+ // FIXME: Need a buffer of framesets
+ /*if (rbusy_) {
+ LOG(WARNING) << "Render frameset dropped: " << fs_align_.timestamp;
+ busy_ = false;
+ //return;
+ }
+ rbusy_ = true;*/
+
+ //LOG(INFO) << "Align complete... " << fs.timestamp;
+
+ // TODO: Reduce mutex locking problem here...
+ {
+ UNIQUE_LOCK(exchange_mtx_, lk);
+ if (new_frame_ == true) LOG(WARNING) << "Frame lost";
+ fs.swapTo(fs_align_);
+ new_frame_ = true;
+ }
- LOG(INFO) << "Align complete... " << fs_align_.timestamp;
- busy_ = false;
- });
+ //fs.resetFull();
+
+ //busy_ = false;
+ //});
return true;
});
}
@@ -104,7 +102,7 @@ Reconstruction::~Reconstruction() {
}
void Reconstruction::addSource(ftl::rgbd::Source *src) {
- src->setChannel(Channel::Depth);
+ //src->setChannel(Channel::Depth);
group_->addSource(src); // TODO: check if source is already in group?
}
@@ -112,7 +110,20 @@ void Reconstruction::addRawCallback(const std::function<void(ftl::rgbd::Source *
group_->addRawCallback(cb);
}
-void Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out) {
+bool Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out) {
+ {
+ UNIQUE_LOCK(exchange_mtx_, lk);
+ if (new_frame_) {
+ new_frame_ = false;
+ fs_align_.swapTo(fs_render_);
+ }
+ }
+ /*if (fs_render_.stale || fs_render_.timestamp <= 0) {
+ LOG(ERROR) << "STALE FRAME TO RENDER";
+ return false;
+ }
+ fs_render_.stale = true;*/
+
// Create scene transform, intended for axis aligning the walls and floor
Eigen::Matrix4d transform;
//if (getConfig()["transform"].is_object()) {
@@ -134,5 +145,7 @@ void Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out)
//}
Eigen::Affine3d sm = Eigen::Affine3d(Eigen::Scaling(double(value("scale", 1.0f))));
- renderer_->render(vs, out, sm.matrix() * transform);
+ bool res = renderer_->render(vs, out, sm.matrix() * transform);
+ //fs_render_.resetFull();
+ return res;
}
\ No newline at end of file
diff --git a/applications/reconstruct/src/reconstruction.hpp b/applications/reconstruct/src/reconstruction.hpp
index 50441bedc..cb3bb8ebd 100644
--- a/applications/reconstruct/src/reconstruction.hpp
+++ b/applications/reconstruct/src/reconstruction.hpp
@@ -23,10 +23,13 @@ class Reconstruction : public ftl::Configurable {
/**
* Do the render for a specified virtual camera.
*/
- void render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out);
+ bool render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out);
private:
bool busy_;
+ bool rbusy_;
+ bool new_frame_;
+ MUTEX exchange_mtx_;
ftl::rgbd::FrameSet fs_render_;
ftl::rgbd::FrameSet fs_align_;
diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index 4f245a077..058d3f989 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -24,6 +24,7 @@
#include <ftl/net/universe.hpp>
#include <ftl/master.hpp>
#include <nlohmann/json.hpp>
+#include <ftl/operators/disparity.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
@@ -54,7 +55,7 @@ static void run(ftl::Configurable *root) {
auto paths = root->get<vector<string>>("paths");
string file = "";
- if (paths && (*paths).size() > 0) file = (*paths)[0];
+ if (paths && (*paths).size() > 0) file = (*paths)[(*paths).size()-1];
Source *source = nullptr;
source = ftl::create<Source>(root, "source");
@@ -65,6 +66,10 @@ static void run(ftl::Configurable *root) {
Streamer *stream = ftl::create<Streamer>(root, "stream", net);
stream->add(source);
+
+ auto pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
+ pipeline->append<ftl::operators::DepthChannel>("depth"); // Ensure there is a depth channel
+ stream->group()->addPipeline(pipeline);
net->start();
diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp
index 9aa214302..dca2eef76 100644
--- a/components/codecs/include/ftl/codecs/channels.hpp
+++ b/components/codecs/include/ftl/codecs/channels.hpp
@@ -14,7 +14,6 @@ enum struct Channel : int {
Depth = 1, // 32S or 32F
Right = 2, // 8UC3 or 8UC4
Colour2 = 2,
- Disparity = 3,
Depth2 = 3,
Deviation = 4,
Screen = 4,
@@ -24,7 +23,6 @@ enum struct Channel : int {
Contribution = 7, // 32F
EnergyVector = 8, // 32FC4
Flow = 9, // 32F
- Smoothing = 9, // 32F
Energy = 10, // 32F
Mask = 11, // 32U
Density = 12, // 32F
@@ -32,7 +30,9 @@ enum struct Channel : int {
Support2 = 14, // 8UC4 (currently)
Segmentation = 15, // 32S?
ColourNormals = 16, // 8UC4
- ColourHighRes = 20, // 8UC3 or 8UC4
+ ColourHighRes = 17, // 8UC3 or 8UC4
+ Disparity = 18,
+ Smoothing = 19, // 32F
AudioLeft = 32,
AudioRight = 33,
@@ -102,6 +102,7 @@ class Channels {
}
inline size_t count() { return std::bitset<32>(mask).count(); }
+ inline bool empty() { return mask == 0; }
inline void clear() { mask = 0; }
static const size_t kMax = 32;
diff --git a/components/codecs/include/ftl/codecs/opencv_decoder.hpp b/components/codecs/include/ftl/codecs/opencv_decoder.hpp
index 53b61f683..0f96cc7ee 100644
--- a/components/codecs/include/ftl/codecs/opencv_decoder.hpp
+++ b/components/codecs/include/ftl/codecs/opencv_decoder.hpp
@@ -16,7 +16,7 @@ class OpenCVDecoder : public ftl::codecs::Decoder {
bool accepts(const ftl::codecs::Packet &pkt);
private:
- cv::Mat tmp_;
+ //cv::Mat tmp_;
};
}
diff --git a/components/codecs/src/channels.cpp b/components/codecs/src/channels.cpp
index effaf9eb2..b33427b70 100644
--- a/components/codecs/src/channels.cpp
+++ b/components/codecs/src/channels.cpp
@@ -8,9 +8,9 @@ struct ChannelInfo {
};
static ChannelInfo info[] = {
- "Colour", CV_8UC3,
+ "Colour", CV_8UC4,
"Depth", CV_32F,
- "Right", CV_8UC3,
+ "Right", CV_8UC4,
"DepthRight", CV_32F,
"Deviation", CV_32F,
"Normals", CV_32FC4,
diff --git a/components/codecs/src/nvpipe_decoder.cpp b/components/codecs/src/nvpipe_decoder.cpp
index d6652549c..844029495 100644
--- a/components/codecs/src/nvpipe_decoder.cpp
+++ b/components/codecs/src/nvpipe_decoder.cpp
@@ -94,9 +94,9 @@ bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
//if (out.rows == ftl::codecs::getHeight(pkt.definition)) {
// Flag 0x1 means frame is in RGB so needs conversion to BGR
if (pkt.flags & 0x1) {
- cv::cuda::cvtColor(tmp_, out, cv::COLOR_RGBA2BGR, 0, stream_);
+ cv::cuda::cvtColor(tmp_, out, cv::COLOR_RGBA2BGRA, 0, stream_);
} else {
- cv::cuda::cvtColor(tmp_, out, cv::COLOR_BGRA2BGR, 0, stream_);
+ //cv::cuda::cvtColor(tmp_, out, cv::COLOR_BGRA2BGR, 0, stream_);
}
/*} else {
LOG(WARNING) << "Resizing decoded frame from " << tmp_.size() << " to " << out.size();
diff --git a/components/codecs/src/opencv_decoder.cpp b/components/codecs/src/opencv_decoder.cpp
index c3c5e9567..f721b462a 100644
--- a/components/codecs/src/opencv_decoder.cpp
+++ b/components/codecs/src/opencv_decoder.cpp
@@ -31,8 +31,15 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
//LOG(INFO) << "DECODE JPEG " << (int)pkt.block_number << "/" << chunk_dim;
+ cv::Mat tmp2_, tmp_;
// Decode in temporary buffers to prevent long locks
- cv::imdecode(pkt.data, cv::IMREAD_UNCHANGED, &tmp_);
+ cv::imdecode(pkt.data, cv::IMREAD_UNCHANGED, &tmp2_);
+
+ if (tmp2_.type() == CV_8UC3) {
+ cv::cvtColor(tmp2_, tmp_, cv::COLOR_BGR2BGRA);
+ } else {
+ tmp_ = tmp2_;
+ }
// Apply colour correction to chunk
//ftl::rgbd::colourCorrection(tmp_rgb, gamma_, temperature_);
@@ -45,7 +52,7 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
if (!tmp_.empty() && tmp_.type() == CV_16U && chunkHead.type() == CV_32F) {
tmp_.convertTo(tmp_, CV_32FC1, 1.0f/1000.0f);
chunkHead.upload(tmp_);
- } else if (!tmp_.empty() && tmp_.type() == CV_8UC3 && chunkHead.type() == CV_8UC3) {
+ } else if (!tmp_.empty() && tmp_.type() == CV_8UC4 && chunkHead.type() == CV_8UC4) {
//tmp_.copyTo(chunkHead);
chunkHead.upload(tmp_);
} else {
@@ -57,7 +64,7 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
tmp_.convertTo(tmp_, CV_32FC1, 1.0f/1000.0f); //(16.0f*10.0f));
cv::resize(tmp_, tmp_, chunkHead.size(), 0, 0, cv::INTER_NEAREST);
chunkHead.upload(tmp_);
- } else if (!tmp_.empty() && tmp_.type() == CV_8UC3 && chunkHead.type() == CV_8UC3) {
+ } else if (!tmp_.empty() && tmp_.type() == CV_8UC4 && chunkHead.type() == CV_8UC4) {
cv::resize(tmp_, tmp_, chunkHead.size());
chunkHead.upload(tmp_);
} else {
diff --git a/components/codecs/test/opencv_codec_unit.cpp b/components/codecs/test/opencv_codec_unit.cpp
index 961658db5..0982551bd 100644
--- a/components/codecs/test/opencv_codec_unit.cpp
+++ b/components/codecs/test/opencv_codec_unit.cpp
@@ -84,8 +84,8 @@ TEST_CASE( "OpenCVEncoder::encode() - A depth test image at preset 0" ) {
TEST_CASE( "OpenCVDecoder::decode() - A colour test image no resolution change" ) {
ftl::codecs::OpenCVEncoder encoder(definition_t::HD1080, definition_t::SD480);
ftl::codecs::OpenCVDecoder decoder;
- cv::cuda::GpuMat in(cv::Size(1024,576), CV_8UC3, cv::Scalar(255,0,0));
- cv::cuda::GpuMat out(cv::Size(1024,576), CV_8UC3, cv::Scalar(0,0,0));
+ cv::cuda::GpuMat in(cv::Size(1024,576), CV_8UC4, cv::Scalar(255,0,0,0));
+ cv::cuda::GpuMat out(cv::Size(1024,576), CV_8UC4, cv::Scalar(0,0,0,0));
std::mutex mtx;
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index b025a39b3..2a55cce0d 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -11,6 +11,8 @@ set(OPERSRC
src/disparity/disp2depth.cu
src/disparity/disparity_to_depth.cpp
src/disparity/bilateral_filter.cpp
+ src/disparity/opencv/disparity_bilateral_filter.cpp
+ src/disparity/opencv/disparity_bilateral_filter.cu
src/segmentation.cu
src/segmentation.cpp
src/mask.cu
@@ -20,6 +22,7 @@ set(OPERSRC
src/mvmls.cpp
src/correspondence.cu
src/clipping.cpp
+ src/depth.cpp
)
diff --git a/components/operators/include/ftl/operators/colours.hpp b/components/operators/include/ftl/operators/colours.hpp
index afad434d4..5f8ba6cd1 100644
--- a/components/operators/include/ftl/operators/colours.hpp
+++ b/components/operators/include/ftl/operators/colours.hpp
@@ -17,6 +17,7 @@ class ColourChannels : public ftl::operators::Operator {
private:
cv::cuda::GpuMat temp_;
+ cv::cuda::GpuMat rbuf_;
};
}
diff --git a/components/operators/include/ftl/operators/disparity.hpp b/components/operators/include/ftl/operators/disparity.hpp
index 0c26c904f..e4ff99d78 100644
--- a/components/operators/include/ftl/operators/disparity.hpp
+++ b/components/operators/include/ftl/operators/disparity.hpp
@@ -77,6 +77,28 @@ class DisparityToDepth : public ftl::operators::Operator {
bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
};
+/**
+ * Create a missing depth channel using Left and Right colour with a complete
+ * disparity pipeline. This is a wrapper operator that combines all of the
+ * disparity to depth steps.
+ */
+class DepthChannel : public ftl::operators::Operator {
+ public:
+ explicit DepthChannel(ftl::Configurable *cfg);
+ ~DepthChannel();
+
+ inline Operator::Type type() const override { return Operator::Type::ManyToMany; }
+
+ bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
+
+ private:
+ ftl::operators::Graph *pipe_;
+ std::vector<cv::cuda::GpuMat> rbuf_;
+ cv::Size depth_size_;
+
+ void _createPipeline();
+};
+
/*
* Optical flow smoothing for depth
*/
diff --git a/components/operators/src/colours.cpp b/components/operators/src/colours.cpp
index 6a49f6ede..955bfed58 100644
--- a/components/operators/src/colours.cpp
+++ b/components/operators/src/colours.cpp
@@ -25,15 +25,39 @@ bool ColourChannels::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgb
cv::cuda::cvtColor(temp_,col, cv::COLOR_BGR2BGRA, 0, cvstream);
}
+ if (in.hasChannel(Channel::Right)) {
+ auto &col = in.get<cv::cuda::GpuMat>(Channel::Right);
+
+ // Convert colour from BGR to BGRA if needed
+ if (col.type() == CV_8UC3) {
+ //cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+ // Convert to 4 channel colour
+ temp_.create(col.size(), CV_8UC4);
+ cv::cuda::swap(col, temp_);
+ cv::cuda::cvtColor(temp_,col, cv::COLOR_BGR2BGRA, 0, cvstream);
+ }
+ }
+
//in.resetTexture(Channel::Colour);
in.createTexture<uchar4>(Channel::Colour, true);
- auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
- if (depth.size() != col.size()) {
- auto &col2 = in.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
- cv::cuda::resize(col, col2, depth.size(), 0.0, 0.0, cv::INTER_LINEAR, cvstream);
- in.createTexture<uchar4>(Channel::ColourHighRes, true);
- in.swapChannels(Channel::Colour, Channel::ColourHighRes);
+ if (in.hasChannel(Channel::Depth)) {
+ auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
+ if (depth.size() != col.size()) {
+ auto &col2 = in.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
+ cv::cuda::resize(col, col2, depth.size(), 0.0, 0.0, cv::INTER_LINEAR, cvstream);
+ in.createTexture<uchar4>(Channel::ColourHighRes, true);
+ in.swapChannels(Channel::Colour, Channel::ColourHighRes);
+ }
+
+ // Ensure right channel is also downsized
+ if (in.hasChannel(Channel::Right)) {
+ auto &right = in.get<cv::cuda::GpuMat>(Channel::Right);
+ if (depth.size() != right.size()) {
+ cv::cuda::resize(right, rbuf_, depth.size(), 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+ cv::cuda::swap(right, rbuf_);
+ }
+ }
}
return true;
diff --git a/components/operators/src/depth.cpp b/components/operators/src/depth.cpp
new file mode 100644
index 000000000..3d332d6ae
--- /dev/null
+++ b/components/operators/src/depth.cpp
@@ -0,0 +1,76 @@
+#include <ftl/operators/disparity.hpp>
+
+#include "ftl/operators/smoothing.hpp"
+#include "ftl/operators/colours.hpp"
+#include "ftl/operators/normals.hpp"
+#include "ftl/operators/filling.hpp"
+#include "ftl/operators/segmentation.hpp"
+#include "ftl/operators/disparity.hpp"
+#include "ftl/operators/mask.hpp"
+
+using ftl::operators::DepthChannel;
+using ftl::codecs::Channel;
+
+DepthChannel::DepthChannel(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+ pipe_ = nullptr;
+}
+
+DepthChannel::~DepthChannel() {
+
+}
+
+void DepthChannel::_createPipeline() {
+ if (pipe_ != nullptr) return;
+
+ pipe_ = ftl::config::create<ftl::operators::Graph>(config(), "depth");
+ depth_size_ = cv::Size( pipe_->value("width", 1280),
+ pipe_->value("height", 720));
+
+ pipe_->append<ftl::operators::ColourChannels>("colour"); // Convert BGR to BGRA
+ pipe_->append<ftl::operators::FixstarsSGM>("algorithm");
+ #ifdef HAVE_OPTFLOW
+ pipe_->append<ftl::operators::OpticalFlowTemporalSmoothing>("optflow_filter");
+ #endif
+ pipe_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter");
+ pipe_->append<ftl::operators::DisparityToDepth>("calculate_depth");
+ pipe_->append<ftl::operators::Normals>("normals"); // Estimate surface normals
+ pipe_->append<ftl::operators::CrossSupport>("cross");
+ pipe_->append<ftl::operators::DiscontinuityMask>("discontinuity_mask");
+ pipe_->append<ftl::operators::AggreMLS>("mls"); // Perform MLS (using smoothing channel)
+}
+
+bool DepthChannel::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
+ auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+
+ rbuf_.resize(in.frames.size());
+
+ for (int i=0; i<in.frames.size(); ++i) {
+ auto &f = in.frames[i];
+ if (!f.hasChannel(Channel::Depth) && f.hasChannel(Channel::Right)) {
+ _createPipeline();
+
+ cv::cuda::GpuMat& left = f.get<cv::cuda::GpuMat>(Channel::Left);
+ cv::cuda::GpuMat& right = f.get<cv::cuda::GpuMat>(Channel::Right);
+ cv::cuda::GpuMat& depth = f.create<cv::cuda::GpuMat>(Channel::Depth);
+ depth.create(left.size(), CV_32FC1);
+
+ if (left.empty() || right.empty()) continue;
+
+ /*if (depth_size_ != left.size()) {
+ auto &col2 = f.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
+ cv::cuda::resize(left, col2, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+ f.createTexture<uchar4>(Channel::ColourHighRes, true);
+ f.swapChannels(Channel::Colour, Channel::ColourHighRes);
+ }
+
+ if (depth_size_ != right.size()) {
+ cv::cuda::resize(right, rbuf_[i], depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+ cv::cuda::swap(right, rbuf_[i]);
+ }*/
+
+ pipe_->apply(f, f, in.sources[i], stream);
+ }
+ }
+
+ return true;
+}
diff --git a/components/operators/src/disparity/bilateral_filter.cpp b/components/operators/src/disparity/bilateral_filter.cpp
index 8c91fac6f..13f2ed1ea 100644
--- a/components/operators/src/disparity/bilateral_filter.cpp
+++ b/components/operators/src/disparity/bilateral_filter.cpp
@@ -1,5 +1,8 @@
#include <ftl/operators/disparity.hpp>
+#include "opencv/joint_bilateral.hpp"
+#include "cuda.hpp"
+
using cv::cuda::GpuMat;
using cv::Size;
@@ -13,15 +16,36 @@ DisparityBilateralFilter::DisparityBilateralFilter(ftl::Configurable* cfg) :
n_disp_ = cfg->value("n_disp", 256);
radius_ = cfg->value("radius", 7);
iter_ = cfg->value("iter", 13);
- filter_ = cv::cuda::createDisparityBilateralFilter(n_disp_ * scale_, radius_, iter_);
+ filter_ = nullptr;
}
bool DisparityBilateralFilter::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
ftl::rgbd::Source *src, cudaStream_t stream) {
- if (!in.hasChannel(Channel::Disparity) || !in.hasChannel(Channel::Colour)) {
+ if (!in.hasChannel(Channel::Colour)) {
+ LOG(ERROR) << "Joint Bilateral Filter is missing Colour";
return false;
+ } else if (!in.hasChannel(Channel::Disparity)) {
+ // Have depth, so calculate disparity...
+ if (in.hasChannel(Channel::Depth)) {
+ // No disparity, so create it.
+ const auto params = src->parameters();
+ const GpuMat &depth = in.get<GpuMat>(Channel::Depth);
+
+ GpuMat &disp = out.create<GpuMat>(Channel::Disparity);
+ disp.create(depth.size(), CV_32FC1);
+
+ LOG(ERROR) << "Calculated disparity from depth";
+
+ ftl::cuda::depth_to_disparity(disp, depth, params, stream);
+ } else {
+ LOG(ERROR) << "Joint Bilateral Filter is missing Disparity and Depth";
+ return false;
+ }
}
+
+ if (!filter_) filter_ = ftl::cuda::createDisparityBilateralFilter(n_disp_ * scale_, radius_, iter_);
+
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
const GpuMat &rgb = in.get<GpuMat>(Channel::Colour);
GpuMat &disp_in = in.get<GpuMat>(Channel::Disparity);
diff --git a/components/operators/src/disparity/cuda.hpp b/components/operators/src/disparity/cuda.hpp
index d4cf06aae..48c8ffd1b 100644
--- a/components/operators/src/disparity/cuda.hpp
+++ b/components/operators/src/disparity/cuda.hpp
@@ -6,6 +6,9 @@ namespace cuda {
void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
const ftl::rgbd::Camera &c, cudaStream_t &stream);
+void depth_to_disparity(cv::cuda::GpuMat &disparity, const cv::cuda::GpuMat &depth,
+ const ftl::rgbd::Camera &c, cudaStream_t &stream);
+
void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
cv::cuda::GpuMat &history, int n_max, float threshold,
diff --git a/components/operators/src/disparity/disp2depth.cu b/components/operators/src/disparity/disp2depth.cu
index 86a3fc290..a2c60def9 100644
--- a/components/operators/src/disparity/disp2depth.cu
+++ b/components/operators/src/disparity/disp2depth.cu
@@ -27,3 +27,32 @@ namespace cuda {
}
}
}
+
+//==============================================================================
+
+__global__ void d2drev_kernel(cv::cuda::PtrStepSz<float> disp, cv::cuda::PtrStepSz<float> depth,
+ ftl::rgbd::Camera cam) {
+
+for (STRIDE_Y(v,disp.rows)) {
+for (STRIDE_X(u,disp.cols)) {
+ float d = depth(v,u);
+ float disparity = (d > cam.maxDepth || d < cam.minDepth) ? 0.0f : ((cam.baseline*cam.fx) / d) + cam.doffs;
+ disp(v,u) = disparity;
+}
+}
+}
+
+namespace ftl {
+namespace cuda {
+void depth_to_disparity(cv::cuda::GpuMat &disparity, const cv::cuda::GpuMat &depth,
+ const ftl::rgbd::Camera &c, cudaStream_t &stream) {
+ dim3 grid(1,1,1);
+ dim3 threads(128, 1, 1);
+ grid.x = cv::cuda::device::divUp(disparity.cols, 128);
+ grid.y = cv::cuda::device::divUp(disparity.rows, 1);
+ d2drev_kernel<<<grid, threads, 0, stream>>>(
+ disparity, depth, c);
+ cudaSafeCall( cudaGetLastError() );
+}
+}
+}
diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp
index 5951802f0..d3cc33772 100644
--- a/components/operators/src/disparity/disparity_to_depth.cpp
+++ b/components/operators/src/disparity/disparity_to_depth.cpp
@@ -9,7 +9,10 @@ using cv::cuda::GpuMat;
bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
ftl::rgbd::Source *src, cudaStream_t stream) {
- if (!in.hasChannel(Channel::Disparity)) { return false; }
+ if (!in.hasChannel(Channel::Disparity)) {
+ LOG(ERROR) << "Missing disparity before convert to depth";
+ return false;
+ }
const auto params = src->parameters();
const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);
diff --git a/components/operators/src/disparity/fixstars_sgm.cpp b/components/operators/src/disparity/fixstars_sgm.cpp
index 79a18685e..ae15033a2 100644
--- a/components/operators/src/disparity/fixstars_sgm.cpp
+++ b/components/operators/src/disparity/fixstars_sgm.cpp
@@ -89,6 +89,8 @@ bool FixstarsSGM::init() {
rbw_.create(size_, CV_8UC1);
disp_int_.create(size_, CV_16SC1);
+ LOG(INFO) << "INIT FIXSTARS";
+
ssgm_ = new sgm::StereoSGM(size_.width, size_.height, max_disp_, 8, 16,
lbw_.step, disp_int_.step / sizeof(short),
sgm::EXECUTE_INOUT_CUDA2CUDA,
@@ -106,12 +108,13 @@ bool FixstarsSGM::updateParameters() {
bool FixstarsSGM::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
if (!in.hasChannel(Channel::Left) || !in.hasChannel(Channel::Right)) {
+ LOG(ERROR) << "Fixstars is missing Left or Right channel";
return false;
}
const auto &l = in.get<GpuMat>(Channel::Left);
const auto &r = in.get<GpuMat>(Channel::Right);
-
+
if (l.size() != size_) {
size_ = l.size();
if (!init()) { return false; }
@@ -120,8 +123,8 @@ bool FixstarsSGM::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream)
auto &disp = out.create<GpuMat>(Channel::Disparity, Format<float>(l.size()));
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
- cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, cvstream);
- cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, cvstream);
+ cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGRA2GRAY, 0, cvstream);
+ cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGRA2GRAY, 0, cvstream);
cvstream.waitForCompletion();
ssgm_->execute(lbw_.data, rbw_.data, disp_int_.data);
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
new file mode 100644
index 000000000..447982808
--- /dev/null
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
@@ -0,0 +1,200 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include <ftl/cuda_common.hpp>
+#include <opencv2/cudastereo.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
+#include "joint_bilateral.hpp"
+
+using namespace cv;
+using namespace cv::cuda;
+
+#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)
+
+Ptr<cuda::DisparityBilateralFilter> ftl::cuda::createDisparityBilateralFilter(int, int, int) { throw_no_cuda(); return Ptr<cuda::DisparityBilateralFilter>(); }
+
+#else /* !defined (HAVE_CUDA) */
+
+#include "disparity_bilateral_filter.hpp"
+
+namespace
+{
+ class DispBilateralFilterImpl : public cv::cuda::DisparityBilateralFilter
+ {
+ public:
+ DispBilateralFilterImpl(int ndisp, int radius, int iters);
+
+ void apply(InputArray disparity, InputArray image, OutputArray dst, Stream& stream);
+
+ int getNumDisparities() const { return ndisp_; }
+ void setNumDisparities(int numDisparities) { ndisp_ = numDisparities; }
+
+ int getRadius() const { return radius_; }
+ void setRadius(int radius);
+
+ int getNumIters() const { return iters_; }
+ void setNumIters(int iters) { iters_ = iters; }
+
+ double getEdgeThreshold() const { return edge_threshold_; }
+ void setEdgeThreshold(double edge_threshold) { edge_threshold_ = (float) edge_threshold; }
+
+ double getMaxDiscThreshold() const { return max_disc_threshold_; }
+ void setMaxDiscThreshold(double max_disc_threshold) { max_disc_threshold_ = (float) max_disc_threshold; }
+
+ double getSigmaRange() const { return sigma_range_; }
+ void setSigmaRange(double sigma_range);
+
+ private:
+ int ndisp_;
+ int radius_;
+ int iters_;
+ float edge_threshold_;
+ float max_disc_threshold_;
+ float sigma_range_;
+
+ GpuMat table_color_;
+ GpuMat table_space_;
+ };
+
+ void calc_color_weighted_table(GpuMat& table_color, float sigma_range, int len)
+ {
+ Mat cpu_table_color(1, len, CV_32F);
+
+ float* line = cpu_table_color.ptr<float>();
+
+ for(int i = 0; i < len; i++)
+ line[i] = static_cast<float>(std::exp(-double(i * i) / (2 * sigma_range * sigma_range)));
+
+ table_color.upload(cpu_table_color);
+ }
+
+ void calc_space_weighted_filter(GpuMat& table_space, int win_size, float dist_space)
+ {
+ int half = (win_size >> 1);
+
+ Mat cpu_table_space(half + 1, half + 1, CV_32F);
+
+ for (int y = 0; y <= half; ++y)
+ {
+ float* row = cpu_table_space.ptr<float>(y);
+ for (int x = 0; x <= half; ++x)
+ row[x] = exp(-sqrt(float(y * y) + float(x * x)) / dist_space);
+ }
+
+ table_space.upload(cpu_table_space);
+ }
+
+ const float DEFAULT_EDGE_THRESHOLD = 0.1f;
+ const float DEFAULT_MAX_DISC_THRESHOLD = 0.2f;
+ const float DEFAULT_SIGMA_RANGE = 10.0f;
+
+ DispBilateralFilterImpl::DispBilateralFilterImpl(int ndisp, int radius, int iters) :
+ ndisp_(ndisp), radius_(radius), iters_(iters),
+ edge_threshold_(DEFAULT_EDGE_THRESHOLD), max_disc_threshold_(DEFAULT_MAX_DISC_THRESHOLD),
+ sigma_range_(DEFAULT_SIGMA_RANGE)
+ {
+ calc_color_weighted_table(table_color_, sigma_range_, 255);
+ calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+ }
+
+ void DispBilateralFilterImpl::setRadius(int radius)
+ {
+ radius_ = radius;
+ calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+ }
+
+ void DispBilateralFilterImpl::setSigmaRange(double sigma_range)
+ {
+ sigma_range_ = (float) sigma_range;
+ calc_color_weighted_table(table_color_, sigma_range_, 255);
+ }
+
+ template <typename T>
+ void disp_bilateral_filter_operator(int ndisp, int radius, int iters, float edge_threshold, float max_disc_threshold,
+ GpuMat& table_color, GpuMat& table_space,
+ const GpuMat& disp, const GpuMat& img,
+ OutputArray _dst, Stream& stream)
+ {
+ using namespace ftl::cuda::device::disp_bilateral_filter;
+
+ const short edge_disc = std::max<short>(short(1), short(ndisp * edge_threshold + 0.5));
+ const short max_disc = short(ndisp * max_disc_threshold + 0.5);
+
+ size_t table_space_step = table_space.step / sizeof(float);
+
+ _dst.create(disp.size(), disp.type());
+ GpuMat dst = _dst.getGpuMat();
+
+ if (dst.data != disp.data)
+ disp.copyTo(dst, stream);
+
+ disp_bilateral_filter<T>(dst, img, img.channels(), iters, table_color.ptr<float>(), (float *)table_space.data, table_space_step, radius, edge_disc, max_disc, StreamAccessor::getStream(stream));
+ }
+
+ void DispBilateralFilterImpl::apply(InputArray _disp, InputArray _image, OutputArray dst, Stream& stream)
+ {
+ typedef void (*bilateral_filter_operator_t)(int ndisp, int radius, int iters, float edge_threshold, float max_disc_threshold,
+ GpuMat& table_color, GpuMat& table_space,
+ const GpuMat& disp, const GpuMat& img, OutputArray dst, Stream& stream);
+ const bilateral_filter_operator_t operators[] =
+ {disp_bilateral_filter_operator<unsigned char>, 0, 0, disp_bilateral_filter_operator<short>, 0, 0, 0, 0};
+
+ CV_Assert( 0 < ndisp_ && 0 < radius_ && 0 < iters_ );
+
+ GpuMat disp = _disp.getGpuMat();
+ GpuMat img = _image.getGpuMat();
+
+ CV_Assert( disp.type() == CV_8U || disp.type() == CV_16S );
+ CV_Assert( img.type() == CV_8UC1 || img.type() == CV_8UC3 || img.type() == CV_8UC4 );
+ CV_Assert( disp.size() == img.size() );
+
+ operators[disp.type()](ndisp_, radius_, iters_, edge_threshold_, max_disc_threshold_,
+ table_color_, table_space_, disp, img, dst, stream);
+ }
+}
+
+Ptr<cuda::DisparityBilateralFilter> ftl::cuda::createDisparityBilateralFilter(int ndisp, int radius, int iters)
+{
+ return makePtr<DispBilateralFilterImpl>(ndisp, radius, iters);
+}
+
+#endif /* !defined (HAVE_CUDA) */
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
new file mode 100644
index 000000000..134095101
--- /dev/null
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
@@ -0,0 +1,220 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of the copyright holders may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#if !defined CUDA_DISABLER
+
+#include "opencv2/core/cuda/common.hpp"
+#include "opencv2/core/cuda/limits.hpp"
+
+#include "disparity_bilateral_filter.hpp"
+
+using namespace cv::cuda::device;
+using namespace cv::cuda;
+using namespace cv;
+
+namespace ftl { namespace cuda { namespace device
+{
+ namespace disp_bilateral_filter
+ {
+ template <int channels>
+ struct DistRgbMax
+ {
+ static __device__ __forceinline__ uchar calc(const uchar* a, const uchar* b)
+ {
+ // TODO: (Nick) Is this the best way to read for performance?
+ uchar x = ::abs(a[0] - b[0]);
+ uchar y = ::abs(a[1] - b[1]);
+ uchar z = ::abs(a[2] - b[2]);
+ return (::max(::max(x, y), z));
+ }
+ };
+
+ template <>
+ struct DistRgbMax<1>
+ {
+ static __device__ __forceinline__ uchar calc(const uchar* a, const uchar* b)
+ {
+ return ::abs(a[0] - b[0]);
+ }
+ };
+
+ template <int channels, typename T>
+ __global__ void disp_bilateral_filter(int t, T* disp, size_t disp_step,
+ const uchar* img, size_t img_step, int h, int w,
+ const float* ctable_color, const float * ctable_space, size_t ctable_space_step,
+ int cradius,
+ short cedge_disc, short cmax_disc)
+ {
+ const int y = blockIdx.y * blockDim.y + threadIdx.y;
+ const int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + t) & 1);
+
+ T dp[5];
+
+ if (y > 0 && y < h - 1 && x > 0 && x < w - 1)
+ {
+ dp[0] = *(disp + (y ) * disp_step + x + 0);
+ dp[1] = *(disp + (y-1) * disp_step + x + 0);
+ dp[2] = *(disp + (y ) * disp_step + x - 1);
+ dp[3] = *(disp + (y+1) * disp_step + x + 0);
+ dp[4] = *(disp + (y ) * disp_step + x + 1);
+
+ if(::abs(dp[1] - dp[0]) >= cedge_disc || ::abs(dp[2] - dp[0]) >= cedge_disc || ::abs(dp[3] - dp[0]) >= cedge_disc || ::abs(dp[4] - dp[0]) >= cedge_disc)
+ {
+ const int ymin = ::max(0, y - cradius);
+ const int xmin = ::max(0, x - cradius);
+ const int ymax = ::min(h - 1, y + cradius);
+ const int xmax = ::min(w - 1, x + cradius);
+
+ float cost[] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+ const uchar* ic = img + y * img_step + channels * x;
+
+ for(int yi = ymin; yi <= ymax; yi++)
+ {
+ const T* disp_y = disp + yi * disp_step;
+
+ for(int xi = xmin; xi <= xmax; xi++)
+ {
+ const uchar* in = img + yi * img_step + channels * xi;
+
+ uchar dist_rgb = DistRgbMax<channels>::calc(in, ic);
+
+ const float weight = ctable_color[dist_rgb] * (ctable_space + ::abs(y-yi)* ctable_space_step)[::abs(x-xi)];
+
+ const T disp_reg = disp_y[xi];
+
+ cost[0] += ::min(cmax_disc, ::abs(disp_reg - dp[0])) * weight;
+ cost[1] += ::min(cmax_disc, ::abs(disp_reg - dp[1])) * weight;
+ cost[2] += ::min(cmax_disc, ::abs(disp_reg - dp[2])) * weight;
+ cost[3] += ::min(cmax_disc, ::abs(disp_reg - dp[3])) * weight;
+ cost[4] += ::min(cmax_disc, ::abs(disp_reg - dp[4])) * weight;
+ }
+ }
+
+ float minimum = numeric_limits<float>::max();
+ int id = 0;
+
+ if (cost[0] < minimum)
+ {
+ minimum = cost[0];
+ id = 0;
+ }
+ if (cost[1] < minimum)
+ {
+ minimum = cost[1];
+ id = 1;
+ }
+ if (cost[2] < minimum)
+ {
+ minimum = cost[2];
+ id = 2;
+ }
+ if (cost[3] < minimum)
+ {
+ minimum = cost[3];
+ id = 3;
+ }
+ if (cost[4] < minimum)
+ {
+ minimum = cost[4];
+ id = 4;
+ }
+
+ *(disp + y * disp_step + x) = dp[id];
+ }
+ }
+ }
+
+ template <typename T>
+ void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float* table_space, size_t table_step, int radius, short edge_disc, short max_disc, cudaStream_t stream)
+ {
+ dim3 threads(32, 8, 1);
+ dim3 grid(1, 1, 1);
+ grid.x = divUp(disp.cols, threads.x << 1);
+ grid.y = divUp(disp.rows, threads.y);
+
+ switch (channels)
+ {
+ case 1:
+ for (int i = 0; i < iters; ++i)
+ {
+ disp_bilateral_filter<1><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+
+ disp_bilateral_filter<1><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+ }
+ break;
+ case 3:
+ for (int i = 0; i < iters; ++i)
+ {
+ disp_bilateral_filter<3><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+
+ disp_bilateral_filter<3><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+ }
+ break;
+ case 4: // Nick: Support 4 channel
+ for (int i = 0; i < iters; ++i)
+ {
+ disp_bilateral_filter<4><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+
+ disp_bilateral_filter<4><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+ cudaSafeCall( cudaGetLastError() );
+ }
+ break;
+ default:
+ CV_Error(cv::Error::BadNumChannels, "Unsupported channels count");
+ }
+
+ if (stream == 0)
+ cudaSafeCall( cudaDeviceSynchronize() );
+ }
+
+ template void disp_bilateral_filter<uchar>(cv::cuda::PtrStepSz<uchar> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, short, short, cudaStream_t stream);
+ template void disp_bilateral_filter<short>(cv::cuda::PtrStepSz<short> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, short, short, cudaStream_t stream);
+ } // namespace bilateral_filter
+}}} // namespace ftl { namespace cuda { namespace cudev
+
+#endif /* CUDA_DISABLER */
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp b/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
new file mode 100644
index 000000000..1f39d07f0
--- /dev/null
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
@@ -0,0 +1,8 @@
+namespace ftl { namespace cuda { namespace device
+{
+ namespace disp_bilateral_filter
+ {
+ template<typename T>
+ void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *, const float *, size_t, int radius, short edge_disc, short max_disc, cudaStream_t stream);
+ }
+}}}
diff --git a/components/operators/src/disparity/opencv/joint_bilateral.hpp b/components/operators/src/disparity/opencv/joint_bilateral.hpp
new file mode 100644
index 000000000..aeb4d9e07
--- /dev/null
+++ b/components/operators/src/disparity/opencv/joint_bilateral.hpp
@@ -0,0 +1,12 @@
+#ifndef _FTL_CUDA_OPENCV_BILATERAL_HPP_
+#define _FTL_CUDA_OPENCV_BILATERAL_HPP_
+
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace cuda {
+ cv::Ptr<cv::cuda::DisparityBilateralFilter> createDisparityBilateralFilter(int ndisp, int radius, int iters);
+}
+}
+
+#endif // _FTL_CUDA_OPENCV_BILATERAL_HPP_
diff --git a/components/operators/src/mask.cpp b/components/operators/src/mask.cpp
index c7dcbb2ac..e914a5ad8 100644
--- a/components/operators/src/mask.cpp
+++ b/components/operators/src/mask.cpp
@@ -15,9 +15,13 @@ DiscontinuityMask::~DiscontinuityMask() {
}
bool DiscontinuityMask::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
+ if (in.hasChannel(Channel::Mask)) return true;
+
int radius = config()->value("radius", 2);
float threshold = config()->value("threshold", 0.1f);
+ if (!in.hasChannel(Channel::Depth) || !in.hasChannel(Channel::Support1)) return false;
+
ftl::cuda::discontinuity(
out.createTexture<int>(Channel::Mask, ftl::rgbd::Format<int>(in.get<cv::cuda::GpuMat>(Channel::Depth).size())),
in.createTexture<uchar4>(Channel::Support1),
diff --git a/components/operators/src/normals.cpp b/components/operators/src/normals.cpp
index 57903aa1d..65fcef37b 100644
--- a/components/operators/src/normals.cpp
+++ b/components/operators/src/normals.cpp
@@ -22,7 +22,8 @@ bool Normals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Sour
}
if (out.hasChannel(Channel::Normals)) {
- LOG(WARNING) << "Output already has normals";
+ //LOG(WARNING) << "Output already has normals";
+ return true;
}
ftl::cuda::normals(
@@ -53,7 +54,8 @@ bool SmoothNormals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd
}
if (out.hasChannel(Channel::Normals)) {
- LOG(WARNING) << "Output already has normals";
+ //LOG(WARNING) << "Output already has normals";
+ return true;
}
auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
diff --git a/components/operators/src/operator.cpp b/components/operators/src/operator.cpp
index dd821ee6f..6b796267d 100644
--- a/components/operators/src/operator.cpp
+++ b/components/operators/src/operator.cpp
@@ -52,15 +52,22 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
if (i.instances[0]->type() == Operator::Type::OneToOne) {
// Make sure there are enough instances
- while (i.instances.size() < in.frames.size()) {
+ //while (i.instances.size() < in.frames.size()) {
+ //i.instances.push_back(i.maker->make());
+ //}
+ if (in.frames.size() > 1) {
i.instances.push_back(i.maker->make());
}
for (int j=0; j<in.frames.size(); ++j) {
- auto *instance = i.instances[j];
+ auto *instance = i.instances[j&0x1];
if (instance->enabled()) {
- instance->apply(in.frames[j], out.frames[j], in.sources[j], stream_actual);
+ try {
+ if (!instance->apply(in.frames[j], out.frames[j], in.sources[j], stream_actual)) return false;
+ } catch (const std::exception &e) {
+ LOG(ERROR) << "Operator exception: " << e.what();
+ }
}
}
} else if (i.instances[0]->type() == Operator::Type::ManyToMany) {
@@ -68,7 +75,7 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
if (instance->enabled()) {
try {
- instance->apply(in, out, stream_actual);
+ if (!instance->apply(in, out, stream_actual)) return false;
} catch (const std::exception &e) {
LOG(ERROR) << "Operator exception: " << e.what();
}
@@ -96,7 +103,7 @@ bool Graph::apply(Frame &in, Frame &out, Source *s, cudaStream_t stream) {
auto *instance = i.instances[0];
if (instance->enabled()) {
- instance->apply(in, out, s, stream);
+ if (!instance->apply(in, out, s, stream)) return false;
}
}
diff --git a/components/operators/src/segmentation.cpp b/components/operators/src/segmentation.cpp
index 9bf7605e1..005c436d2 100644
--- a/components/operators/src/segmentation.cpp
+++ b/components/operators/src/segmentation.cpp
@@ -16,7 +16,9 @@ CrossSupport::~CrossSupport() {
bool CrossSupport::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
bool use_mask = config()->value("discon_support", false);
- if (use_mask) {
+
+ if (use_mask && !in.hasChannel(Channel::Support2)) {
+ if (!in.hasChannel(Channel::Mask)) return false;
ftl::cuda::support_region(
in.createTexture<int>(Channel::Mask),
out.createTexture<uchar4>(Channel::Support2, ftl::rgbd::Format<uchar4>(in.get<cv::cuda::GpuMat>(Channel::Colour).size())),
@@ -24,7 +26,7 @@ bool CrossSupport::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd:
config()->value("h_max", 5),
config()->value("symmetric", false), stream
);
- } else {
+ } else if (!in.hasChannel(Channel::Support1)) {
ftl::cuda::support_region(
in.createTexture<uchar4>(Channel::Colour),
out.createTexture<uchar4>(Channel::Support1, ftl::rgbd::Format<uchar4>(in.get<cv::cuda::GpuMat>(Channel::Colour).size())),
diff --git a/components/renderers/cpp/include/ftl/render/tri_render.hpp b/components/renderers/cpp/include/ftl/render/tri_render.hpp
index 27b6db318..d15ba9c73 100644
--- a/components/renderers/cpp/include/ftl/render/tri_render.hpp
+++ b/components/renderers/cpp/include/ftl/render/tri_render.hpp
@@ -44,6 +44,7 @@ class Triangular : public ftl::render::Renderer {
float3 light_pos_;
Eigen::Matrix4d transform_;
float scale_;
+ int64_t last_frame_;
cv::cuda::GpuMat env_image_;
ftl::cuda::TextureObject<uchar4> env_tex_;
@@ -55,6 +56,8 @@ class Triangular : public ftl::render::Renderer {
void _reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t);
void _dibr(ftl::rgbd::Frame &, const Eigen::Matrix4d &t, cudaStream_t);
void _mesh(ftl::rgbd::Frame &, ftl::rgbd::Source *, const Eigen::Matrix4d &t, cudaStream_t);
+
+ bool _alreadySeen() const { return last_frame_ == scene_->timestamp; }
};
}
diff --git a/components/renderers/cpp/src/tri_render.cpp b/components/renderers/cpp/src/tri_render.cpp
index d1d0894f4..6df15bbd1 100644
--- a/components/renderers/cpp/src/tri_render.cpp
+++ b/components/renderers/cpp/src/tri_render.cpp
@@ -147,6 +147,7 @@ Triangular::Triangular(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::r
//filters_ = ftl::create<ftl::Filters>(this, "filters");
//filters_->create<ftl::filters::DepthSmoother>("hfnoise");
+ last_frame_ = -1;
}
Triangular::~Triangular() {
@@ -579,6 +580,13 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
int aligned_source = value("aligned_source",-1);
if (aligned_source >= 0 && aligned_source < scene_->frames.size()) {
+ // Can only send at originally received frame rate due to reuse of
+ // encodings that can't be sent twice.
+ if (_alreadySeen()) {
+ out.reset();
+ return false;
+ }
+
// FIXME: Output may not be same resolution as source!
cudaSafeCall(cudaStreamSynchronize(stream_));
scene_->frames[aligned_source].copyTo(Channel::Depth + Channel::Colour + Channel::Smoothing + Channel::Confidence, out);
@@ -596,6 +604,7 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
//out.resetTexture(Channel::Normals);
}
+ last_frame_ = scene_->timestamp;
return true;
}
@@ -704,5 +713,6 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
}
cudaSafeCall(cudaStreamSynchronize(stream_));
+ last_frame_ = scene_->timestamp;
return true;
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 712d29170..cc5aeec50 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -62,8 +62,7 @@ class Source {
capability_t capabilities_;
ftl::rgbd::Source *host_;
ftl::rgbd::Camera params_;
- cv::cuda::GpuMat rgb_;
- cv::cuda::GpuMat depth_;
+ ftl::rgbd::Frame frame_;
int64_t timestamp_;
//Eigen::Matrix4f pose_;
};
diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
index 8411c71a6..109eb008a 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
@@ -11,11 +11,13 @@
#include <ftl/codecs/channels.hpp>
#include <ftl/rgbd/format.hpp>
#include <ftl/codecs/bitrates.hpp>
+#include <ftl/codecs/packet.hpp>
#include <ftl/cuda_common.hpp>
#include <type_traits>
#include <array>
+#include <list>
namespace ftl {
namespace rgbd {
@@ -90,6 +92,18 @@ public:
template <typename T>
ftl::cuda::TextureObject<T> &createTexture(ftl::codecs::Channel c, bool interpolated=false);
+ /**
+ * Append encoded data for a channel. This will move the data, invalidating
+ * the original packet structure. It is to be used to allow data that is
+ * already encoded to be transmitted or saved again without re-encoding.
+ * A called to `create` will clear all encoded data for that channel.
+ */
+ void pushPacket(ftl::codecs::Channel c, ftl::codecs::Packet &pkt);
+
+ const std::list<ftl::codecs::Packet> &getPackets(ftl::codecs::Channel c) const;
+
+ void mergeEncoding(ftl::rgbd::Frame &f);
+
void resetTexture(ftl::codecs::Channel c);
/**
@@ -97,6 +111,11 @@ public:
*/
void reset();
+ /**
+ * Reset all channels and release memory.
+ */
+ void resetFull();
+
bool empty(ftl::codecs::Channels c);
inline bool empty(ftl::codecs::Channel c) {
@@ -159,6 +178,7 @@ private:
ftl::cuda::TextureObjectBase tex;
cv::Mat host;
cv::cuda::GpuMat gpu;
+ std::list<ftl::codecs::Packet> encoded;
};
std::array<ChannelData, ftl::codecs::Channels::kMax> data_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
index f18b52635..ee1322b73 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp
@@ -29,6 +29,8 @@ struct FrameSet {
void upload(ftl::codecs::Channels, cudaStream_t stream=0);
void download(ftl::codecs::Channels, cudaStream_t stream=0);
void swapTo(ftl::rgbd::FrameSet &);
+
+ void resetFull();
};
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/group.hpp b/components/rgbd-sources/include/ftl/rgbd/group.hpp
index fdd7ba741..8df9ab1f3 100644
--- a/components/rgbd-sources/include/ftl/rgbd/group.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/group.hpp
@@ -12,12 +12,16 @@
#include <vector>
namespace ftl {
+namespace operators {
+ class Graph;
+}
+
namespace rgbd {
class Source;
// Allows a latency of 20 frames maximum
-static const size_t kFrameBufferSize = 20;
+static const size_t kMaxFramesets = 15;
/**
* Manage a group of RGB-D sources to obtain synchronised sets of frames from
@@ -55,6 +59,12 @@ class Group {
*/
void addGroup(ftl::rgbd::Group *);
+ /**
+ * Add a pipeline to be run after each frame is received from source but
+ * before it as added to a synchronised frameset.
+ */
+ void addPipeline(ftl::operators::Graph *g) { pipeline_ = g; };
+
/**
* Provide a function to be called once per frame with a valid frameset
* at the specified latency. The function may not be called under certain
@@ -95,20 +105,25 @@ class Group {
* the reference point, this may already be several frames old. Latency
* does not correspond to actual current time.
*/
- void setLatency(int frames) { latency_ = frames; }
+ void setLatency(int frames) { }
void stop() {}
int streamID(const ftl::rgbd::Source *s) const;
private:
- std::vector<FrameSet> framesets_;
+ std::list<FrameSet*> framesets_; // Active framesets
+ std::list<FrameSet*> allocated_; // Keep memory allocations
+
std::vector<Source*> sources_;
+ ftl::operators::Graph *pipeline_;
size_t head_;
std::function<bool(FrameSet &)> callback_;
MUTEX mutex_;
int mspf_;
- int latency_;
+ float latency_;
+ float fps_;
+ int stats_count_;
int64_t last_ts_;
std::atomic<int> jobs_;
volatile bool skip_;
@@ -120,13 +135,19 @@ class Group {
/* Insert a new frameset into the buffer, along with all intermediate
* framesets between the last in buffer and the new one.
*/
- void _addFrameset(int64_t timestamp);
+ ftl::rgbd::FrameSet *_addFrameset(int64_t timestamp);
void _retrieveJob(ftl::rgbd::Source *);
void _computeJob(ftl::rgbd::Source *);
/* Find a frameset with given latency in frames. */
- ftl::rgbd::FrameSet *_getFrameset(int f);
+ ftl::rgbd::FrameSet *_getFrameset();
+
+ /* Search for a matching frameset. */
+ ftl::rgbd::FrameSet *_findFrameset(int64_t ts);
+ void _freeFrameset(ftl::rgbd::FrameSet *);
+
+ void _recordStats(float fps, float latency);
};
}
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 895817359..dc41f10a1 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -32,6 +32,7 @@ class VirtualSource;
class Player;
typedef std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)> RawCallback;
+typedef std::function<void(int64_t,ftl::rgbd::Frame&)> FrameCallback;
/**
* RGBD Generic data source configurable entity. This class hides the
@@ -176,14 +177,14 @@ class Source : public ftl::Configurable {
SHARED_MUTEX &mutex() { return mutex_; }
- std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> &callback() { return callback_; }
+ const FrameCallback &callback() { return callback_; }
/**
* Set the callback that receives decoded frames as they are generated.
* There can be only a single such callback as the buffers can be swapped
* by the callback.
*/
- void setCallback(std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> cb);
+ void setCallback(const FrameCallback &cb);
void removeCallback() { callback_ = nullptr; }
/**
@@ -207,7 +208,8 @@ class Source : public ftl::Configurable {
* Notify of a decoded or available pair of frames. This calls the source
* callback after having verified the correct resolution of the frames.
*/
- void notify(int64_t ts, cv::cuda::GpuMat &c1, cv::cuda::GpuMat &c2);
+ //void notify(int64_t ts, cv::cuda::GpuMat &c1, cv::cuda::GpuMat &c2);
+ void notify(int64_t ts, ftl::rgbd::Frame &f);
// ==== Inject Data into stream ============================================
@@ -227,8 +229,8 @@ class Source : public ftl::Configurable {
SHARED_MUTEX mutex_;
ftl::codecs::Channel channel_;
cudaStream_t stream_;
- std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> callback_;
- std::list<std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)>> rawcallbacks_;
+ FrameCallback callback_;
+ std::list<RawCallback> rawcallbacks_;
detail::Source *_createImplementation();
detail::Source *_createFileImpl(const ftl::URI &uri);
diff --git a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
index 1fa1cb486..8c3277556 100644
--- a/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/streamer.hpp
@@ -107,6 +107,8 @@ class Streamer : public ftl::Configurable {
*/
void add(ftl::rgbd::Group *grp);
+ ftl::rgbd::Group *group() { return &group_; }
+
void remove(Source *);
void remove(const std::string &);
diff --git a/components/rgbd-sources/include/ftl/rgbd/virtual.hpp b/components/rgbd-sources/include/ftl/rgbd/virtual.hpp
index f0ab3a93b..f9bf5e7f5 100644
--- a/components/rgbd-sources/include/ftl/rgbd/virtual.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/virtual.hpp
@@ -11,7 +11,7 @@ class VirtualSource : public ftl::rgbd::Source {
explicit VirtualSource(ftl::config::json_t &cfg);
~VirtualSource();
- void onRender(const std::function<void(ftl::rgbd::Frame &)> &);
+ void onRender(const std::function<bool(ftl::rgbd::Frame &)> &);
/**
* Write frames into source buffers from an external renderer. Virtual
diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp
index 3c9fe54d8..c1b76e117 100644
--- a/components/rgbd-sources/src/frame.cpp
+++ b/components/rgbd-sources/src/frame.cpp
@@ -11,6 +11,19 @@ static cv::cuda::GpuMat noneGPU;
void Frame::reset() {
channels_.clear();
gpu_.clear();
+ for (size_t i=0u; i<Channels::kMax; ++i) {
+ data_[i].encoded.clear();
+ }
+}
+
+void Frame::resetFull() {
+ channels_.clear();
+ gpu_.clear();
+ for (size_t i=0u; i<Channels::kMax; ++i) {
+ data_[i].gpu = cv::cuda::GpuMat();
+ data_[i].host = cv::Mat();
+ data_[i].encoded.clear();
+ }
}
void Frame::download(Channel c, cv::cuda::Stream stream) {
@@ -39,6 +52,37 @@ void Frame::upload(Channels c, cv::cuda::Stream stream) {
}
}
+void Frame::pushPacket(ftl::codecs::Channel c, ftl::codecs::Packet &pkt) {
+ if (hasChannel(c)) {
+ auto &m1 = _get(c);
+ m1.encoded.emplace_back() = std::move(pkt);
+ } else {
+ LOG(ERROR) << "Channel " << (int)c << " doesn't exist for packet push";
+ }
+}
+
+const std::list<ftl::codecs::Packet> &Frame::getPackets(ftl::codecs::Channel c) const {
+ if (!hasChannel(c)) {
+ throw ftl::exception(ftl::Formatter() << "Frame channel does not exist: " << (int)c);
+ }
+
+ auto &m1 = _get(c);
+ return m1.encoded;
+}
+
+void Frame::mergeEncoding(ftl::rgbd::Frame &f) {
+ //LOG(INFO) << "MERGE " << (unsigned int)f.channels_;
+ for (auto c : channels_) {
+ //if (!f.hasChannel(c)) f.create<cv::cuda::GpuMat>(c);
+ if (f.hasChannel(c)) {
+ auto &m1 = _get(c);
+ auto &m2 = f._get(c);
+ m1.encoded.splice(m1.encoded.begin(), m2.encoded);
+ //LOG(INFO) << "SPLICED: " << m1.encoded.size();
+ }
+ }
+}
+
bool Frame::empty(ftl::codecs::Channels channels) {
for (auto c : channels) {
if (empty(c)) return true;
@@ -70,6 +114,12 @@ void Frame::swapTo(ftl::codecs::Channels channels, Frame &f) {
auto temptex = std::move(m2.tex);
m2.tex = std::move(m1.tex);
m1.tex = std::move(temptex);
+
+ if (m2.encoded.size() > 0 || m1.encoded.size() > 0) {
+ auto tempenc = std::move(m2.encoded);
+ m2.encoded = std::move(m1.encoded);
+ m1.encoded = std::move(tempenc);
+ }
}
}
}
@@ -83,6 +133,12 @@ void Frame::swapChannels(ftl::codecs::Channel a, ftl::codecs::Channel b) {
auto temptex = std::move(m2.tex);
m2.tex = std::move(m1.tex);
m1.tex = std::move(temptex);
+
+ if (m2.encoded.size() > 0 || m1.encoded.size() > 0) {
+ auto tempenc = std::move(m2.encoded);
+ m2.encoded = std::move(m1.encoded);
+ m1.encoded = std::move(tempenc);
+ }
}
void Frame::copyTo(ftl::codecs::Channels channels, Frame &f) {
@@ -94,6 +150,9 @@ void Frame::copyTo(ftl::codecs::Channels channels, Frame &f) {
if (channels.has(c)) {
if (isCPU(c)) get<cv::Mat>(c).copyTo(f.create<cv::Mat>(c));
else get<cv::cuda::GpuMat>(c).copyTo(f.create<cv::cuda::GpuMat>(c));
+ auto &m1 = _get(c);
+ auto &m2 = f._get(c);
+ m2.encoded = m1.encoded; //std::move(m1.encoded); // TODO: Copy?
}
}
}
@@ -176,13 +235,15 @@ template <> cv::Mat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::Form
channels_ += c;
gpu_ -= c;
- auto &m = _get(c).host;
+ auto &m = _get(c);
+
+ m.encoded.clear(); // Remove all old encoded data
if (!f.empty()) {
- m.create(f.size(), f.cvType);
+ m.host.create(f.size(), f.cvType);
}
- return m;
+ return m.host;
}
template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &f) {
@@ -192,13 +253,15 @@ template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::r
channels_ += c;
gpu_ += c;
- auto &m = _get(c).gpu;
+ auto &m = _get(c);
+
+ m.encoded.clear(); // Remove all old encoded data
if (!f.empty()) {
- m.create(f.size(), f.cvType);
+ m.gpu.create(f.size(), f.cvType);
}
- return m;
+ return m.gpu;
}
template <> cv::Mat &Frame::create(ftl::codecs::Channel c) {
@@ -208,8 +271,11 @@ template <> cv::Mat &Frame::create(ftl::codecs::Channel c) {
channels_ += c;
gpu_ -= c;
- auto &m = _get(c).host;
- return m;
+ auto &m = _get(c);
+
+ m.encoded.clear(); // Remove all old encoded data
+
+ return m.host;
}
template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c) {
@@ -219,8 +285,11 @@ template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c) {
channels_ += c;
gpu_ += c;
- auto &m = _get(c).gpu;
- return m;
+ auto &m = _get(c);
+
+ m.encoded.clear(); // Remove all old encoded data
+
+ return m.gpu;
}
void Frame::resetTexture(ftl::codecs::Channel c) {
diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp
index 38232e3f6..44be010a3 100644
--- a/components/rgbd-sources/src/frameset.cpp
+++ b/components/rgbd-sources/src/frameset.cpp
@@ -37,3 +37,11 @@ void FrameSet::swapTo(ftl::rgbd::FrameSet &fs) {
stale = true;
}
+
+void FrameSet::resetFull() {
+ //count = 0;
+ //stale = false;
+ for (auto &f : frames) {
+ f.resetFull();
+ }
+}
diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp
index aad850d2c..ca64d4764 100644
--- a/components/rgbd-sources/src/group.cpp
+++ b/components/rgbd-sources/src/group.cpp
@@ -1,19 +1,31 @@
#include <ftl/rgbd/group.hpp>
#include <ftl/rgbd/source.hpp>
#include <ftl/timer.hpp>
+#include <ftl/operators/operator.hpp>
#include <chrono>
using ftl::rgbd::Group;
using ftl::rgbd::Source;
-using ftl::rgbd::kFrameBufferSize;
+using ftl::rgbd::kMaxFramesets;
using std::vector;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;
using ftl::codecs::Channel;
-Group::Group() : framesets_(kFrameBufferSize), head_(0) {
- framesets_[0].timestamp = -1;
+Group::Group() : pipeline_(nullptr), head_(0) {
+ //framesets_[0].timestamp = -1;
+ //framesets_[0].stale = true;
+
+ /*for (auto &i : framesets_) {
+ i.stale = true;
+ }*/
+
+ // Allocate some initial framesets
+ //for (int i=0; i<10; ++i) {
+ // allocated_.push_back(new ftl::rgbd::FrameSet);
+ //}
+
jobs_ = 0;
skip_ = false;
//setFPS(20);
@@ -21,7 +33,9 @@ Group::Group() : framesets_(kFrameBufferSize), head_(0) {
mspf_ = ftl::timer::getInterval();
name_ = "NoName";
- setLatency(5);
+ latency_ = 0.0f;;
+ stats_count_ = 0;
+ fps_ = 0.0f;
}
Group::~Group() {
@@ -50,54 +64,54 @@ void Group::addSource(ftl::rgbd::Source *src) {
size_t ix = sources_.size();
sources_.push_back(src);
- src->setCallback([this,ix,src](int64_t timestamp, cv::cuda::GpuMat &rgb, cv::cuda::GpuMat &depth) {
+ src->setCallback([this,ix,src](int64_t timestamp, ftl::rgbd::Frame &frame) {
if (timestamp == 0) return;
auto chan = src->getChannel();
- //LOG(INFO) << "SRC CB: " << timestamp << " (" << framesets_[head_].timestamp << ")";
+ //LOG(INFO) << "SRC CB (" << name_ << "): " << timestamp << " (" << ")";
UNIQUE_LOCK(mutex_, lk);
- if (timestamp > framesets_[head_].timestamp) {
+ auto *fs = _findFrameset(timestamp);
+
+ if (!fs) {
// Add new frameset
- _addFrameset(timestamp);
- } else if (framesets_[(head_+1)%kFrameBufferSize].timestamp > timestamp) {
+ fs = _addFrameset(timestamp);
+
+ if (!fs) return;
+ } /*else if (framesets_[(head_+1)%kFrameBufferSize].timestamp > timestamp) {
// Too old, just ditch it
LOG(WARNING) << "Received frame too old for buffer";
return;
- }
+ }*/
// Search backwards to find match
- for (size_t i=0; i<kFrameBufferSize; ++i) {
- FrameSet &fs = framesets_[(head_+kFrameBufferSize-i) % kFrameBufferSize];
- if (fs.timestamp == timestamp) {
+ //for (size_t i=0; i<kFrameBufferSize; ++i) {
+ // FrameSet &fs = framesets_[(head_+kFrameBufferSize-i) % kFrameBufferSize];
+ //if (fs.timestamp == timestamp) {
lk.unlock();
- SHARED_LOCK(fs.mtx, lk2);
-
- //LOG(INFO) << "Adding frame: " << ix << " for " << timestamp;
- // Ensure channels match source mat format
- //fs.channel1[ix].create(rgb.size(), rgb.type());
- //fs.channel2[ix].create(depth.size(), depth.type());
- fs.frames[ix].create<cv::cuda::GpuMat>(Channel::Colour, Format<uchar3>(rgb.size())); //.create(rgb.size(), rgb.type());
- if (chan != Channel::None) fs.frames[ix].create<cv::cuda::GpuMat>(chan, ftl::rgbd::FormatBase(depth.cols, depth.rows, depth.type())); //.create(depth.size(), depth.type());
-
- //cv::swap(rgb, fs.channel1[ix]);
- //cv::swap(depth, fs.channel2[ix]);
- cv::cuda::swap(rgb, fs.frames[ix].get<cv::cuda::GpuMat>(Channel::Colour));
- if (chan != Channel::None) cv::cuda::swap(depth, fs.frames[ix].get<cv::cuda::GpuMat>(chan));
-
- ++fs.count;
- fs.mask |= (1 << ix);
-
- if (fs.count == sources_.size()) {
- //LOG(INFO) << "COMPLETE SET: " << fs.timestamp;
- } else if (fs.count > sources_.size()) {
- LOG(ERROR) << "Too many frames for frame set: " << fs.timestamp << " sources=" << sources_.size();
+ SHARED_LOCK(fs->mtx, lk2);
+
+ frame.swapTo(ftl::codecs::kAllChannels, fs->frames[ix]);
+
+ if (fs->count+1 == sources_.size()) {
+ if (pipeline_) {
+ pipeline_->apply(*fs, *fs, 0);
+ }
+ }
+
+ ++fs->count;
+ fs->mask |= (1 << ix);
+
+ if (fs->count == sources_.size()) {
+ //LOG(INFO) << "COMPLETE SET (" << name_ << "): " << fs->timestamp;
+ } else if (fs->count > sources_.size()) {
+ LOG(ERROR) << "Too many frames for frame set: " << fs->timestamp << " sources=" << sources_.size();
} else {
//LOG(INFO) << "INCOMPLETE SET (" << ix << "): " << fs.timestamp;
}
- if (callback_ && fs.count == sources_.size()) {
+ /*if (callback_ && fs->count == sources_.size()) {
try {
if (callback_(fs)) {
// TODO: Remove callback if returns false?
@@ -108,12 +122,12 @@ void Group::addSource(ftl::rgbd::Source *src) {
// Reset count to prevent multiple reads of these frames
//fs.count = 0;
- }
+ }*/
return;
- }
- }
- DLOG(WARNING) << "Frame timestamp not found in buffer";
+ //}
+ //}
+ //LOG(WARNING) << "Frame timestamp not found in buffer";
});
}
@@ -153,9 +167,9 @@ int Group::streamID(const ftl::rgbd::Source *s) const {
}
void Group::sync(std::function<bool(ftl::rgbd::FrameSet &)> cb) {
- if (latency_ == 0) {
- callback_ = cb;
- }
+ //if (latency_ == 0) {
+ // callback_ = cb;
+ //}
// 1. Capture camera frames with high precision
cap_id_ = ftl::timer::add(ftl::timer::kTimerHighPrecision, [this](int64_t ts) {
@@ -163,7 +177,6 @@ void Group::sync(std::function<bool(ftl::rgbd::FrameSet &)> cb) {
if (skip_) return true;
- last_ts_ = ts;
for (auto s : sources_) {
s->capture(ts);
}
@@ -183,6 +196,7 @@ void Group::sync(std::function<bool(ftl::rgbd::FrameSet &)> cb) {
// 3. Issue IO retrieve ad compute jobs before finding a valid
// frame at required latency to pass to callback.
main_id_ = ftl::timer::add(ftl::timer::kTimerMain, [this,cb](int64_t ts) {
+ //if (skip_) LOG(ERROR) << "SKIPPING TIMER JOB " << ts;
if (skip_) return true;
jobs_++;
@@ -191,41 +205,55 @@ void Group::sync(std::function<bool(ftl::rgbd::FrameSet &)> cb) {
ftl::pool.push([this,s](int id) {
_retrieveJob(s);
+ //if (jobs_ == 0) LOG(INFO) << "LAST JOB = Retrieve";
--jobs_;
});
ftl::pool.push([this,s](int id) {
_computeJob(s);
+ //if (jobs_ == 0) LOG(INFO) << "LAST JOB = Compute";
--jobs_;
});
}
// Find a previous frameset and specified latency and do the sync
// callback with that frameset.
- if (latency_ > 0) {
+ //if (latency_ > 0) {
ftl::rgbd::FrameSet *fs = nullptr;
UNIQUE_LOCK(mutex_, lk);
- fs = _getFrameset(latency_);
+ fs = _getFrameset();
+
+ //LOG(INFO) << "Latency for " << name_ << " = " << (latency_*ftl::timer::getInterval()) << "ms";
if (fs) {
UNIQUE_LOCK(fs->mtx, lk2);
lk.unlock();
-
- try {
- cb(*fs);
- //LOG(INFO) << "Frameset processed (" << name_ << "): " << fs->timestamp;
- } catch(std::exception &e) {
- LOG(ERROR) << "Exception in group sync callback: " << e.what();
- }
-
// The buffers are invalid after callback so mark stale
fs->stale = true;
+
+ //ftl::pool.push([this,fs,cb](int) {
+ try {
+ cb(*fs);
+ //LOG(INFO) << "Frameset processed (" << name_ << "): " << fs->timestamp;
+ } catch(std::exception &e) {
+ LOG(ERROR) << "Exception in group sync callback: " << e.what();
+ }
+
+ //fs->resetFull();
+
+ lk.lock();
+ _freeFrameset(fs);
+
+ jobs_--;
+ //});
} else {
//LOG(INFO) << "NO FRAME FOUND: " << last_ts_ - latency_*mspf_;
+ //latency_++;
+ jobs_--;
}
- }
+ //}
- jobs_--;
+ //if (jobs_ == 0) LOG(INFO) << "LAST JOB = Main";
return true;
});
}
@@ -242,47 +270,130 @@ void Group::removeRawCallback(const std::function<void(ftl::rgbd::Source*, const
}
}
-//ftl::rgbd::FrameSet &Group::_getRelativeFrameset(int rel) {
-// int idx = (rel < 0) ? (head_+kFrameBufferSize+rel)%kFrameBufferSize : (head_+rel)%kFrameBufferSize;
-// return framesets_[idx];
-//}
-
-ftl::rgbd::FrameSet *Group::_getFrameset(int f) {
- const int64_t lookfor = last_ts_-f*mspf_;
+static void mergeFrameset(ftl::rgbd::FrameSet &f1, ftl::rgbd::FrameSet &f2) {
+ // Prepend all frame encodings in f2 into corresponding frame in f1.
+ for (int i=0; i<f1.frames.size(); ++i) {
+ f1.frames[i].mergeEncoding(f2.frames[i]);
+ }
+}
- for (size_t i=1; i<kFrameBufferSize; ++i) {
- int idx = (head_+kFrameBufferSize-i)%kFrameBufferSize;
+void Group::_recordStats(float fps, float latency) {
+ latency_ += latency;
+ fps_ += fps;
+ ++stats_count_;
+
+ if (fps_/float(stats_count_) <= float(stats_count_)) {
+ fps_ /= float(stats_count_);
+ latency_ /= float(stats_count_);
+ LOG(INFO) << name_ << ": fps = " << fps_ << ", latency = " << latency_;
+ fps_ = 0.0f;
+ latency_ = 0.0f;
+ stats_count_ = 0;
+ }
+}
- if (framesets_[idx].timestamp == lookfor && framesets_[idx].count != sources_.size()) {
- LOG(WARNING) << "Required frame not complete in '" << name_ << "' (timestamp=" << (framesets_[idx].timestamp) << " buffer=" << i << ")";
- //framesets_[idx].stale = true;
- //return &framesets_[idx];
- continue;
+ftl::rgbd::FrameSet *Group::_findFrameset(int64_t ts) {
+ // Search backwards to find match
+ for (auto f : framesets_) {
+ if (f->timestamp == ts) {
+ return f;
+ } else if (f->timestamp < ts) {
+ return nullptr;
}
+ }
+
+ return nullptr;
+}
- if (framesets_[idx].stale) return nullptr;
+/*
+ * Get the most recent completed frameset that isn't stale.
+ * Note: Must occur inside a mutex lock.
+ */
+ftl::rgbd::FrameSet *Group::_getFrameset() {
+ for (auto i=framesets_.begin(); i!=framesets_.end(); i++) {
+ auto *f = *i;
+ if (!f->stale && f->count == sources_.size()) {
+ //LOG(INFO) << "GET FRAMESET and remove: " << f->timestamp;
+ auto j = framesets_.erase(i);
+
+ int count = 0;
+ // Merge all previous frames
+ for (; j!=framesets_.end(); j++) {
+ ++count;
+
+ auto *f2 = *j;
+ j = framesets_.erase(j);
+ mergeFrameset(*f,*f2);
+ _freeFrameset(f2);
+ }
- if (framesets_[idx].timestamp == lookfor && framesets_[idx].count == sources_.size()) {
- //framesets_[idx].stale = false;
- return &framesets_[idx];
- } else if (framesets_[idx].timestamp < lookfor && framesets_[idx].count == sources_.size()) {
- //framesets_[idx].stale = true;
- return &framesets_[idx];
- }
+ //if (count > 0) LOG(INFO) << "COUNT = " << count;
+ int64_t now = ftl::timer::get_time();
+ float framerate = 1000.0f / float(now - last_ts_);
+ _recordStats(framerate, now - f->timestamp);
+ last_ts_ = now;
+ return f;
+ }
}
+
return nullptr;
}
-void Group::_addFrameset(int64_t timestamp) {
- int count = (framesets_[head_].timestamp == -1) ? 200 : (timestamp - framesets_[head_].timestamp) / mspf_;
+void Group::_freeFrameset(ftl::rgbd::FrameSet *fs) {
+ allocated_.push_back(fs);
+}
+
+ftl::rgbd::FrameSet *Group::_addFrameset(int64_t timestamp) {
+ if (allocated_.size() == 0) {
+ if (framesets_.size() < kMaxFramesets) {
+ allocated_.push_back(new ftl::rgbd::FrameSet);
+ } else {
+ LOG(ERROR) << "Could not allocate frameset.";
+ return nullptr;
+ }
+ }
+ FrameSet *newf = allocated_.front();
+ allocated_.pop_front();
+
+ newf->timestamp = timestamp;
+ newf->count = 0;
+ newf->mask = 0;
+ newf->stale = false;
+ newf->frames.resize(sources_.size());
+
+ for (auto &f : newf->frames) f.reset();
+
+ if (newf->sources.size() != sources_.size()) {
+ newf->sources.clear();
+ for (auto s : sources_) newf->sources.push_back(s);
+ }
+
+ // Insertion sort by timestamp
+ for (auto i=framesets_.begin(); i!=framesets_.end(); i++) {
+ auto *f = *i;
+
+ if (timestamp > f->timestamp) {
+ framesets_.insert(i, newf);
+ return newf;
+ }
+ }
+
+ framesets_.push_back(newf);
+ return newf;
+
+
+ //int count = (framesets_[head_].timestamp == -1) ? 1 : (timestamp - framesets_[head_].timestamp) / mspf_;
//LOG(INFO) << "Massive timestamp difference: " << count;
// Allow for massive timestamp changes (Windows clock adjust)
// Only add a single frameset for large changes
- if (count < -int(kFrameBufferSize) || count >= kFrameBufferSize-1) {
+ //if (count < -int(kFrameBufferSize) || count >= kFrameBufferSize-1) {
+ /*if (framesets_[head_].timestamp < timestamp) {
head_ = (head_+1) % kFrameBufferSize;
+ //if (framesets_[head_].stale == false) LOG(FATAL) << "Buffer exceeded";
+
#ifdef DEBUG_MUTEX
std::unique_lock<std::shared_timed_mutex> lk(framesets_[head_].mtx, std::defer_lock);
#else
@@ -296,8 +407,6 @@ void Group::_addFrameset(int64_t timestamp) {
framesets_[head_].count = 0;
framesets_[head_].mask = 0;
framesets_[head_].stale = false;
- //framesets_[head_].channel1.resize(sources_.size());
- //framesets_[head_].channel2.resize(sources_.size());
framesets_[head_].frames.resize(sources_.size());
for (auto &f : framesets_[head_].frames) f.reset();
@@ -306,40 +415,31 @@ void Group::_addFrameset(int64_t timestamp) {
framesets_[head_].sources.clear();
for (auto s : sources_) framesets_[head_].sources.push_back(s);
}
- return;
- }
-
- if (count < 1) return;
-
- // Must make sure to also insert missing framesets
- for (int i=0; i<count; ++i) {
- int64_t lt = (framesets_[head_].timestamp == -1) ? timestamp-mspf_ : framesets_[head_].timestamp;
- head_ = (head_+1) % kFrameBufferSize;
- #ifdef DEBUG_MUTEX
- std::unique_lock<std::shared_timed_mutex> lk(framesets_[head_].mtx, std::defer_lock);
- #else
- std::unique_lock<std::shared_mutex> lk(framesets_[head_].mtx, std::defer_lock);
- #endif
- if (!lk.try_lock()) {
- LOG(ERROR) << "Frameset in use!! (" << name_ << ") " << framesets_[head_].timestamp << " stale=" << framesets_[head_].stale;
- continue;
- }
- framesets_[head_].timestamp = lt+mspf_;
- framesets_[head_].count = 0;
- framesets_[head_].mask = 0;
- framesets_[head_].stale = false;
- //framesets_[head_].channel1.resize(sources_.size());
- //framesets_[head_].channel2.resize(sources_.size());
- framesets_[head_].frames.resize(sources_.size());
+ // Find number of valid frames that will be skipped
+ int count = 0;
+ //for (int j=1; j<kFrameBufferSize; ++j) {
+ int idx2 = (head_+kFrameBufferSize-1)%kFrameBufferSize;
+ //if (framesets_[idx2].stale || framesets_[idx2].timestamp <= 0) break;
+ ++count;
+
+ // Make sure encoded packets are moved from skipped frames
+ //if (framesets_[idx2].count >= framesets_[idx2].sources.size()) {
+ if (framesets_[idx2].stale == false) {
+ mergeFrameset(framesets_[head_], framesets_[idx2]);
+ framesets_[idx2].stale = true;
+ framesets_[idx2].timestamp = -1;
+ }
+ //}
- for (auto &f : framesets_[head_].frames) f.reset();
+ float framerate = 1000.0f / float((count+1)*ftl::timer::getInterval());
+ _recordStats(framerate, ftl::timer::get_time() - timestamp);
- if (framesets_[head_].sources.size() != sources_.size()) {
- framesets_[head_].sources.clear();
- for (auto s : sources_) framesets_[head_].sources.push_back(s);
- }
- }
+ return;
+ } else {
+ LOG(ERROR) << "Old frame received: " << (framesets_[head_].timestamp - timestamp);
+ return;
+ }*/
}
void Group::setName(const std::string &name) {
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index f4f217af1..384978e7f 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -32,6 +32,8 @@ using ftl::rgbd::capability_t;
using ftl::codecs::Channel;
using ftl::rgbd::detail::FileSource;
using ftl::rgbd::Camera;
+using ftl::rgbd::RawCallback;
+using ftl::rgbd::FrameCallback;
std::map<std::string, ftl::rgbd::Player*> Source::readers__;
@@ -247,12 +249,12 @@ const ftl::rgbd::Camera Source::parameters(ftl::codecs::Channel chan) const {
return (impl_) ? impl_->parameters(chan) : parameters();
}
-void Source::setCallback(std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> cb) {
+void Source::setCallback(const FrameCallback &cb) {
if (bool(callback_)) LOG(ERROR) << "Source already has a callback: " << getURI();
callback_ = cb;
}
-void Source::addRawCallback(const std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)> &f) {
+void Source::addRawCallback(const RawCallback &f) {
UNIQUE_LOCK(mutex_,lk);
rawcallbacks_.push_back(f);
}
@@ -298,12 +300,8 @@ Camera Camera::scaled(int width, int height) const {
return newcam;
}
-void Source::notify(int64_t ts, cv::cuda::GpuMat &c1, cv::cuda::GpuMat &c2) {
- // Ensure correct scaling of images and parameters.
- int max_width = max(impl_->params_.width, max(c1.cols, c2.cols));
- int max_height = max(impl_->params_.height, max(c1.rows, c2.rows));
-
- if (callback_) callback_(ts, c1, c2);
+void Source::notify(int64_t ts, ftl::rgbd::Frame &f) {
+ if (callback_) callback_(ts, f);
}
void Source::inject(const Eigen::Matrix4d &pose) {
diff --git a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
index 554a558b3..01b602c20 100644
--- a/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
+++ b/components/rgbd-sources/src/sources/ftlfile/file_source.cpp
@@ -6,6 +6,7 @@
using ftl::rgbd::detail::FileSource;
using ftl::codecs::codec_t;
using ftl::codecs::Channel;
+using cv::cuda::GpuMat;
void FileSource::_createDecoder(int ix, const ftl::codecs::Packet &pkt) {
if (decoders_[ix]) {
@@ -23,11 +24,12 @@ void FileSource::_createDecoder(int ix, const ftl::codecs::Packet &pkt) {
FileSource::FileSource(ftl::rgbd::Source *s, ftl::rgbd::Player *r, int sid) : ftl::rgbd::detail::Source(s) {
reader_ = r;
has_calibration_ = false;
- decoders_[0] = nullptr;
- decoders_[1] = nullptr;
+
+ for (int i=0; i<32; ++i) decoders_[i] = nullptr;
+
cache_read_ = -1;
cache_write_ = 0;
- realtime_ = host_->value("realtime", true);
+ realtime_ = host_->value("realtime", false);
timestamp_ = r->getStartTime();
sourceid_ = sid;
freeze_ = host_->value("freeze", false);
@@ -39,7 +41,7 @@ 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);
+ //host_->notifyRaw(spkt, pkt);
// Some channels are to be directly handled by the source object and
// do not proceed to any subsequent step.
@@ -60,6 +62,8 @@ FileSource::FileSource(ftl::rgbd::Source *s, ftl::rgbd::Player *r, int sid) : ft
return;
}
+ available_channels_ |= spkt.channel;
+
// FIXME: For bad and old FTL files where wrong channel is used
if (pkt.codec == codec_t::POSE) {
_processPose(pkt);
@@ -77,9 +81,8 @@ FileSource::FileSource(ftl::rgbd::Source *s, ftl::rgbd::Player *r, int sid) : ft
cache_[cache_write_].emplace_back();
auto &c = cache_[cache_write_].back();
- // TODO: Attempt to avoid this copy operation
c.spkt = spkt;
- c.pkt = pkt;
+ c.pkt = std::move(pkt);
});
}
@@ -161,9 +164,10 @@ bool FileSource::compute(int n, int b) {
// Freeze frame requires a copy to be made each time...
if (have_frozen_) {
cv::cuda::GpuMat t1, t2;
- if (!rgb_.empty()) rgb_.copyTo(t1);
- if (!depth_.empty()) depth_.copyTo(t2);
- host_->notify(timestamp_, t1, t2);
+ // FIXME: Implement this?
+ //if (!rgb_.empty()) rgb_.copyTo(t1);
+ //if (!depth_.empty()) depth_.copyTo(t2);
+ //host_->notify(timestamp_, t1, t2);
return true;
}
@@ -173,6 +177,17 @@ bool FileSource::compute(int n, int b) {
int64_t lastts = 0;
int lastc = 0;
+ frame_.reset();
+
+ // Decide which channels to decode
+ decode_channels_ = Channel::Colour;
+
+ if (available_channels_.has(host_->getChannel())) {
+ decode_channels_ |= host_->getChannel();
+ } else if (host_->getChannel() == Channel::Depth && available_channels_.has(Channel::Right)) {
+ decode_channels_ |= Channel::Right;
+ }
+
// Go through previously read and cached frames in sequence
// needs to be done due to P-Frames
for (auto i=cache_[cache_read_].begin(); i!=cache_[cache_read_].end(); ++i) {
@@ -187,32 +202,38 @@ bool FileSource::compute(int n, int b) {
lastc++;
}
- if (c.spkt.channel == Channel::Colour) {
- rgb_.create(cv::Size(ftl::codecs::getWidth(c.pkt.definition),ftl::codecs::getHeight(c.pkt.definition)), CV_8UC3);
- _createDecoder(0, c.pkt);
+ // Has this channel been requested?
+ if (decode_channels_.has(c.spkt.channel)) { //c.spkt.channel == Channel::Colour || c.spkt.channel == host_->getChannel()) {
- try {
- decoders_[0]->decode(c.pkt, rgb_);
- } catch (std::exception &e) {
- LOG(INFO) << "Decoder exception: " << e.what();
+ // Create channel only if not existing
+ if (!frame_.hasChannel(c.spkt.channel)) {
+ frame_.create<GpuMat>(c.spkt.channel);
}
- } else if (host_->getChannel() == c.spkt.channel) {
- depth_.create(cv::Size(ftl::codecs::getWidth(c.pkt.definition),ftl::codecs::getHeight(c.pkt.definition)), CV_32F);
- _createDecoder(1, c.pkt);
- try {
- decoders_[1]->decode(c.pkt, depth_);
- } catch (std::exception &e) {
- LOG(INFO) << "Decoder exception: " << e.what();
+
+ int channum = int(c.spkt.channel);
+ if (!ftl::codecs::isFloatChannel(c.spkt.channel)) {
+ frame_.get<GpuMat>(c.spkt.channel).create(cv::Size(ftl::codecs::getWidth(c.pkt.definition),ftl::codecs::getHeight(c.pkt.definition)), CV_8UC4);
+ _createDecoder(channum, c.pkt);
+
+ try {
+ decoders_[channum]->decode(c.pkt, frame_.get<GpuMat>(c.spkt.channel));
+ } catch (std::exception &e) {
+ LOG(INFO) << "Decoder exception: " << e.what();
+ }
+ } else {
+ frame_.get<GpuMat>(c.spkt.channel).create(cv::Size(ftl::codecs::getWidth(c.pkt.definition),ftl::codecs::getHeight(c.pkt.definition)), CV_32F);
+ _createDecoder(channum, c.pkt);
+ try {
+ decoders_[channum]->decode(c.pkt, frame_.get<GpuMat>(c.spkt.channel));
+ } catch (std::exception &e) {
+ LOG(INFO) << "Decoder exception: " << e.what();
+ }
}
+
+ frame_.pushPacket(c.spkt.channel, c.pkt);
}
-
- //_createDecoder((c.spkt.channel == Channel::Colour) ? 0 : 1, c.pkt);
-
- /*try {
- decoders_[(c.spkt.channel == Channel::Colour) ? 0 : 1]->decode(c.pkt, (c.spkt.channel == Channel::Colour) ? rgb_ : depth_);
- } catch (std::exception &e) {
- LOG(INFO) << "Decoder exception: " << e.what();
- }*/
+
+ //host_->notifyRaw(c.spkt, c.pkt);
}
// FIXME: Consider case of Channel::None
@@ -221,15 +242,17 @@ bool FileSource::compute(int n, int b) {
return false;
}
+ //LOG(INFO) << "MERGE " << cache_[cache_read_].size();
+
cache_[cache_read_].clear();
- if (rgb_.empty() || depth_.empty()) return false;
+ //if (rgb_.empty() || depth_.empty()) return false;
// Inform about a decoded frame pair
if (freeze_ && !have_frozen_) {
have_frozen_ = true;
} else {
- host_->notify(timestamp_, rgb_, depth_);
+ host_->notify(timestamp_, frame_);
}
return true;
}
diff --git a/components/rgbd-sources/src/sources/ftlfile/file_source.hpp b/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
index f8a26da37..839121f3a 100644
--- a/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
+++ b/components/rgbd-sources/src/sources/ftlfile/file_source.hpp
@@ -40,12 +40,15 @@ class FileSource : public detail::Source {
int cache_write_;
int sourceid_;
- ftl::codecs::Decoder *decoders_[2];
+ ftl::codecs::Decoder *decoders_[32]; // 32 is max video channels
bool realtime_;
bool freeze_;
bool have_frozen_;
+ ftl::codecs::Channels decode_channels_;
+ ftl::codecs::Channels available_channels_;
+
void _processCalibration(ftl::codecs::Packet &pkt);
void _processPose(ftl::codecs::Packet &pkt);
void _removeChannel(ftl::codecs::Channel channel);
diff --git a/components/rgbd-sources/src/sources/ftlfile/player.cpp b/components/rgbd-sources/src/sources/ftlfile/player.cpp
index cabbce642..9558c316c 100644
--- a/components/rgbd-sources/src/sources/ftlfile/player.cpp
+++ b/components/rgbd-sources/src/sources/ftlfile/player.cpp
@@ -34,7 +34,12 @@ Player::Player(std::istream &s) : stream_(&s), reader_(s) {
c->on("reverse", [this,c](const ftl::config::Event &e) {
reversed_ = c->value("reverse", false);
});
- }
+ } else {
+ looping_ = true;
+ paused_ = false;
+ speed_ = 1.0f;
+ reversed_ = false;
+ }
}
Player::~Player() {
diff --git a/components/rgbd-sources/src/sources/net/net.cpp b/components/rgbd-sources/src/sources/net/net.cpp
index c29ad5491..452647008 100644
--- a/components/rgbd-sources/src/sources/net/net.cpp
+++ b/components/rgbd-sources/src/sources/net/net.cpp
@@ -22,6 +22,7 @@ using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using std::tuple;
using ftl::codecs::Channel;
+using cv::cuda::GpuMat;
using ftl::codecs::codec_t;
// ===== NetFrameQueue =========================================================
@@ -92,6 +93,11 @@ void NetFrameQueue::freeFrame(NetFrame &f) {
// ===== NetSource =============================================================
+int64_t NetSource::last_msg_ = 0;
+float NetSource::req_bitrate_ = 0.0f;
+float NetSource::sample_count_ = 0.0f;
+MUTEX NetSource::msg_mtx_;
+
NetSource::NetSource(ftl::rgbd::Source *host)
: ftl::rgbd::detail::Source(host), active_(false), minB_(9), maxN_(1), adaptive_(0), queue_(3) {
@@ -266,10 +272,21 @@ void NetSource::_processPose(const ftl::codecs::Packet &pkt) {
}
}
-void NetSource::_checkDataRate(size_t tx_size, int64_t tx_latency) {
+void NetSource::_checkDataRate(size_t tx_size, int64_t tx_latency, int64_t ts) {
float actual_mbps = (float(tx_size) * 8.0f * (1000.0f / float(tx_latency))) / 1048576.0f;
float min_mbps = (float(tx_size) * 8.0f * (1000.0f / float(ftl::timer::getInterval()))) / 1048576.0f;
- if (actual_mbps < min_mbps) LOG(WARNING) << "Bitrate = " << actual_mbps << "Mbps, min required = " << min_mbps << "Mbps";
+ if (actual_mbps > 0.0f && actual_mbps < min_mbps) LOG(WARNING) << "Bitrate = " << actual_mbps << "Mbps, min required = " << min_mbps << "Mbps";
+
+ UNIQUE_LOCK(msg_mtx_,lk);
+ req_bitrate_ += float(tx_size) * 8.0f;
+ sample_count_ += 1.0f;
+
+ if (ts - last_msg_ >= 1000) {
+ LOG(INFO) << "Required Bitrate = " << (req_bitrate_ / float(ts - last_msg_) * 1000.0f / 1048576.0f) << "Mbps";
+ last_msg_ = ts;
+ req_bitrate_ = 0.0f;
+ sample_count_ = 0.0f;
+ }
}
void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
@@ -299,7 +316,7 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk
//LOG(INFO) << "PACKET: " << spkt.timestamp << ", " << (int)spkt.channel << ", " << (int)pkt.codec;
const cv::Size size = cv::Size(ftl::codecs::getWidth(pkt.definition), ftl::codecs::getHeight(pkt.definition));
- NetFrame &frame = queue_.getFrame(spkt.timestamp, size, CV_8UC3, (isFloatChannel(chan) ? CV_32FC1 : CV_8UC3));
+ NetFrame &frame = queue_.getFrame(spkt.timestamp, size, CV_8UC4, (isFloatChannel(chan) ? CV_32FC1 : CV_8UC4));
if (timestamp_ > 0 && frame.timestamp <= timestamp_) {
LOG(ERROR) << "Duplicate frame - " << frame.timestamp << " received=" << int(rchan) << " uri=" << uri_;
@@ -328,7 +345,7 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk
// Update frame statistics
frame.tx_size += pkt.data.size();
- frame.channel[channum].create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC3));
+ frame.channel[channum].create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4));
// Only decode if this channel is wanted.
if (rchan == Channel::Colour || rchan == chan) {
@@ -354,7 +371,7 @@ void NetSource::_recvPacket(short ttimeoff, const ftl::codecs::StreamPacket &spk
// TODO:(Nick) Decode directly into double buffer if no scaling
- _checkDataRate(pkt.data.size(), now-(spkt.timestamp+ttimeoff));
+ _checkDataRate(pkt.data.size(), now-(spkt.timestamp+ttimeoff), spkt.timestamp);
if (frame.chunk_count[channum] == frame.chunk_total[channum]) ++frame.channel_count;
@@ -375,7 +392,13 @@ void NetSource::_completeFrame(NetFrame &frame, int64_t latency) {
// Note: Not used currently
adaptive_ = abr_.selectBitrate(frame);
- host_->notify(frame.timestamp, frame.channel[0], frame.channel[1]);
+ frame_.reset();
+ cv::cuda::swap(frame_.create<GpuMat>(Channel::Colour), frame.channel[0]);
+
+ if (host_->getChannel() != Channel::None)
+ cv::cuda::swap(frame_.create<GpuMat>(host_->getChannel()), frame.channel[1]);
+
+ host_->notify(frame.timestamp, frame_);
queue_.freeFrame(frame);
N_--;
@@ -391,7 +414,7 @@ void NetSource::setPose(const Eigen::Matrix4d &pose) {
active_ = false;
}
} catch (...) {
-
+ LOG(ERROR) << "Exception when setting pose";
}
//Source::setPose(pose);
}
@@ -450,11 +473,11 @@ bool NetSource::compute(int n, int b) {
N_ = maxN_;
// Verify depth destination is of required type
- if (isFloatChannel(chan) && depth_.type() != CV_32F) {
+ /*if (isFloatChannel(chan)) {
depth_.create(cv::Size(params_.width, params_.height), CV_32FC1); // 0.0f
- } else if (!isFloatChannel(chan) && depth_.type() != CV_8UC3) {
- depth_.create(cv::Size(params_.width, params_.height), CV_8UC3); // cv::Scalar(0,0,0)
- }
+ } else if (!isFloatChannel(chan)) {
+ depth_.create(cv::Size(params_.width, params_.height), CV_8UC3); // FIXME: Use 8UC4
+ }*/
if (prev_chan_ != chan) {
host_->getNet()->send(peer_, "set_channel", *host_->get<string>("uri"), chan);
diff --git a/components/rgbd-sources/src/sources/net/net.hpp b/components/rgbd-sources/src/sources/net/net.hpp
index 515bb8a5f..299325643 100644
--- a/components/rgbd-sources/src/sources/net/net.hpp
+++ b/components/rgbd-sources/src/sources/net/net.hpp
@@ -63,6 +63,11 @@ class NetSource : public detail::Source {
ftl::rgbd::detail::ABRController abr_;
int last_bitrate_;
+ static int64_t last_msg_;
+ static float req_bitrate_;
+ static float sample_count_;
+ static MUTEX msg_mtx_;
+
//#ifdef HAVE_NVPIPE
//NvPipe *nv_channel1_decoder_;
//NvPipe *nv_channel2_decoder_;
@@ -89,7 +94,7 @@ class NetSource : public detail::Source {
void _processCalibration(const ftl::codecs::Packet &pkt);
void _processConfig(const ftl::codecs::Packet &pkt);
void _processPose(const ftl::codecs::Packet &pkt);
- void _checkDataRate(size_t tx_size, int64_t tx_latency);
+ void _checkDataRate(size_t tx_size, int64_t tx_latency, int64_t ts);
};
}
diff --git a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp
index 2b55356c8..68c1a9acf 100644
--- a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp
+++ b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp
@@ -5,6 +5,8 @@
using ftl::rgbd::detail::RealsenseSource;
using std::string;
+using ftl::codecs::Channel;
+using cv::cuda::GpuMat;
RealsenseSource::RealsenseSource(ftl::rgbd::Source *host)
: ftl::rgbd::detail::Source(host), align_to_depth_(RS2_STREAM_DEPTH) {
@@ -56,12 +58,11 @@ bool RealsenseSource::compute(int n, int b) {
cv::Mat tmp_depth(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes());
tmp_depth.convertTo(tmp_depth, CV_32FC1, scale_);
- depth_.upload(tmp_depth);
+ frame_.get<GpuMat>(Channel::Depth).upload(tmp_depth);
cv::Mat tmp_rgb(cv::Size(w, h), CV_8UC4, (void*)rscolour_.get_data(), cv::Mat::AUTO_STEP);
- rgb_.upload(tmp_rgb);
+ frame_.get<GpuMat>(Channel::Colour).upload(tmp_rgb);
- auto cb = host_->callback();
- if (cb) cb(timestamp_, rgb_, depth_);
+ host_->notify(timestamp_, frame_);
return true;
}
diff --git a/components/rgbd-sources/src/sources/snapshot/snapshot_source.cpp b/components/rgbd-sources/src/sources/snapshot/snapshot_source.cpp
index 61acdb9d8..95d8b9e07 100644
--- a/components/rgbd-sources/src/sources/snapshot/snapshot_source.cpp
+++ b/components/rgbd-sources/src/sources/snapshot/snapshot_source.cpp
@@ -9,6 +9,8 @@
using namespace ftl::rgbd;
using ftl::rgbd::detail::SnapshotSource;
+using cv::cuda::GpuMat;
+using ftl::codecs::Channel;
using std::string;
using std::vector;
@@ -64,13 +66,13 @@ bool SnapshotSource::compute(int n, int b) {
//snap_rgb_.copyTo(rgb_);
//snap_depth_.copyTo(depth_);
cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream_);
- rgb_.upload(snap_rgb_, cvstream);
- depth_.upload(snap_depth_, cvstream);
+ frame_.create<GpuMat>(Channel::Colour).upload(snap_rgb_, cvstream);
+ frame_.create<GpuMat>(Channel::Depth).upload(snap_depth_, cvstream);
cudaStreamSynchronize(stream_);
//auto cb = host_->callback();
//if (cb) cb(timestamp_, rgb_, depth_);
- host_->notify(timestamp_, rgb_, depth_);
+ host_->notify(timestamp_, frame_);
frame_idx_ = (frame_idx_ + 1) % snapshot_.getFramesCount();
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index e8eff732b..78bc1dba3 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -209,7 +209,17 @@ bool StereoVideoSource::compute(int n, int b) {
return false;
}
- if (chan == Channel::Depth) {
+ cv::cuda::GpuMat& left = frame.get<cv::cuda::GpuMat>(Channel::Left);
+ cv::cuda::GpuMat& right = frame.get<cv::cuda::GpuMat>(Channel::Right);
+
+ if (left.empty() || right.empty()) {
+ return false;
+ }
+
+ //stream_.waitForCompletion();
+ host_->notify(timestamp_, frame);
+
+ /*if (chan == Channel::Depth) {
// stereo algorithms assume input same size as output
bool resize = (depth_size_ != color_size_);
@@ -235,24 +245,16 @@ bool StereoVideoSource::compute(int n, int b) {
cv::cuda::swap(fullres_right_, right);
}
- host_->notify(timestamp_,
- frame.get<cv::cuda::GpuMat>(Channel::Left),
- frame.get<cv::cuda::GpuMat>(Channel::Depth));
+ host_->notify(timestamp_, frame);
} else if (chan == Channel::Right) {
- stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
- host_->notify(timestamp_,
- frame.get<cv::cuda::GpuMat>(Channel::Left),
- frame.get<cv::cuda::GpuMat>(Channel::Right));
+ stream_.waitForCompletion();
+ host_->notify(timestamp_, frame);
} else {
- stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
- auto &left = frame.get<cv::cuda::GpuMat>(Channel::Left);
- depth_.create(left.size(), left.type());
- host_->notify(timestamp_,
- frame.get<cv::cuda::GpuMat>(Channel::Left),
- depth_);
- }
+ stream_.waitForCompletion();
+ host_->notify(timestamp_, frame);
+ }*/
return true;
}
diff --git a/components/rgbd-sources/src/sources/virtual/virtual.cpp b/components/rgbd-sources/src/sources/virtual/virtual.cpp
index a1d1040c6..0827c0ffd 100644
--- a/components/rgbd-sources/src/sources/virtual/virtual.cpp
+++ b/components/rgbd-sources/src/sources/virtual/virtual.cpp
@@ -80,17 +80,18 @@ class VirtualImpl : public ftl::rgbd::detail::Source {
bool compute(int n, int b) override {
if (callback) {
- frame.reset();
+ frame_.reset();
+ bool goodFrame = false;
try {
- callback(frame);
+ goodFrame = callback(frame_);
} catch (std::exception &e) {
LOG(ERROR) << "Exception in render callback: " << e.what();
} catch (...) {
LOG(ERROR) << "Unknown exception in render callback";
}
- if (frame.hasChannel(Channel::Colour)) {
+ /*if (frame.hasChannel(Channel::Colour)) {
//frame.download(Channel::Colour);
cv::cuda::swap(frame.get<cv::cuda::GpuMat>(Channel::Colour), rgb_);
} else {
@@ -101,9 +102,9 @@ class VirtualImpl : public ftl::rgbd::detail::Source {
frame.hasChannel(host_->getChannel())) {
//frame.download(host_->getChannel());
cv::cuda::swap(frame.get<cv::cuda::GpuMat>(host_->getChannel()), depth_);
- }
+ }*/
- host_->notify(timestamp_, rgb_, depth_);
+ if (goodFrame) host_->notify(timestamp_, frame_);
}
return true;
}
@@ -114,8 +115,8 @@ class VirtualImpl : public ftl::rgbd::detail::Source {
bool isReady() override { return true; }
- std::function<void(ftl::rgbd::Frame &)> callback;
- ftl::rgbd::Frame frame;
+ std::function<bool(ftl::rgbd::Frame &)> callback;
+ //ftl::rgbd::Frame frame;
ftl::rgbd::Camera params_right_;
};
@@ -128,7 +129,7 @@ VirtualSource::~VirtualSource() {
}
-void VirtualSource::onRender(const std::function<void(ftl::rgbd::Frame &)> &f) {
+void VirtualSource::onRender(const std::function<bool(ftl::rgbd::Frame &)> &f) {
dynamic_cast<VirtualImpl*>(impl_)->callback = f;
}
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index a672c1bd1..6492d2ccb 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -49,7 +49,7 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
//group_.setFPS(value("fps", 20));
group_.setLatency(4);
- group_.setName("StreamGroup");
+ group_.setName("NetStreamer");
compress_level_ = value("compression", 1);
@@ -196,7 +196,7 @@ void Streamer::add(Source *src) {
s->hq_bitrate = ftl::codecs::kPresetBest;
}
- //LOG(INFO) << "RAW CALLBACK";
+ LOG(INFO) << "RAW CALLBACK";
_transmitPacket(s, spkt, pkt, Quality::Any);
});
}
@@ -227,7 +227,7 @@ void Streamer::add(ftl::rgbd::Group *grp) {
//group_.addSource(src);
src->addRawCallback([this,s](Source *src, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
- //LOG(INFO) << "RAW CALLBACK";
+ LOG(INFO) << "RAW CALLBACK2";
_transmitPacket(s, spkt, pkt, Quality::Any);
});
}
@@ -266,6 +266,7 @@ void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID
try {
mastertime = net_->call<int64_t>(peer, "__ping__");
} catch (...) {
+ LOG(ERROR) << "Ping failed";
// Reset time peer and remove timer
time_peer_ = ftl::UUID(0);
return false;
@@ -446,62 +447,107 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) {
if (!fs.sources[j]->isReady()) continue;
if (src->clientCount == 0) continue;
//if (fs.channel1[j].empty() || (fs.sources[j]->getChannel() != ftl::rgbd::kChanNone && fs.channel2[j].empty())) continue;
- if (!fs.frames[j].hasChannel(Channel::Colour) || !fs.frames[j].hasChannel(fs.sources[j]->getChannel())) continue;
+ if (!fs.frames[j].hasChannel(Channel::Colour) || !fs.frames[j].hasChannel(fs.sources[j]->getChannel())) {
+ LOG(WARNING) << "Missing required channel when streaming: " << (int)fs.sources[j]->getChannel();
+ continue;
+ }
- bool hasChan2 = fs.sources[j]->getChannel() != Channel::None;
+ bool hasChan2 = fs.sources[j]->getChannel() != Channel::None &&
+ fs.frames[j].hasChannel(fs.sources[j]->getChannel());
totalclients += src->clientCount;
// Do we need to do high quality encoding?
if (src->hq_count > 0) {
- if (!src->hq_encoder_c1) src->hq_encoder_c1 = ftl::codecs::allocateEncoder(
- definition_t::HD1080, hq_devices_, hq_codec_);
- if (!src->hq_encoder_c2 && hasChan2) src->hq_encoder_c2 = ftl::codecs::allocateEncoder(
- definition_t::HD1080, hq_devices_, hq_codec_);
+
+ auto chan = fs.sources[j]->getChannel();
// Do we have the resources to do a HQ encoding?
- if (src->hq_encoder_c1 && (!hasChan2 || src->hq_encoder_c2)) {
- auto *enc1 = src->hq_encoder_c1;
- auto *enc2 = src->hq_encoder_c2;
+ ///if (src->hq_encoder_c1 && (!hasChan2 || src->hq_encoder_c2)) {
+ //auto *enc1 = src->hq_encoder_c1;
+ //auto *enc2 = src->hq_encoder_c2;
MUTEX mtx;
std::condition_variable cv;
bool chan2done = false;
if (hasChan2) {
- ftl::pool.push([this,&fs,enc2,src,hasChan2,&cv,j,&chan2done](int id) {
- // TODO: Stagger the reset between nodes... random phasing
- if (fs.timestamp % (10*ftl::timer::getInterval()) == 0) enc2->reset();
-
- auto chan = fs.sources[j]->getChannel();
-
- try {
- enc2->encode(fs.frames[j].get<cv::cuda::GpuMat>(chan), src->hq_bitrate, [this,src,hasChan2,chan,&cv,&chan2done](const ftl::codecs::Packet &blk){
- _transmitPacket(src, blk, chan, hasChan2, Quality::High);
- chan2done = true;
- cv.notify_one();
+ if (fs.frames[j].getPackets(chan).size() == 0) {
+
+ // Allocate an encoder
+ if (!src->hq_encoder_c2) src->hq_encoder_c2 = ftl::codecs::allocateEncoder(
+ definition_t::HD1080, hq_devices_, hq_codec_);
+
+ auto *enc = src->hq_encoder_c2;
+
+ // Do we have an encoder to continue with?
+ if (enc) {
+ ftl::pool.push([this,&fs,enc,src,hasChan2,&cv,j,&chan2done](int id) {
+ // TODO: Stagger the reset between nodes... random phasing
+ if (fs.timestamp % (10*ftl::timer::getInterval()) == 0) enc->reset();
+
+ auto chan = fs.sources[j]->getChannel();
+
+ try {
+ enc->encode(fs.frames[j].get<cv::cuda::GpuMat>(chan), src->hq_bitrate, [this,src,hasChan2,chan,&cv,&chan2done](const ftl::codecs::Packet &blk){
+ _transmitPacket(src, blk, chan, hasChan2, Quality::High);
+ chan2done = true;
+ cv.notify_one();
+ });
+ } catch (std::exception &e) {
+ LOG(ERROR) << "Exception in encoder: " << e.what();
+ chan2done = true;
+ cv.notify_one();
+ }
});
- } catch (std::exception &e) {
- LOG(ERROR) << "Exception in encoder: " << e.what();
+ } else {
chan2done = true;
- cv.notify_one();
+ LOG(ERROR) << "Insufficient encoder resources";
}
- });
+ } else {
+ // Already have an encoding so send this
+ const auto &packets = fs.frames[j].getPackets(chan);
+ LOG(INFO) << "Send existing chan2 encoding: " << packets.size();
+ for (const auto &i : packets) {
+ _transmitPacket(src, i, chan, hasChan2, Quality::High);
+ }
+ }
} else {
- if (enc2) enc2->reset();
+ // No second channel requested...
+ if (src->hq_encoder_c2) src->hq_encoder_c2->reset();
chan2done = true;
}
+
- // TODO: Stagger the reset between nodes... random phasing
- if (fs.timestamp % (10*ftl::timer::getInterval()) == 0) enc1->reset();
- enc1->encode(fs.frames[j].get<cv::cuda::GpuMat>(Channel::Colour), src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
- _transmitPacket(src, blk, Channel::Colour, hasChan2, Quality::High);
- });
+ // TODO: Use ColourHighQuality if available
+ if (fs.frames[j].getPackets(Channel::Colour).size() == 0) {
+ if (!src->hq_encoder_c1) src->hq_encoder_c1 = ftl::codecs::allocateEncoder(
+ definition_t::HD1080, hq_devices_, hq_codec_);
+ auto *enc = src->hq_encoder_c1;
+
+ if (enc) {
+ // TODO: Stagger the reset between nodes... random phasing
+ if (fs.timestamp % (10*ftl::timer::getInterval()) == 0) enc->reset();
+ enc->encode(fs.frames[j].get<cv::cuda::GpuMat>(Channel::Colour), src->hq_bitrate, [this,src,hasChan2](const ftl::codecs::Packet &blk){
+ _transmitPacket(src, blk, Channel::Colour, hasChan2, Quality::High);
+ });
+ } else {
+ LOG(ERROR) << "Insufficient encoder resources";
+ }
+ } else {
+ const auto &packets = fs.frames[j].getPackets(Channel::Colour);
+ // FIXME: Adjust block number and total to match number of packets
+ // Also requires the receiver to decode in block number order.
+ LOG(INFO) << "Send existing encoding: " << packets.size();
+ for (const auto &i : packets) {
+ _transmitPacket(src, i, Channel::Colour, hasChan2, Quality::High);
+ }
+ }
// Ensure both channels have been completed.
std::unique_lock<std::mutex> lk(mtx);
cv.wait(lk, [&chan2done]{ return chan2done; });
- }
+ //}
}
// Do we need to do low quality encoding?
@@ -546,7 +592,7 @@ void Streamer::_process(ftl::rgbd::FrameSet &fs) {
// Make sure to unlock so clients can connect!
//lk.unlock();
slk.unlock();
- sleep_for(milliseconds(50));
+ //sleep_for(milliseconds(50));
} else _cleanUp();
}
@@ -563,6 +609,7 @@ void Streamer::_transmitPacket(StreamSource *src, const ftl::codecs::Packet &pkt
}
void Streamer::_transmitPacket(StreamSource *src, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt, Quality q) {
+ //LOG(INFO) << "TRANSMIT: " << spkt.timestamp;
// Lock to prevent clients being added / removed
//SHARED_LOCK(src->mutex,lk);
auto c = src->clients.begin();
--
GitLab