Working group code

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 {
@@ -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);
 };

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