From 42a1bb195223111387b9f515d1023e62bdb964b0 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 25 Jul 2019 10:36:48 +0300
Subject: [PATCH] Working group code
---
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 | 6 +++
components/rgbd-sources/src/group.cpp | 47 ++++++++++++++-----
6 files changed, 85 insertions(+), 11 deletions(-)
create mode 100644 applications/groupview/CMakeLists.txt
create mode 100644 applications/groupview/src/main.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 683b42a2c..c2454a027 100644
--- a/components/rgbd-sources/include/ftl/rgbd/group.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/group.hpp
@@ -1,6 +1,9 @@
#ifndef _FTL_RGBD_GROUP_HPP_
#define _FTL_RGBD_GROUP_HPP_
+#include <ftl/threads.hpp>
+
+#include <opencv2/opencv.hpp>
#include <vector>
namespace ftl {
@@ -27,6 +30,7 @@ class 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);
@@ -34,6 +38,8 @@ class Group {
std::vector<FrameSet> framesets_;
std::vector<Source*> sources_;
size_t head_;
+ std::function<bool(const FrameSet &)> callback_;
+ MUTEX mutex_;
void _addFrameset(int64_t timestamp);
};
diff --git a/components/rgbd-sources/src/group.cpp b/components/rgbd-sources/src/group.cpp
index 40e1a806a..b675e40f0 100644
--- a/components/rgbd-sources/src/group.cpp
+++ b/components/rgbd-sources/src/group.cpp
@@ -6,8 +6,8 @@ using ftl::rgbd::Source;
using ftl::rgbd::kFrameBufferSize;
using std::vector;
-Group::Group() : framesets_(kFrameBufferSize), current_(0) {
-
+Group::Group() : framesets_(kFrameBufferSize), head_(0) {
+ framesets_[0].timestamp = -1;
}
Group::~Group() {
@@ -15,15 +15,17 @@ 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) {
- // TODO: Lock
+ if (timestamp == 0) return;
+ UNIQUE_LOCK(mutex_, lk);
if (timestamp > framesets_[head_].timestamp) {
// Add new frameset
_addFrameset(timestamp);
- } else (framesets_[(head_+1)%kFrameBufferSize].timestamp > timestamp) {
+ } else if (framesets_[(head_+1)%kFrameBufferSize].timestamp > timestamp) {
// Too old, just ditch it
LOG(WARNING) << "Received frame too old for buffer";
return;
@@ -31,12 +33,22 @@ void Group::addSource(ftl::rgbd::Source *src) {
// Search backwards to find match
for (size_t i=0; i<kFrameBufferSize; ++i) {
- FrameSet &fs = framesets_[(head+kFrameBufferSize-i) % kFrameBufferSize];
+ 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(fx.channel2[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;
}
}
@@ -52,13 +64,18 @@ void Group::sync(int N, int B) {
}
}
+void Group::sync(std::function<bool(const FrameSet &)> cb) {
+ callback_ = cb;
+ sync(-1,-1);
+}
+
bool Group::getFrames(FrameSet &fs, bool complete) {
// Use oldest frameset or search back until first complete set is found?
if (complete) {
- // TODO: Lock
+ UNIQUE_LOCK(mutex_, lk);
// Search backwards to find match
for (size_t i=0; i<kFrameBufferSize; ++i) {
- FrameSet &f = framesets_[(head+kFrameBufferSize-i) % kFrameBufferSize];
+ 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...
@@ -68,19 +85,27 @@ bool Group::getFrames(FrameSet &fs, bool complete) {
LOG(WARNING) << "No complete frame set found";
return false;
}
+
+ return false;
}
void Group::_addFrameset(int64_t timestamp) {
- int count = (timestamp - framesets_[head_].timestamp) / 40;
+ 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";
+ //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;
+ 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);
}
}
--
GitLab