Merge branch 'feature/58/group' into 'master'

Implements #58 rgbd groups

Closes #58

See merge request nicolas.pope/ftl!70

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)

applications/groupview/CMakeLists.txt

+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})
+
+

applications/groupview/src/main.cpp

+#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;
+}

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

components/rgbd-sources/include/ftl/rgbd/group.hpp

+#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_

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);

components/rgbd-sources/src/group.cpp

+#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);
+	}
+}
+
+

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_--;