Add frame swapping and channel iterator

applications/reconstruct/src/main.cpp

@@ -92,23 +92,25 @@ static void run(ftl::Configurable *root) {
 		}
 	}
 
+	ftl::rgbd::FrameSet scene_A;  // Output of align process
+	ftl::rgbd::FrameSet scene_B;  // Input of render process
+
 	//ftl::voxhash::SceneRep *scene = ftl::create<ftl::voxhash::SceneRep>(root, "voxelhash");
 	ftl::rgbd::Streamer *stream = ftl::create<ftl::rgbd::Streamer>(root, "stream", net);
-	ftl::rgbd::Source *virt = ftl::create<ftl::rgbd::Source>(root, "virtual", net);
-	ftl::render::Splatter *splat = new ftl::render::Splatter();
+	ftl::rgbd::VirtualSource *virt = ftl::create<ftl::rgbd::VirtualSource>(root, "virtual", net);
+	ftl::render::Splatter *splat = ftl::create<ftl::render::Splatter>(root, "renderer", scene);
 	ftl::rgbd::Group group;
 	ftl::ILW align;
 
-	//auto virtimpl = new ftl::rgbd::VirtualSource(virt);
-	//virt->customImplementation(virtimpl);
-	//virtimpl->setScene(scene);
+	// Generate virtual camera render when requested by streamer
+	virt->onRender([splat,&scene_B](ftl::rgbd::Frame &out) {
+		splat->render(scene_B, out);
+	});
 	stream->add(virt);
 
 	for (size_t i=0; i<sources.size(); i++) {
 		Source *in = sources[i];
 		in->setChannel(Channel::Depth);
-		//stream->add(in);
-		//scene->addSource(in);
 		group.addSource(in);
 	}
 
@@ -116,87 +118,37 @@ static void run(ftl::Configurable *root) {
 
 	bool busy = false;
 
-	ftl::rgbd::FrameSet scene;
-
 	group.setName("ReconGroup");
 	group.sync([splat,virt,&busy,&slave](ftl::rgbd::FrameSet &fs) -> bool {
 		//cudaSetDevice(scene->getCUDADevice());
+
+		if (slave.isPaused()) return true;
 		
 		if (busy) {
 			LOG(INFO) << "Group frameset dropped: " << fs.timestamp;
 			return true;
 		}
 		busy = true;
-		//scene->nextFrame();
-
-		// Send all frames to GPU, block until done?
-		// TODO: Allow non-block and keep frameset locked until later
-		if (!slave.isPaused()) {
-			//scene->upload(fs);
-			for (auto &f : fs.frames) {
-				f.upload(Channel::Colour + Channel::Depth); // TODO: (Nick) Add scene stream
-			}
-		}
 
-		//int64_t ts = fs.timestamp;
+		// Swap the entire frameset to allow rapid return
+		fs.swapTo(scene_A);
 
-		ftl::pool.push([splat,virt,&busy,&fs,&slave](int id) {
+		ftl::pool.push([&scene_B,&scene_A,&busy,&slave](int id) {
 			//cudaSetDevice(scene->getCUDADevice());
 			// TODO: Release frameset here...
 			//cudaSafeCall(cudaStreamSynchronize(scene->getIntegrationStream()));
 
-			if (!slave.isPaused()) {
-				//scene->integrate();
-				//scene->garbage();
-				align.process(fs, scene);
-			}
+			// Send all frames to GPU, block until done?
+			scene_A.upload(Channel::Colour + Channel::Depth);  // TODO: (Nick) Add scene stream.
+			align.process(scene_A);
 
-			// Don't render here... but update timestamp.
-			splat->render(scene, virt); //, scene->getIntegrationStream());
+			// TODO: To use second GPU, could do a download, swap, device change,
+			// then upload to other device. Or some direct device-2-device copy.
+			scene_A.swapTo(scene_B);
 			busy = false;
 		});
 		return true;
 	});
-
-
-	/*int active = sources.size();
-	while (ftl::running) {
-		if (active == 0) {
-			LOG(INFO) << "Waiting for sources...";
-			sleep_for(milliseconds(1000));
-		}
-
-		active = 0;
-
-		if (!slave.isPaused()) {
-			// Mark voxels as cleared
-			scene->nextFrame();
-		
-			// Grab, upload frames and allocate voxel blocks
-			active = scene->upload();
-
-			// Make sure previous virtual camera frame has finished rendering
-			//stream->wait();
-			cudaSafeCall(cudaStreamSynchronize(scene->getIntegrationStream()));
-
-
-			// Merge new frames into the voxel structure
-			scene->integrate();
-
-			//LOG(INFO) << "Allocated: " << scene->getOccupiedCount();
-
-			// Remove any redundant voxels
-			scene->garbage();
-
-		} else {
-			active = 1;
-		}
-
-		splat->render(virt, scene->getIntegrationStream());
-
-		// Start virtual camera rendering and previous frame compression
-		stream->poll();
-	}*/
 }
 
 int main(int argc, char **argv) {

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

@@ -28,6 +28,21 @@ enum struct Channel : int {
 
 class Channels {
     public:
+
+	class iterator {
+		public:
+		iterator(const Channels &c, unsigned int ix) : channels_(c), ix_(ix) { }
+		iterator operator++();
+		iterator operator++(int junk);
+		inline ftl::rgbd::Channel operator*() { return static_cast<Channel>(static_cast<int>(ix_)); }
+		//ftl::rgbd::Channel operator->() { return ptr_; }
+		inline bool operator==(const iterator& rhs) { return ix_ == rhs.ix_; }
+		inline bool operator!=(const iterator& rhs) { return ix_ != rhs.ix_; }
+		private:
+		const Channels &channels_;
+		unsigned int ix_;
+	};
+
     inline Channels() { mask = 0; }
     inline explicit Channels(unsigned int m) { mask = m; }
     inline explicit Channels(Channel c) { mask = (c == Channel::None) ? 0 : 0x1 << static_cast<unsigned int>(c); }
@@ -48,6 +63,9 @@ class Channels {
         return mask & (0x1 << c);
     }
 
+	inline iterator begin() { return iterator(*this, 0); }
+	inline iterator end() { return iterator(*this, 32); }
+
     inline operator unsigned int() { return mask; }
     inline operator bool() { return mask > 0; }
     inline operator Channel() {
@@ -63,10 +81,19 @@ class Channels {
 
     static const size_t kMax = 32;
 
+	static Channels All();
+
     private:
     unsigned int mask;
 };
 
+inline Channels::iterator Channels::iterator::operator++() { Channels::iterator i = *this; while (++ix_ < 32 && !channels_.has(ix_)); return i; }
+inline Channels::iterator Channels::iterator::operator++(int junk) { while (++ix_ < 32 && !channels_.has(ix_)); return *this; }
+
+inline Channels Channels::All() {
+	return Channels(0xFFFFFFFFu);
+}
+
 static const Channels kNoChannels;
 static const Channels kAllChannels(0xFFFFFFFFu);
 

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

@@ -40,6 +40,13 @@ public:
 	void download(ftl::rgbd::Channels c, cv::cuda::Stream& stream=cv::cuda::Stream::Null());
 	void upload(ftl::rgbd::Channels c, cv::cuda::Stream& stream=cv::cuda::Stream::Null());
 
+	/**
+	 * Perform a buffer swap of the selected channels. This is intended to be
+	 * a copy from `this` to the passed frame object but by buffer swap
+	 * instead of memory copy, meaning `this` may become invalid afterwards.
+	 */
+	void swapTo(ftl::rgbd::Channels, Frame &);
+
 	/**
 	 * Create a channel with a given format. This will discard any existing
 	 * data associated with the channel and ensure all data structures and

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

@@ -24,6 +24,10 @@ struct FrameSet {
 	std::atomic<unsigned int> mask;		// Mask of all sources that contributed
 	bool stale;						// True if buffers have been invalidated
 	SHARED_MUTEX mtx;
+
+	void upload(ftl::rgbd::Channels, cudaStream_t stream=0);
+	void download(ftl::rgbd::Channels, cudaStream_t stream=0);
+	void swapTo(ftl::rgbd::FrameSet &);
 };
 
 }

components/rgbd-sources/src/frame.cpp

@@ -39,6 +39,28 @@ void Frame::upload(Channels c, cv::cuda::Stream& stream) {
 	}
 }
 
+void Frame::swapTo(ftl::rgbd::Channels channels, Frame &f) {
+	// For all channels in this frame object
+	for (auto c : channels_) {
+		// Should we swap this channel?
+		if (channels.has(c)) {
+			// Does 'f' have this channel?
+			if (!f.hasChannel(c)) {
+				// No, so create it first
+				// FIXME: Allocate the memory as well?
+				if (isCPU(c)) f.create<cv::Mat>(c);
+				else f.create<cv::cuda::GpuMat>(c);
+			}
+
+			auto &m1 = _get(c);
+			auto &m2 = f._get(c);
+
+			cv::swap(m1.host, m2.host);
+			cv::cuda::swap(m1.gpu, m2.gpu);
+		}
+	}
+}
+
 template<> cv::Mat& Frame::get(ftl::rgbd::Channel channel) {
 	if (channel == Channel::None) {
 		DLOG(WARNING) << "Cannot get the None channel from a Frame";

components/rgbd-sources/src/frameset.cpp

+#include <ftl/rgbd/frameset.hpp>
+
+using ftl::rgbd::FrameSet;
+using ftl::rgbd::Channels;
+using ftl::rgbd::Channel;
+
+void FrameSet::upload(ftl::rgbd::Channels c, cudaStream_t stream) {
+	for (auto &f : frames) {
+		f.upload(c, stream);
+	}
+}
+
+void FrameSet::download(ftl::rgbd::Channels c, cudaStream_t stream) {
+	for (auto &f : frames) {
+		f.download(c, stream);
+	}
+}
+
+void FrameSet::swapTo(ftl::rgbd::FrameSet &fs) {
+	UNIQUE_LOCK(fs.mtx, lk);
+
+	if (fs.frames.size() != frames.size()) {
+		// Assume "this" is correct and "fs" is not.
+		fs.sources.clear();
+		for (auto s : sources) fs.sources.push_back(s);
+		fs.frames.resize(frames.size());
+	}
+
+	fs.timestamp = timestamp;
+	fs.count = count;
+	fs.stale = stale;
+	fs.mask = mask;
+
+	for (size_t i=0; i<frames.size(); ++i) {
+		frames[i].swap(Channels::All(), fs.frames[i]);
+	}
+
+	stale = true;
+}

components/rgbd-sources/test/frame_unit.cpp

@@ -3,6 +3,7 @@
 
 using ftl::rgbd::Frame;
 using ftl::rgbd::Channel;
+using ftl::rgbd::Channels;
 using ftl::rgbd::Format;
 
 TEST_CASE("Frame::create() cpu mat", "") {
@@ -267,3 +268,16 @@ TEST_CASE("Frame::getTexture()", "") {
 		REQUIRE( !hadexception );
 	}
 }
+
+TEST_CASE("Frame::swapTo()", "") {
+	SECTION("Single host channel to empty frame") {
+		Frame f1;
+		Frame f2;
+
+		f1.create<cv::Mat>(Channel::Colour, Format<uchar3>(100,100));
+		f1.swapTo(Channels::All(), f2);
+
+		REQUIRE( f2.hasChannel(Channel::Colour) );
+		REQUIRE( (f2.get<cv::Mat>(Channel::Colour).cols == 100) );
+	}
+}