From 7c1756aee82dd151e24bb135f25fa2a022bf3bde Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Thu, 25 Jul 2019 11:16:53 +0300
Subject: [PATCH] Implements #58 rgbd groups
---
CMakeLists.txt | 1 +
applications/groupview/CMakeLists.txt | 11 ++
applications/groupview/src/main.cpp | 30 +++++
components/rgbd-sources/CMakeLists.txt | 1 +
.../rgbd-sources/include/ftl/rgbd/group.hpp | 50 ++++++++
.../rgbd-sources/include/ftl/rgbd/source.hpp | 5 +
components/rgbd-sources/src/group.cpp | 112 ++++++++++++++++++
components/rgbd-sources/src/net.cpp | 7 ++
8 files changed, 217 insertions(+)
create mode 100644 applications/groupview/CMakeLists.txt
create mode 100644 applications/groupview/src/main.cpp
create mode 100644 components/rgbd-sources/src/group.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0f0b1e56a..8db5a8886 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -176,6 +176,7 @@ add_subdirectory(components/net)
add_subdirectory(components/rgbd-sources)
add_subdirectory(components/control/cpp)
add_subdirectory(applications/calibration)
+add_subdirectory(applications/groupview)
if (BUILD_RENDERER)
add_subdirectory(components/renderers)
diff --git a/applications/groupview/CMakeLists.txt b/applications/groupview/CMakeLists.txt
new file mode 100644
index 000000000..25370e7b0
--- /dev/null
+++ b/applications/groupview/CMakeLists.txt
@@ -0,0 +1,11 @@
+set(GVIEWSRC
+ src/main.cpp
+)
+
+add_executable(ftl-view ${GVIEWSRC})
+
+target_include_directories(ftl-view PRIVATE src)
+
+target_link_libraries(ftl-view ftlcommon ftlnet ftlrgbd Threads::Threads ${OpenCV_LIBS})
+
+
diff --git a/applications/groupview/src/main.cpp b/applications/groupview/src/main.cpp
new file mode 100644
index 000000000..564d87d8b
--- /dev/null
+++ b/applications/groupview/src/main.cpp
@@ -0,0 +1,30 @@
+#include <loguru.hpp>
+#include <ftl/configuration.hpp>
+#include <ftl/net/universe.hpp>
+#include <ftl/rgbd/source.hpp>
+#include <ftl/rgbd/group.hpp>
+
+int main(int argc, char **argv) {
+ auto root = ftl::configure(argc, argv, "viewer_default");
+ ftl::net::Universe *net = ftl::create<ftl::net::Universe>(root, "net");
+
+ net->start();
+ net->waitConnections();
+
+ auto sources = ftl::createArray<ftl::rgbd::Source>(root, "sources", net);
+
+ ftl::rgbd::Group group;
+ for (auto s : sources) group.addSource(s);
+
+ group.sync([](const ftl::rgbd::FrameSet &fs) {
+ LOG(INFO) << "Complete set: " << fs.timestamp;
+ return true;
+ });
+
+ while (ftl::running) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ for (auto s : sources) s->grab();
+ }
+
+ return 0;
+}
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 1e6c15dc3..5983d1d94 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -8,6 +8,7 @@ set(RGBDSRC
src/net.cpp
src/streamer.cpp
src/colour.cpp
+ src/group.cpp
# src/algorithms/rtcensus.cpp
# src/algorithms/rtcensus_sgm.cpp
# src/algorithms/opencv_sgbm.cpp
diff --git a/components/rgbd-sources/include/ftl/rgbd/group.hpp b/components/rgbd-sources/include/ftl/rgbd/group.hpp
index e69de29bb..c2454a027 100644
--- a/components/rgbd-sources/include/ftl/rgbd/group.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/group.hpp
@@ -0,0 +1,50 @@
+#ifndef _FTL_RGBD_GROUP_HPP_
+#define _FTL_RGBD_GROUP_HPP_
+
+#include <ftl/threads.hpp>
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+namespace ftl {
+namespace rgbd {
+
+class Source;
+
+struct FrameSet {
+ int64_t timestamp;
+ std::vector<Source*> sources;
+ std::vector<cv::Mat> channel1;
+ std::vector<cv::Mat> channel2;
+ int count;
+ unsigned int mask;
+};
+
+static const size_t kFrameBufferSize = 10;
+
+class Group {
+ public:
+ Group();
+ ~Group();
+
+ void addSource(ftl::rgbd::Source *);
+
+ void sync(int N=-1, int B=-1);
+ void sync(std::function<bool(const FrameSet &)>);
+
+ bool getFrames(FrameSet &, bool complete=false);
+
+ private:
+ std::vector<FrameSet> framesets_;
+ std::vector<Source*> sources_;
+ size_t head_;
+ std::function<bool(const FrameSet &)> callback_;
+ MUTEX mutex_;
+
+ void _addFrameset(int64_t timestamp);
+};
+
+}
+}
+
+#endif // _FTL_RGBD_GROUP_HPP_
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 440f566ed..0b537aecd 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -191,6 +191,10 @@ class Source : public ftl::Configurable {
SHARED_MUTEX &mutex() { return mutex_; }
+ std::function<void(int64_t, const cv::Mat &, const cv::Mat &)> &callback() { return callback_; }
+ void setCallback(std::function<void(int64_t, const cv::Mat &, const cv::Mat &)> cb) { callback_ = cb; }
+
+
private:
detail::Source *impl_;
cv::Mat rgb_;
@@ -205,6 +209,7 @@ class Source : public ftl::Configurable {
channel_t channel_;
cudaStream_t stream_;
int64_t timestamp_;
+ std::function<void(int64_t, const cv::Mat &, const cv::Mat &)> callback_;
detail::Source *_createImplementation();
detail::Source *_createFileImpl(const ftl::URI &uri);
diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp
new file mode 100644
index 000000000..09cf14076
--- /dev/null
+++ b/components/rgbd-sources/src/group.cpp
@@ -0,0 +1,112 @@
+#include <ftl/rgbd/group.hpp>
+#include <ftl/rgbd/source.hpp>
+
+using ftl::rgbd::Group;
+using ftl::rgbd::Source;
+using ftl::rgbd::kFrameBufferSize;
+using std::vector;
+
+Group::Group() : framesets_(kFrameBufferSize), head_(0) {
+ framesets_[0].timestamp = -1;
+}
+
+Group::~Group() {
+
+}
+
+void Group::addSource(ftl::rgbd::Source *src) {
+ UNIQUE_LOCK(mutex_, lk);
+ size_t ix = sources_.size();
+ sources_.push_back(src);
+
+ src->setCallback([this,ix](int64_t timestamp, const cv::Mat &rgb, const cv::Mat &depth) {
+ if (timestamp == 0) return;
+ UNIQUE_LOCK(mutex_, lk);
+ if (timestamp > framesets_[head_].timestamp) {
+ // Add new frameset
+ _addFrameset(timestamp);
+ } 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) {
+ //LOG(INFO) << "Adding frame: " << ix << " for " << timestamp;
+ rgb.copyTo(fs.channel1[ix]);
+ depth.copyTo(fs.channel2[ix]);
+ ++fs.count;
+ fs.mask |= (1 << ix);
+
+ if (callback_ && fs.count == sources_.size()) {
+ //LOG(INFO) << "DOING CALLBACK";
+ if (callback_(fs)) {
+ //sources_[ix]->grab();
+ //LOG(INFO) << "GRAB";
+ }
+ }
+
+ return;
+ }
+ }
+ LOG(WARNING) << "Frame timestamp not found in buffer";
+ });
+}
+
+// TODO: This should be a callback
+// Callback returns true if it wishes to continue receiving frames.
+void Group::sync(int N, int B) {
+ for (auto s : sources_) {
+ s->grab(N,B);
+ }
+}
+
+void Group::sync(std::function<bool(const ftl::rgbd::FrameSet &)> cb) {
+ callback_ = cb;
+ sync(-1,-1);
+}
+
+bool Group::getFrames(ftl::rgbd::FrameSet &fs, bool complete) {
+ // Use oldest frameset or search back until first complete set is found?
+ if (complete) {
+ UNIQUE_LOCK(mutex_, lk);
+ // Search backwards to find match
+ for (size_t i=0; i<kFrameBufferSize; ++i) {
+ FrameSet &f = framesets_[(head_+kFrameBufferSize-i) % kFrameBufferSize];
+ if (f.count == sources_.size()) {
+ LOG(INFO) << "Complete set found";
+ fs = f; // FIXME: This needs to move or copy safely...
+ return true;
+ }
+ }
+ LOG(WARNING) << "No complete frame set found";
+ return false;
+ }
+
+ return false;
+}
+
+void Group::_addFrameset(int64_t timestamp) {
+ int count = (framesets_[head_].timestamp == -1) ? 1 : (timestamp - framesets_[head_].timestamp) / 40;
+ // Must make sure to also insert missing framesets
+ //LOG(INFO) << "Adding " << count << " framesets for " << timestamp << " head=" << framesets_[head_].timestamp;
+
+ //if (count > 10 || count < 1) return;
+
+ for (int i=0; i<count; ++i) {
+ int64_t lt = (framesets_[head_].timestamp == -1) ? timestamp-40 : framesets_[head_].timestamp;
+ head_ = (head_+1) % kFrameBufferSize;
+ framesets_[head_].timestamp = lt+40;
+ framesets_[head_].count = 0;
+ framesets_[head_].mask = 0;
+ framesets_[head_].channel1.resize(sources_.size());
+ framesets_[head_].channel2.resize(sources_.size());
+
+ for (auto s : sources_) framesets_[head_].sources.push_back(s);
+ }
+}
+
+
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index 25270084d..fb8ab4837 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -153,6 +153,13 @@ void NetSource::_recvChunk(int64_t frame, int chunk, bool delta, const vector<un
timestamp_ = current_frame_*40; // FIXME: Don't hardcode 40ms
current_frame_ = frame;
+
+ if (host_->callback()) {
+ //ftl::pool.push([this](id) {
+ // UNIQUE_LOCK(host_->mutex(),lk);
+ host_->callback()(timestamp_, rgb_, depth_);
+ //});
+ }
} else if (frame < current_frame_) {
LOG(WARNING) << "Chunk dropped";
if (chunk == 0) N_--;
--
GitLab