Merge branch 'feature/datachannel' into 'master'

Add data channels to frames

See merge request nicolas.pope/ftl!219

applications/gui/src/src_window.cpp

@@ -83,6 +83,12 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
 		// Request the channels required by current camera configuration
 		interceptor_->select(fs.id, _aggregateChannels());
 
+		/*if (fs.frames[0].hasChannel(Channel::Data)) {
+			int data = 0;
+			fs.frames[0].get(Channel::Data, data);
+			LOG(INFO) << "GOT DATA : " << data;
+		}*/
+
 		const auto *cstream = interceptor_;
 		_createDefaultCameras(fs, cstream->available(fs.id).has(Channel::Depth));
 

applications/reconstruct/src/main.cpp

@@ -188,6 +188,7 @@ static void run(ftl::Configurable *root) {
 			reconstr->setGenerator(gen);
 			reconstr->onFrameSet([sender,i](ftl::rgbd::FrameSet &fs) {
 				fs.id = i;
+				//fs.frames[0].create(Channel::Data, 44);
 				sender->post(fs);
 				return true;
 			});

components/common/cpp/include/ftl/utility/vectorbuffer.hpp

+#ifndef _FTL_UTILITY_VECTORBUFFER_HPP_
+#define _FTL_UTILITY_VECTORBUFFER_HPP_
+
+#include <vector>
+
+namespace ftl {
+namespace util {
+class FTLVectorBuffer {
+	public:
+	inline explicit FTLVectorBuffer(std::vector<unsigned char> &v) : vector_(v) {}
+
+	inline void write(const char *data, std::size_t size) {
+		vector_.insert(vector_.end(), (const unsigned char*)data, (const unsigned char*)data+size);
+	}
+
+	private:
+	std::vector<unsigned char> &vector_;
+};
+}
+}
+
+#endif  // _FTL_UTILITY_VECTORBUFFER_HPP_

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

@@ -13,6 +13,7 @@
 #include <ftl/rgbd/camera.hpp>
 #include <ftl/codecs/codecs.hpp>
 #include <ftl/codecs/packet.hpp>
+#include <ftl/utility/vectorbuffer.hpp>
 
 #include <ftl/cuda_common.hpp>
 
@@ -196,6 +197,12 @@ public:
 	 */
 	template <typename T> T &create(ftl::codecs::Channel c);
 
+	/**
+	 * Set the value of a channel. Some channels should not be modified via the
+	 * non-const get method, for example the data channels.
+	 */
+	template <typename T> void create(ftl::codecs::Channel channel, const T &value);
+
 	/**
 	 * Create a CUDA texture object for a channel. This version takes a format
 	 * argument to also create (or recreate) the associated GpuMat.
@@ -268,6 +275,7 @@ public:
 	inline bool hasChannel(ftl::codecs::Channel channel) const {
 		int c = static_cast<int>(channel);
 		if (c >= 64 && c <= 68) return true;
+		else if (c >= 2048) return data_channels_.has(channel);
 		else if (c >= 32) return false;
 		else return channels_.has(channel);
 	}
@@ -276,6 +284,9 @@ public:
 	 * Obtain a mask of all available channels in the frame.
 	 */
 	inline ftl::codecs::Channels<0> getChannels() const { return channels_; }
+	inline ftl::codecs::Channels<0> getVideoChannels() const { return channels_; }
+
+	inline ftl::codecs::Channels<2048> getDataChannels() const { return data_channels_; }
 
 	/**
 	 * Is the channel data currently located on GPU. This also returns false if
@@ -313,6 +324,8 @@ public:
 	 */
 	template <typename T> const T& get(ftl::codecs::Channel channel) const;
 
+	template <typename T> void get(ftl::codecs::Channel channel, T &params) const;
+
 	/**
 	 * Method to get reference to the channel content.
 	 * @param	Channel type
@@ -371,6 +384,13 @@ public:
 	 */
 	std::string getConfigString() const;
 
+	/**
+	 * Access the raw data channel vector object.
+	 */
+	const std::vector<unsigned char> &getRawData(ftl::codecs::Channel c) const;
+
+	void createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v);
+
 	/**
 	 * Wrapper to access a config property. If the property does not exist or
 	 * is not of the requested type then the returned optional is false.
@@ -409,9 +429,11 @@ private:
 	};
 
 	std::array<ChannelData, ftl::codecs::Channels<0>::kMax> data_;
+	std::unordered_map<int, std::vector<unsigned char>> data_data_;
 
 	ftl::codecs::Channels<0> channels_;	// Does it have a channel
 	ftl::codecs::Channels<0> gpu_;		// Is the channel on a GPU
+	ftl::codecs::Channels<2048> data_channels_;
 
 	// Persistent state
 	FrameState state_;
@@ -432,11 +454,34 @@ template<> cv::cuda::GpuMat& Frame::get(ftl::codecs::Channel channel);
 //template<> const Eigen::Matrix4d &Frame::get(ftl::codecs::Channel channel) const;
 template<> const ftl::rgbd::Camera &Frame::get(ftl::codecs::Channel channel) const;
 
+// Default data channel implementation
+template <typename T>
+void Frame::get(ftl::codecs::Channel channel, T &params) const {
+	if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel");
+	if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist");
+
+	const auto &i = data_data_.find(static_cast<int>(channel));
+	if (i == data_data_.end()) throw ftl::exception("Data channel does not exist");
+
+	auto unpacked = msgpack::unpack((const char*)(*i).second.data(), (*i).second.size());
+	unpacked.get().convert(params);
+}
+
 template <> cv::Mat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &);
 template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &);
 template <> cv::Mat &Frame::create(ftl::codecs::Channel c);
 template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c);
 
+template <typename T>
+void Frame::create(ftl::codecs::Channel channel, const T &value) {
+	if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel");
+
+	data_channels_ += channel;
+	data_data_.insert({static_cast<int>(channel),{}});
+	ftl::util::FTLVectorBuffer buf(data_data_[static_cast<int>(channel)]);
+	msgpack::pack(buf, value);
+}
+
 template <typename T>
 ftl::cuda::TextureObject<T> &Frame::getTexture(ftl::codecs::Channel c) {
 	if (!channels_.has(c)) throw ftl::exception(ftl::Formatter() << "Texture channel does not exist: " << (int)c);

components/rgbd-sources/src/frame.cpp

@@ -76,6 +76,7 @@ void Frame::reset() {
 	origin_ = nullptr;
 	channels_.clear();
 	gpu_.clear();
+	data_channels_.clear();
 	for (size_t i=0u; i<Channels<0>::kMax; ++i) {
 		data_[i].encoded.clear();
 	}
@@ -190,6 +191,10 @@ void Frame::swapTo(ftl::codecs::Channels<0> channels, Frame &f) {
 			}
 		}
 	}
+
+	f.data_data_ = std::move(data_data_);
+	f.data_channels_ = data_channels_;
+	data_channels_.clear();
 }
 
 void Frame::swapChannels(ftl::codecs::Channel a, ftl::codecs::Channel b) {
@@ -225,6 +230,9 @@ void Frame::copyTo(ftl::codecs::Channels<0> channels, Frame &f) {
 			m2.encoded = m1.encoded; //std::move(m1.encoded);  // TODO: Copy?
 		}
 	}
+
+	f.data_data_ = data_data_;
+	f.data_channels_ = data_channels_;
 }
 
 template<> cv::Mat& Frame::get(ftl::codecs::Channel channel) {
@@ -435,3 +443,15 @@ std::string ftl::rgbd::Frame::getConfigString() const {
 	return get<nlohmann::json>(ftl::codecs::Channel::Configuration).dump();
 }
 
+const std::vector<unsigned char> &ftl::rgbd::Frame::getRawData(ftl::codecs::Channel channel) const {
+	if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Non data channel");
+	if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist");
+
+	return data_data_.at(static_cast<int>(channel));
+}
+
+void ftl::rgbd::Frame::createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v) {
+	data_data_.insert({static_cast<int>(c), v});
+	data_channels_ += c;
+}
+

components/rgbd-sources/test/frame_unit.cpp

@@ -377,3 +377,125 @@ TEST_CASE("Frame::get() Pose", "") {
 		REQUIRE( pose1 == pose2 );
 	}
 }
+
+TEST_CASE("Frame::get() Data channel", "") {
+	SECTION("Get valid data") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		f.create(Channel::Data, val_in);
+		f.get(Channel::Data, val_out);
+
+		REQUIRE( std::get<0>(val_in) == std::get<0>(val_out) );
+		REQUIRE( std::get<1>(val_in) == std::get<1>(val_out) );
+	}
+
+	SECTION("Read from non existing channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		//f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Data, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Read from non data channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		//f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Colour, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Use non data channel") {
+		Frame f;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		decltype(val_in) val_out;
+
+		bool except = false;
+		try {
+			f.create(Channel::Colour, val_in);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Mix types") {
+		Frame f;
+		
+		std::string val_in = "Hello World";
+		std::tuple<int,float> val_out;
+
+		f.create(Channel::Data, val_in);
+
+		bool except = false;
+		try {
+			f.get(Channel::Data, val_out);
+		} catch (...) {
+			except = true;
+		}
+
+		REQUIRE( except );
+	}
+
+	SECTION("Has channel after create") {
+		Frame f;
+		
+		std::string val_in = "Hello World";
+
+		REQUIRE( !f.hasChannel(Channel::Data) );
+		f.create(Channel::Data, val_in);
+		REQUIRE( f.hasChannel(Channel::Data) );
+	}
+}
+
+TEST_CASE("Frame::swapTo() Data channel", "") {
+	SECTION("Swap valid data") {
+		Frame f1;
+		Frame f2;
+		
+		auto val_in = std::make_tuple(55,87.0f);
+		auto val_in2 = std::make_tuple(52,7.0f);
+		decltype(val_in) val_out;
+
+		f1.create(Channel::Data, val_in);
+
+		REQUIRE( f1.hasChannel(Channel::Data) );
+		REQUIRE( !f2.hasChannel(Channel::Data) );
+
+		f1.swapTo(Channels<0>::All(), f2);
+
+		REQUIRE( !f1.hasChannel(Channel::Data) );
+		REQUIRE( f2.hasChannel(Channel::Data) );
+
+		f1.create(Channel::Data, val_in2);
+		f2.get(Channel::Data, val_out);
+
+		REQUIRE( std::get<0>(val_in) == std::get<0>(val_out) );
+		REQUIRE( std::get<1>(val_in) == std::get<1>(val_out) );
+	}
+}

components/streams/src/injectors.cpp

 #include "injectors.hpp"
+#include <ftl/utility/vectorbuffer.hpp>
 
 using ftl::codecs::Channel;
-
-class VectorBuffer2 {
-	public:
-	inline explicit VectorBuffer2(std::vector<unsigned char> &v) : vector_(v) {}
-
-	inline void write(const char *data, std::size_t size) {
-		vector_.insert(vector_.end(), (const unsigned char*)data, (const unsigned char*)data+size);
-	}
-
-	private:
-	std::vector<unsigned char> &vector_;
-};
+using ftl::util::FTLVectorBuffer;
 
 void ftl::stream::injectCalibration(ftl::stream::Stream *stream, const ftl::rgbd::FrameSet &fs, int ix, bool right) {
 	ftl::stream::injectCalibration(stream, fs.frames[ix], fs.timestamp, ix, right);
@@ -38,7 +28,7 @@ void ftl::stream::injectConfig(ftl::stream::Stream *stream, const ftl::rgbd::Fra
 	pkt.frame_count = 1;
 	pkt.flags = 0;
 
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, fs.frames[ix].getConfigString());
 
 	stream->post(spkt, pkt);
@@ -62,7 +52,7 @@ void ftl::stream::injectPose(ftl::stream::Stream *stream, const ftl::rgbd::Frame
 
 	auto &pose = f.getPose();
 	std::vector<double> data(pose.data(), pose.data() + 4*4);
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, data);
 
 	stream->post(spkt, pkt);
@@ -88,7 +78,7 @@ void ftl::stream::injectCalibration(ftl::stream::Stream *stream, const ftl::rgbd
 	pkt.frame_count = 1;
 	pkt.flags = 0;
 
-	VectorBuffer2 buf(pkt.data);
+	FTLVectorBuffer buf(pkt.data);
 	msgpack::pack(buf, data);
 	stream->post(spkt, pkt);
 }

components/streams/src/receiver.cpp

@@ -103,7 +103,11 @@ void Receiver::setStream(ftl::stream::Stream *s) {
 			//return;
 		//}
 
-		if (channum >= 64) {
+		if (channum >= 2048) {
+			InternalStates &frame = _getFrame(spkt);
+			frame.frame.createRawData(spkt.channel, pkt.data);
+			return;
+		} else if (channum >= 64) {
 			for (int i=0; i<pkt.frame_count; ++i) {
 				InternalStates &frame = _getFrame(spkt,i);
 

components/streams/src/sender.cpp

@@ -141,6 +141,25 @@ void Sender::post(const ftl::rgbd::FrameSet &fs) {
 				available += c;
 			}
         }
+
+		// FIXME: Allow data channel selection rather than always send
+		for (auto c : frame.getDataChannels()) {
+			StreamPacket spkt;
+			spkt.version = 4;
+			spkt.timestamp = fs.timestamp;
+			spkt.streamID = 0; //fs.id;
+			spkt.frame_number = i;
+			spkt.channel = c;
+
+			ftl::codecs::Packet pkt;
+			pkt.codec = ftl::codecs::codec_t::MSGPACK;
+			pkt.definition = ftl::codecs::definition_t::Any;
+			pkt.frame_count = 1;
+			pkt.flags = 0;
+			pkt.bitrate = 0;
+			pkt.data = frame.getRawData(c);
+			stream_->post(spkt, pkt);
+		}
     }
 
 	for (auto c : available) {