diff --git a/components/common/cpp/include/ftl/utility/vectorbuffer.hpp b/components/common/cpp/include/ftl/utility/vectorbuffer.hpp new file mode 100644 index 0000000000000000000000000000000000000000..0e33ff9356975750ab09040475b39392334cd0e0 --- /dev/null +++ b/components/common/cpp/include/ftl/utility/vectorbuffer.hpp @@ -0,0 +1,22 @@ +#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_ diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp index f4dc769870fe87ef4614afe37a1cc288554bd89b..eb59f93931d728ad048db4814a6cfbf01716da24 100644 --- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp +++ b/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); } @@ -313,6 +321,8 @@ public: */ template <typename T> const T& get(ftl::codecs::Channel channel) const; + template <typename T> void get(ftl::codecs::Channel channel, T ¶ms) const; + /** * Method to get reference to the channel content. * @param Channel type @@ -409,9 +419,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 +444,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 ¶ms) 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); diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp index a0b9be98b1c00ed45ae5f65953d41fbd8eabec12..4ed2b43512a68d0d2ac47e2ed987204db9c0e901 100644 --- a/components/rgbd-sources/src/frame.cpp +++ b/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) { diff --git a/components/rgbd-sources/test/frame_unit.cpp b/components/rgbd-sources/test/frame_unit.cpp index 656971ae47b0dc8f6abc489854e05c7598589295..1c7db3e498e2776a0fd75447d6573bfdf3255588 100644 --- a/components/rgbd-sources/test/frame_unit.cpp +++ b/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) ); + } +} diff --git a/components/streams/src/injectors.cpp b/components/streams/src/injectors.cpp index fdc27df8a898ec97ee5baa0a182faf7baca133ef..f75945576e349ff86f010b10aeddf800fb6119c2 100644 --- a/components/streams/src/injectors.cpp +++ b/components/streams/src/injectors.cpp @@ -1,18 +1,8 @@ #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); }