diff --git a/examples/protonect/CMakeLists.txt b/examples/protonect/CMakeLists.txt index d9f0323e9..6961117e4 100644 --- a/examples/protonect/CMakeLists.txt +++ b/examples/protonect/CMakeLists.txt @@ -8,6 +8,7 @@ SET(MY_DIR ${libfreenect2_SOURCE_DIR}) OPTION(ENABLE_CXX11 "Enable C++11 support" OFF) OPTION(ENABLE_OPENCL "Enable OpenCL support" ON) OPTION(ENABLE_OPENGL "Enable OpenGL support" ON) +OPTION(ENABLE_VAAPI "Enable VA-API support" ON) IF(ENABLE_CXX11) INCLUDE(CheckCXXCompilerFlag) @@ -146,6 +147,25 @@ IF(ENABLE_OPENCL AND OPENCL_FOUND) ) ENDIF(ENABLE_OPENCL AND OPENCL_FOUND) +IF(ENABLE_VAAPI) + INCLUDE(FindPkgConfig) + PKG_CHECK_MODULES(VAAPI libva libva-drm) +ENDIF(ENABLE_VAAPI) + +IF(ENABLE_VAAPI AND VAAPI_FOUND) + SET(LIBFREENECT2_WITH_VAAPI_SUPPORT 1) + INCLUDE_DIRECTORIES(${VAAPI_INCLUDE_DIRS}) + + LIST(APPEND SOURCES + src/vaapi_jpeg_rgb_packet_processor.cpp + ) + + LIST(APPEND LIBRARIES + ${VAAPI_LIBRARIES} + jpeg + ) +ENDIF(ENABLE_VAAPI AND VAAPI_FOUND) + CONFIGURE_FILE("${MY_DIR}/include/libfreenect2/config.h.in" "${MY_DIR}/include/libfreenect2/config.h" @ONLY) GENERATE_RESOURCES(${RESOURCES_INC_FILE} ${MY_DIR} ${RESOURCES}) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index 2f3304118..5d3cc6e3f 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -87,13 +87,18 @@ int main(int argc, char *argv[]) libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir]; libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth]; - cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC3, rgb->data)); + int rgb_type = CV_8UC3; + if (rgb->bytes_per_pixel == 4) + { + rgb_type = CV_8UC4; + } + cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, rgb_type, rgb->data)); cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f); cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f); if (!registered) registered = new unsigned char[depth->height*depth->width*rgb->bytes_per_pixel]; registration->apply(rgb,depth,registered); - cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered)); + cv::imshow("registered", cv::Mat(depth->height, depth->width, rgb_type, registered)); int key = cv::waitKey(1); protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape diff --git a/examples/protonect/include/libfreenect2/async_packet_processor.h b/examples/protonect/include/libfreenect2/async_packet_processor.h index 807dbead7..275f979bb 100644 --- a/examples/protonect/include/libfreenect2/async_packet_processor.h +++ b/examples/protonect/include/libfreenect2/async_packet_processor.h @@ -76,6 +76,11 @@ class AsyncPacketProcessor : public PacketProcessor } packet_condition_.notify_one(); } + + virtual libfreenect2::DoubleBuffer *getPacketBuffer() + { + return processor_->getPacketBuffer(); + } private: PacketProcessorPtr processor_; bool current_packet_available_; diff --git a/examples/protonect/include/libfreenect2/config.h.in b/examples/protonect/include/libfreenect2/config.h.in index f3abf7d43..bf8f127cf 100644 --- a/examples/protonect/include/libfreenect2/config.h.in +++ b/examples/protonect/include/libfreenect2/config.h.in @@ -39,6 +39,8 @@ #define LIBFREENECT2_API #endif +#cmakedefine LIBFREENECT2_WITH_VAAPI_SUPPORT + #cmakedefine LIBFREENECT2_WITH_OPENGL_SUPPORT #cmakedefine LIBFREENECT2_WITH_OPENCL_SUPPORT diff --git a/examples/protonect/include/libfreenect2/depth_packet_processor.h b/examples/protonect/include/libfreenect2/depth_packet_processor.h index 2ce183b37..a6a83d1d7 100644 --- a/examples/protonect/include/libfreenect2/depth_packet_processor.h +++ b/examples/protonect/include/libfreenect2/depth_packet_processor.h @@ -40,6 +40,7 @@ namespace libfreenect2 struct LIBFREENECT2_API DepthPacket { uint32_t sequence; + uint32_t timestamp; unsigned char *buffer; size_t buffer_length; }; diff --git a/examples/protonect/include/libfreenect2/depth_packet_stream_parser.h b/examples/protonect/include/libfreenect2/depth_packet_stream_parser.h index 7773cadc4..4a8691872 100644 --- a/examples/protonect/include/libfreenect2/depth_packet_stream_parser.h +++ b/examples/protonect/include/libfreenect2/depth_packet_stream_parser.h @@ -63,7 +63,6 @@ class LIBFREENECT2_API DepthPacketStreamParser : public DataCallback private: libfreenect2::BaseDepthPacketProcessor *processor_; - libfreenect2::DoubleBuffer buffer_; libfreenect2::Buffer work_buffer_; uint32_t current_sequence_; diff --git a/examples/protonect/include/libfreenect2/double_buffer.h b/examples/protonect/include/libfreenect2/double_buffer.h index 49eb1f155..06a095ac3 100644 --- a/examples/protonect/include/libfreenect2/double_buffer.h +++ b/examples/protonect/include/libfreenect2/double_buffer.h @@ -47,14 +47,14 @@ class LIBFREENECT2_API DoubleBuffer DoubleBuffer(); virtual ~DoubleBuffer(); - void allocate(size_t buffer_size); + virtual void allocate(size_t buffer_size); void swap(); Buffer& front(); Buffer& back(); -private: +protected: Buffer buffer_[2]; unsigned char front_buffer_index_; diff --git a/examples/protonect/include/libfreenect2/frame_listener.hpp b/examples/protonect/include/libfreenect2/frame_listener.hpp index fbefaeb6b..091e5781a 100644 --- a/examples/protonect/include/libfreenect2/frame_listener.hpp +++ b/examples/protonect/include/libfreenect2/frame_listener.hpp @@ -28,6 +28,7 @@ #define FRAME_LISTENER_HPP_ #include +#include #include namespace libfreenect2 @@ -42,18 +43,23 @@ struct LIBFREENECT2_API Frame Depth = 4 }; + uint32_t timestamp; + uint32_t sequence; size_t width, height, bytes_per_pixel; unsigned char* data; - Frame(size_t width, size_t height, size_t bytes_per_pixel) : + Frame(size_t width, size_t height, size_t bytes_per_pixel, bool alloc = true) : width(width), height(height), - bytes_per_pixel(bytes_per_pixel) + bytes_per_pixel(bytes_per_pixel), + data(NULL) { + if (!alloc) + return; data = new unsigned char[width * height * bytes_per_pixel]; } - ~Frame() + virtual ~Frame() { delete[] data; } diff --git a/examples/protonect/include/libfreenect2/packet_pipeline.h b/examples/protonect/include/libfreenect2/packet_pipeline.h index b1bf467c2..1007ac370 100644 --- a/examples/protonect/include/libfreenect2/packet_pipeline.h +++ b/examples/protonect/include/libfreenect2/packet_pipeline.h @@ -63,6 +63,7 @@ class LIBFREENECT2_API BasePacketPipeline : public PacketPipeline virtual void initialize(); virtual DepthPacketProcessor *createDepthPacketProcessor() = 0; + virtual RgbPacketProcessor *createRgbPacketProcessor(); public: virtual ~BasePacketPipeline(); diff --git a/examples/protonect/include/libfreenect2/packet_processor.h b/examples/protonect/include/libfreenect2/packet_processor.h index ea21b9d8a..c8dd0073a 100644 --- a/examples/protonect/include/libfreenect2/packet_processor.h +++ b/examples/protonect/include/libfreenect2/packet_processor.h @@ -26,6 +26,7 @@ #ifndef PACKET_PROCESSOR_H_ #define PACKET_PROCESSOR_H_ +#include namespace libfreenect2 { @@ -38,6 +39,10 @@ class PacketProcessor virtual bool ready() { return true; } virtual void process(const PacketT &packet) = 0; + virtual libfreenect2::DoubleBuffer *getPacketBuffer() { return &default_buffer_; } + +protected: + libfreenect2::DoubleBuffer default_buffer_; }; template diff --git a/examples/protonect/include/libfreenect2/rgb_packet_processor.h b/examples/protonect/include/libfreenect2/rgb_packet_processor.h index 98decfb71..0a47f1e85 100644 --- a/examples/protonect/include/libfreenect2/rgb_packet_processor.h +++ b/examples/protonect/include/libfreenect2/rgb_packet_processor.h @@ -41,6 +41,7 @@ struct LIBFREENECT2_API RgbPacket { uint32_t sequence; + uint32_t timestamp; unsigned char *jpeg_buffer; size_t jpeg_buffer_length; }; @@ -80,5 +81,21 @@ class LIBFREENECT2_API TurboJpegRgbPacketProcessor : public RgbPacketProcessor TurboJpegRgbPacketProcessorImpl *impl_; }; +#ifdef LIBFREENECT2_WITH_VAAPI_SUPPORT +class VaapiJpegRgbPacketProcessorImpl; + +class VaapiJpegRgbPacketProcessor : public RgbPacketProcessor +{ +public: + VaapiJpegRgbPacketProcessor(); + virtual ~VaapiJpegRgbPacketProcessor(); + virtual libfreenect2::DoubleBuffer *getPacketBuffer(); +protected: + virtual void process(const libfreenect2::RgbPacket &packet); +private: + VaapiJpegRgbPacketProcessorImpl *impl_; +}; +#endif //LIBFREENECT2_WITH_VAAPI_SUPPORT + } /* namespace libfreenect2 */ #endif /* RGB_PACKET_PROCESSOR_H_ */ diff --git a/examples/protonect/include/libfreenect2/rgb_packet_stream_parser.h b/examples/protonect/include/libfreenect2/rgb_packet_stream_parser.h index ac1cd0caf..0a5046b2f 100644 --- a/examples/protonect/include/libfreenect2/rgb_packet_stream_parser.h +++ b/examples/protonect/include/libfreenect2/rgb_packet_stream_parser.h @@ -48,7 +48,6 @@ class LIBFREENECT2_API RgbPacketStreamParser : public DataCallback virtual void onDataReceived(unsigned char* buffer, size_t length); private: - libfreenect2::DoubleBuffer buffer_; BaseRgbPacketProcessor *processor_; }; diff --git a/examples/protonect/src/cpu_depth_packet_processor.cpp b/examples/protonect/src/cpu_depth_packet_processor.cpp index 1ddae71f7..765f68a53 100644 --- a/examples/protonect/src/cpu_depth_packet_processor.cpp +++ b/examples/protonect/src/cpu_depth_packet_processor.cpp @@ -743,6 +743,11 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) impl_->startTiming(); + impl_->ir_frame->timestamp = packet.timestamp; + impl_->depth_frame->timestamp = packet.timestamp; + impl_->ir_frame->sequence = packet.sequence; + impl_->depth_frame->sequence = packet.sequence; + cv::Mat m = cv::Mat::zeros(424, 512, CV_32FC(9)), m_filtered = cv::Mat::zeros(424, 512, CV_32FC(9)), m_max_edge_test = cv::Mat::ones(424, 512, CV_8UC1); float *m_ptr = m.ptr(); diff --git a/examples/protonect/src/depth_packet_stream_parser.cpp b/examples/protonect/src/depth_packet_stream_parser.cpp index be26faa3c..53f4df268 100644 --- a/examples/protonect/src/depth_packet_stream_parser.cpp +++ b/examples/protonect/src/depth_packet_stream_parser.cpp @@ -27,7 +27,6 @@ #include #include #include -#include namespace libfreenect2 { @@ -39,12 +38,8 @@ DepthPacketStreamParser::DepthPacketStreamParser() : { size_t single_image = 512*424*11/8; - buffer_.allocate((single_image) * 10); - buffer_.front().length = buffer_.front().capacity; - buffer_.back().length = buffer_.back().capacity; - - work_buffer_.data = new unsigned char[single_image * 2]; - work_buffer_.capacity = single_image * 2; + work_buffer_.data = new unsigned char[single_image]; + work_buffer_.capacity = single_image; work_buffer_.length = 0; } @@ -55,47 +50,50 @@ DepthPacketStreamParser::~DepthPacketStreamParser() void DepthPacketStreamParser::setPacketProcessor(libfreenect2::BaseDepthPacketProcessor *processor) { processor_ = (processor != 0) ? processor : noopProcessor(); + + DoubleBuffer &buffer_ = *processor_->getPacketBuffer(); + size_t single_image = 512*424*11/8; + + buffer_.allocate((single_image) * 10); + buffer_.front().length = buffer_.front().capacity; + buffer_.back().length = buffer_.back().capacity; } void DepthPacketStreamParser::onDataReceived(unsigned char* buffer, size_t in_length) { - // TODO: simplify this crap (so code, such unreadable, wow ;) + DoubleBuffer &buffer_ = *processor_->getPacketBuffer(); Buffer &wb = work_buffer_; - size_t in_offset = 0; - - while(in_offset < in_length) + if(in_length == 0) + { + //synchronize to subpacket boundary + wb.length = 0; + } + else { - unsigned char *ptr_in = buffer + in_offset, *ptr_out = wb.data + wb.length; DepthSubPacketFooter *footer = 0; bool footer_found = false; - size_t max_length = std::min(wb.capacity - wb.length, in_length - 8); - - for(; in_offset < max_length; ++in_offset) + if(wb.length + in_length == wb.capacity + sizeof(DepthSubPacketFooter)) { - footer = reinterpret_cast(ptr_in); - - if(footer->magic0 == 0x0 && footer->magic1 == 0x9) - { - footer_found = true; - break; - } + in_length -= sizeof(DepthSubPacketFooter); + footer = reinterpret_cast(&buffer[in_length]); + footer_found = true; + } - *ptr_out = *ptr_in; - ++ptr_in; - ++ptr_out; + if(wb.length + in_length > wb.capacity) + { + std::cerr << "[DepthPacketStreamParser::onDataReceived] subpacket too large" << std::endl; + wb.length = 0; + return; } - wb.length = ptr_out - wb.data; + memcpy(wb.data + wb.length, buffer, in_length); + wb.length += in_length; if(footer_found) { - if((in_length - in_offset) < sizeof(DepthSubPacketFooter)) - { - std::cerr << "[DepthPacketStreamParser::onDataReceived] incomplete footer detected!" << std::endl; - } - else if(footer->length > wb.length) + if(footer->length != wb.length) { std::cerr << "[DepthPacketStreamParser::onDataReceived] image data too short!" << std::endl; } @@ -111,6 +109,7 @@ void DepthPacketStreamParser::onDataReceived(unsigned char* buffer, size_t in_le DepthPacket packet; packet.sequence = current_sequence_; + packet.timestamp = footer->timestamp; packet.buffer = buffer_.back().data; packet.buffer_length = buffer_.back().length; @@ -118,7 +117,7 @@ void DepthPacketStreamParser::onDataReceived(unsigned char* buffer, size_t in_le } else { - //std::cerr << "[DepthPacketStreamParser::handleNewData] skipping depth packet!" << std::endl; + std::cerr << "[DepthPacketStreamParser::onDataReceived] skipping depth packet" << std::endl; } } else @@ -147,32 +146,6 @@ void DepthPacketStreamParser::onDataReceived(unsigned char* buffer, size_t in_le // reset working buffer wb.length = 0; - // skip header - in_offset += sizeof(DepthSubPacketFooter); - } - else - { - if((wb.length + 8) >= wb.capacity) - { - std::cerr << "[DepthPacketStreamParser::onDataReceived] working buffer full, resetting it!" << std::endl; - wb.length = 0; - ptr_out = wb.data; - } - - // copy remaining 8 bytes - if((in_length - in_offset) != 8) - { - std::cerr << "[DepthPacketStreamParser::onDataReceived] remaining data should be 8 bytes, but is " << (in_length - in_offset) << std::endl; - } - - for(; in_offset < in_length; ++in_offset) - { - *ptr_out = *ptr_in; - ++ptr_in; - ++ptr_out; - } - - wb.length = ptr_out - wb.data; } } } diff --git a/examples/protonect/src/opencl_depth_packet_processor.cpp b/examples/protonect/src/opencl_depth_packet_processor.cpp index 2f405e247..62bd850d3 100644 --- a/examples/protonect/src/opencl_depth_packet_processor.cpp +++ b/examples/protonect/src/opencl_depth_packet_processor.cpp @@ -653,6 +653,11 @@ void OpenCLDepthPacketProcessor::process(const DepthPacket &packet) impl_->startTiming(); + impl_->ir_frame->timestamp = packet.timestamp; + impl_->depth_frame->timestamp = packet.timestamp; + impl_->ir_frame->sequence = packet.sequence; + impl_->depth_frame->sequence = packet.sequence; + impl_->run(packet); impl_->stopTiming(); diff --git a/examples/protonect/src/opengl_depth_packet_processor.cpp b/examples/protonect/src/opengl_depth_packet_processor.cpp index b67362e3f..addd2ff1d 100644 --- a/examples/protonect/src/opengl_depth_packet_processor.cpp +++ b/examples/protonect/src/opengl_depth_packet_processor.cpp @@ -940,6 +940,11 @@ void OpenGLDepthPacketProcessor::process(const DepthPacket &packet) if(has_listener) { + ir->timestamp = packet.timestamp; + depth->timestamp = packet.timestamp; + ir->sequence = packet.sequence; + depth->sequence = packet.sequence; + if(!this->listener_->onNewFrame(Frame::Ir, ir)) { delete ir; diff --git a/examples/protonect/src/packet_pipeline.cpp b/examples/protonect/src/packet_pipeline.cpp index 1c66f66b6..bbab9e448 100644 --- a/examples/protonect/src/packet_pipeline.cpp +++ b/examples/protonect/src/packet_pipeline.cpp @@ -34,12 +34,20 @@ PacketPipeline::~PacketPipeline() { } +RgbPacketProcessor *BasePacketPipeline::createRgbPacketProcessor() +{ +#ifdef LIBFREENECT2_WITH_VAAPI_SUPPORT + return new VaapiJpegRgbPacketProcessor(); +#endif + return new TurboJpegRgbPacketProcessor(); +} + void BasePacketPipeline::initialize() { rgb_parser_ = new RgbPacketStreamParser(); depth_parser_ = new DepthPacketStreamParser(); - rgb_processor_ = new TurboJpegRgbPacketProcessor(); + rgb_processor_ = createRgbPacketProcessor(); depth_processor_ = createDepthPacketProcessor(); async_rgb_processor_ = new AsyncPacketProcessor(rgb_processor_); diff --git a/examples/protonect/src/rgb_packet_stream_parser.cpp b/examples/protonect/src/rgb_packet_stream_parser.cpp index 2fb0fdfc0..76b2a7517 100644 --- a/examples/protonect/src/rgb_packet_stream_parser.cpp +++ b/examples/protonect/src/rgb_packet_stream_parser.cpp @@ -36,15 +36,32 @@ namespace libfreenect2 LIBFREENECT2_PACK(struct RawRgbPacket { uint32_t sequence; - uint32_t unknown0; + uint32_t magic_header; // is 'BBBB' equal 0x42424242 unsigned char jpeg_buffer[0]; }); +// starting from JPEG EOI: 0xff 0xd9 +// char pad_0xa5[]; //0-3 bytes alignment of 0xa5 +// char filler[filler_length] = "ZZZZ..."; +LIBFREENECT2_PACK(struct RgbPacketFooter { + uint32_t magic_header; // is '9999' equal 0x39393939 + uint32_t sequence; + uint32_t filler_length; + uint32_t unknown1; // seems 0 always + uint32_t unknown2; // seems 0 always + uint32_t timestamp; + float exposure; // ? ranges from 0.5 to about 60.0 with powerfull light at camera or totally covered + float gain; // ? ranges from 1.0 when camera is clear to 1.5 when camera is covered. + uint32_t magic_footer; // is 'BBBB' equal 0x42424242 + uint32_t packet_size; + float unknown3; // seems 1.0f always + uint32_t unknown4[3]; // seems to be 0 all the time. +}); + RgbPacketStreamParser::RgbPacketStreamParser() : processor_(noopProcessor()) { - buffer_.allocate(1920*1080*3+sizeof(RgbPacket)); } RgbPacketStreamParser::~RgbPacketStreamParser() @@ -54,10 +71,14 @@ RgbPacketStreamParser::~RgbPacketStreamParser() void RgbPacketStreamParser::setPacketProcessor(BaseRgbPacketProcessor *processor) { processor_ = (processor != 0) ? processor : noopProcessor(); + + DoubleBuffer &buffer_ = *processor_->getPacketBuffer(); + buffer_.allocate(1920*1080*3+sizeof(RgbPacket)); } void RgbPacketStreamParser::onDataReceived(unsigned char* buffer, size_t length) { + DoubleBuffer &buffer_ = *processor_->getPacketBuffer(); Buffer &fb = buffer_.front(); // package containing data @@ -71,12 +92,54 @@ void RgbPacketStreamParser::onDataReceived(unsigned char* buffer, size_t length) else { std::cerr << "[RgbPacketStreamParser::onDataReceived] buffer overflow!" << std::endl; + fb.length = 0; + return; } - // not full transfer buffer and we already have some data -> signals end of rgb image packet - // TODO: better method, is unknown0 a magic? detect JPEG magic? - if(length < 0x4000 && fb.length > sizeof(RgbPacket)) + // not enough data to do anything + if (fb.length <= sizeof(RawRgbPacket) + sizeof(RgbPacketFooter)) + return; + + RgbPacketFooter* footer = reinterpret_cast(&fb.data[fb.length - sizeof(RgbPacketFooter)]); + + if (footer->magic_header == 0x39393939 && footer->magic_footer == 0x42424242) { + RawRgbPacket *raw_packet = reinterpret_cast(fb.data); + + if (fb.length != footer->packet_size || raw_packet->sequence != footer->sequence) + { + std::cerr << "[RgbPacketStreamParser::onDataReceived] packetsize or sequence doesn't match!" << std::endl; + fb.length = 0; + return; + } + + if (fb.length - sizeof(RawRgbPacket) - sizeof(RgbPacketFooter) < footer->filler_length) + { + std::cerr << "[RgbPacketStreamParser::onDataReceived] not enough space for packet filler!" << std::endl; + fb.length = 0; + return; + } + + size_t jpeg_length = 0; + //check for JPEG EOI 0xff 0xd9 within 0 to 3 alignment bytes + size_t length_no_filler = fb.length - sizeof(RawRgbPacket) - sizeof(RgbPacketFooter) - footer->filler_length; + for (size_t i = 0; i < 4; i++) + { + if (length_no_filler < i + 2) + break; + size_t eoi = length_no_filler - i; + + if (raw_packet->jpeg_buffer[eoi - 2] == 0xff && raw_packet->jpeg_buffer[eoi - 1] == 0xd9) + jpeg_length = eoi; + } + + if (jpeg_length == 0) + { + std::cerr << "[RgbPacketStreamParser::onDataReceived] no JPEG detected!" << std::endl; + fb.length = 0; + return; + } + // can the processor handle the next image? if(processor_->ready()) { @@ -86,8 +149,9 @@ void RgbPacketStreamParser::onDataReceived(unsigned char* buffer, size_t length) RawRgbPacket *raw_packet = reinterpret_cast(bb.data); RgbPacket rgb_packet; rgb_packet.sequence = raw_packet->sequence; + rgb_packet.timestamp = footer->timestamp; rgb_packet.jpeg_buffer = raw_packet->jpeg_buffer; - rgb_packet.jpeg_buffer_length = bb.length - sizeof(RawRgbPacket); + rgb_packet.jpeg_buffer_length = jpeg_length; // call the processor processor_->process(rgb_packet); diff --git a/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp b/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp index ebc0d6a63..f2a92d9f2 100644 --- a/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp +++ b/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp @@ -112,6 +112,9 @@ void TurboJpegRgbPacketProcessor::process(const RgbPacket &packet) { impl_->startTiming(); + impl_->frame->timestamp = packet.timestamp; + impl_->frame->sequence = packet.sequence; + int r = tjDecompress2(impl_->decompressor, packet.jpeg_buffer, packet.jpeg_buffer_length, impl_->frame->data, 1920, 1920 * tjPixelSize[TJPF_BGR], 1080, TJPF_BGR, 0); if(r == 0) diff --git a/examples/protonect/src/vaapi_jpeg_rgb_packet_processor.cpp b/examples/protonect/src/vaapi_jpeg_rgb_packet_processor.cpp new file mode 100644 index 000000000..c3be6e352 --- /dev/null +++ b/examples/protonect/src/vaapi_jpeg_rgb_packet_processor.cpp @@ -0,0 +1,469 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +static inline VAStatus vaSafeCall(VAStatus err) +{ + if (err != VA_STATUS_SUCCESS) + throw std::runtime_error(vaErrorStr(err)); + return err; +} + +namespace libfreenect2 +{ + +struct VaapiFrame: Frame +{ + VADisplay display; + VAImage image; + + VaapiFrame(VADisplay display, size_t width, size_t height, size_t bytes_per_pixel): + Frame(width, height, bytes_per_pixel, false), + display(display) + { + /* Create image */ + VAImageFormat format = {0}; + format.fourcc = VA_FOURCC_BGRX; + format.byte_order = VA_LSB_FIRST; + format.bits_per_pixel = bytes_per_pixel; + format.depth = 8; + + vaSafeCall(vaCreateImage(display, &format, width, height, &image)); + } + + void draw(VASurfaceID surface) + { + if (data != NULL) + vaSafeCall(vaUnmapBuffer(display, image.buf)); + vaSafeCall(vaGetImage(display, surface, 0, 0, width, height, image.image_id)); + vaSafeCall(vaMapBuffer(display, image.buf, (void**)&data)); + } + + ~VaapiFrame() + { + if (data != NULL) + { + vaSafeCall(vaUnmapBuffer(display, image.buf)); + data = NULL; + } + vaSafeCall(vaDestroyImage(display, image.image_id)); + } +}; + +class VaapiDoubleBuffer: public DoubleBuffer +{ +private: + VADisplay display; + VAContextID context; + VABufferID buffers[2]; + +public: + VaapiDoubleBuffer(VADisplay display, VAContextID context): + DoubleBuffer(), + display(display), context(context) + { + buffers[0] = VA_INVALID_ID; + buffers[1] = VA_INVALID_ID; + } + + virtual ~VaapiDoubleBuffer() + { + if(buffers[0] != VA_INVALID_ID) + { + vaSafeCall(vaUnmapBuffer(display, buffers[0])); + vaSafeCall(vaDestroyBuffer(display, buffers[0])); + } + if(buffers[1] != VA_INVALID_ID) + { + vaSafeCall(vaUnmapBuffer(display, buffers[1])); + vaSafeCall(vaDestroyBuffer(display, buffers[1])); + } + } + + virtual void allocate(size_t size) + { + vaSafeCall(vaCreateBuffer(display, context, VASliceDataBufferType, size, 1, NULL, &buffers[0])); + vaSafeCall(vaCreateBuffer(display, context, VASliceDataBufferType, size, 1, NULL, &buffers[1])); + + buffer_[0].capacity = size; + buffer_[0].length = 0; + vaSafeCall(vaMapBuffer(display, buffers[0], (void**)&buffer_[0].data)); + + buffer_[1].capacity = size; + buffer_[1].length = 0; + vaSafeCall(vaMapBuffer(display, buffers[1], (void**)&buffer_[1].data)); + } + + VABufferID back_id() + { + return buffers[(front_buffer_index_ + 1) & 1]; + } +}; + +class VaapiJpegRgbPacketProcessorImpl +{ +public: + int drm_fd; + VADisplay display; + VAConfigID config; + VASurfaceID surface; + VAContextID context; + VABufferID pic_param_buf; + VABufferID iq_buf; + VABufferID huff_buf; + VABufferID slice_param_buf; + bool jpeg_first_packet; + size_t jpeg_header_size; + + struct jpeg_decompress_struct dinfo; + struct jpeg_error_mgr jerr; + + static const int WIDTH = 1920; + static const int HEIGHT = 1080; + + VaapiFrame *frame; + + VaapiDoubleBuffer *packet_buffer; + + double timing_acc; + double timing_acc_n; + + double timing_current_start; + + VaapiJpegRgbPacketProcessorImpl() + { + dinfo.err = jpeg_std_error(&jerr); + jpeg_create_decompress(&dinfo); + + initializeVaapi(); + + newFrame(); + + packet_buffer = new VaapiDoubleBuffer(display, context); + + timing_acc = 0.0; + timing_acc_n = 0.0; + timing_current_start = 0.0; + + jpeg_first_packet = true; + } + + ~VaapiJpegRgbPacketProcessorImpl() + { + delete packet_buffer; + delete frame; + if (!jpeg_first_packet) + { + vaSafeCall(vaDestroyBuffer(display, pic_param_buf)); + vaSafeCall(vaDestroyBuffer(display, iq_buf)); + vaSafeCall(vaDestroyBuffer(display, huff_buf)); + vaSafeCall(vaDestroyBuffer(display, slice_param_buf)); + } + vaSafeCall(vaDestroyContext(display, context)); + vaSafeCall(vaDestroySurfaces(display, &surface, 1)); + vaSafeCall(vaDestroyConfig(display, config)); + vaSafeCall(vaTerminate(display)); + if (drm_fd >= 0) + close(drm_fd); + jpeg_destroy_decompress(&dinfo); + } + + void newFrame() + { + frame = new VaapiFrame(display, 1920, 1080, 4); + } + + void startTiming() + { + timing_current_start = cv::getTickCount(); + } + + void stopTiming() + { + timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); + timing_acc_n += 1.0; + + if(timing_acc_n >= 100.0) + { + double avg = (timing_acc / timing_acc_n); + std::cout << "[VaapiJpegRgbPacketProcessor] avg. time: " << (avg * 1000) << "ms -> ~" << (1.0/avg) << "Hz" << std::endl; + timing_acc = 0.0; + timing_acc_n = 0.0; + } + } + + void initializeVaapi() + { + /* Open display */ + static const char *drm_devices[] = { + "/dev/dri/renderD128", + "/dev/dri/card0", + NULL, + }; + for (int i = 0; drm_devices[i]; i++) { + drm_fd = open(drm_devices[i], O_RDWR); + if (drm_fd < 0) + continue; + display = vaGetDisplayDRM(drm_fd); + if (vaDisplayIsValid(display)) + break; + close(drm_fd); + drm_fd = -1; + display = NULL; + } + if (!vaDisplayIsValid(display)) + throw std::runtime_error("display not found"); + + /* Initialize and create config */ + int major_ver, minor_ver; + vaSafeCall(vaInitialize(display, &major_ver, &minor_ver)); + + const char *driver = vaQueryVendorString(display); + std::cerr << "[VaapiJpegRgbPacketProcessor::initializeVaapi] driver: " << driver << std::endl; + + int max_entrypoints = vaMaxNumEntrypoints(display); + if (max_entrypoints <= 0) + throw std::runtime_error("invalid MaxNumEntrypoints"); + + VAEntrypoint entrypoints[max_entrypoints]; + int num_entrypoints; + vaSafeCall(vaQueryConfigEntrypoints(display, VAProfileJPEGBaseline, entrypoints, &num_entrypoints)); + if (num_entrypoints < 0 || num_entrypoints > max_entrypoints) + throw std::runtime_error("invalid number of entrypoints"); + + int vld_entrypoint; + for (vld_entrypoint = 0; vld_entrypoint < num_entrypoints; vld_entrypoint++) { + if (entrypoints[vld_entrypoint] == VAEntrypointVLD) + break; + } + if (vld_entrypoint == num_entrypoints) + throw std::runtime_error("did not find VLD"); + + VAConfigAttrib attr; + attr.type = VAConfigAttribRTFormat; + vaSafeCall(vaGetConfigAttributes(display, VAProfileJPEGBaseline, VAEntrypointVLD, &attr, 1)); + unsigned int rtformat = VA_RT_FORMAT_YUV444; + if ((attr.value & rtformat) == 0) { + std::cerr << "[VaapiJpegRgbPacketProcessor::initializeVaapi] warning: YUV444 not supported by libva, chroma will be halved" << std::endl; + rtformat = VA_RT_FORMAT_YUV420; + } + if ((attr.value & rtformat) == 0) + throw std::runtime_error("does not support YUV420"); + + vaSafeCall(vaCreateConfig(display, VAProfileJPEGBaseline, VAEntrypointVLD, &attr, 1, &config)); + + /* Create surface and context */ + vaSafeCall(vaCreateSurfaces(display, rtformat, WIDTH, HEIGHT, &surface, 1, NULL, 0)); + + vaSafeCall(vaCreateContext(display, config, WIDTH, HEIGHT, 0, &surface, 1, &context)); + } + + VABufferID createBuffer(VABufferType type, unsigned int size, void *data) + { + VABufferID buffer; + vaSafeCall(vaCreateBuffer(display, context, type, size, 1, data, &buffer)); + if (buffer == VA_INVALID_ID) + throw std::runtime_error("created invalid buffer"); + return buffer; + } + + void createParameters(struct jpeg_decompress_struct &dinfo) + { + /* Picture Parameter */ + VAPictureParameterBufferJPEGBaseline pic = {0}; + pic.picture_width = dinfo.image_width; + pic.picture_height = dinfo.image_height; + for (int i = 0; i< dinfo.num_components; i++) { + pic.components[i].component_id = dinfo.comp_info[i].component_id; + pic.components[i].h_sampling_factor = dinfo.comp_info[i].h_samp_factor; + pic.components[i].v_sampling_factor = dinfo.comp_info[i].v_samp_factor; + pic.components[i].quantiser_table_selector = dinfo.comp_info[i].quant_tbl_no; + } + pic.num_components = dinfo.num_components; + pic_param_buf = createBuffer(VAPictureParameterBufferType, sizeof(pic), &pic); + + /* IQ Matrix */ + VAIQMatrixBufferJPEGBaseline iq = {0}; + for (int i = 0; i < NUM_QUANT_TBLS; i++) { + if (!dinfo.quant_tbl_ptrs[i]) + continue; + iq.load_quantiser_table[i] = 1; + /* Assuming dinfo.data_precision == 8 */ + const int natural_order[DCTSIZE2] = { + 0, 1, 8, 16, 9, 2, 3, 10, + 17, 24, 32, 25, 18, 11, 4, 5, + 12, 19, 26, 33, 40, 48, 41, 34, + 27, 20, 13, 6, 7, 14, 21, 28, + 35, 42, 49, 56, 57, 50, 43, 36, + 29, 22, 15, 23, 30, 37, 44, 51, + 58, 59, 52, 45, 38, 31, 39, 46, + 53, 60, 61, 54, 47, 55, 62, 63, + }; + + for (int j = 0; j < DCTSIZE2; j++) + iq.quantiser_table[i][j] = dinfo.quant_tbl_ptrs[i]->quantval[natural_order[j]]; + } + iq_buf = createBuffer(VAIQMatrixBufferType, sizeof(iq), &iq); + + /* Huffman Table */ + VAHuffmanTableBufferJPEGBaseline huff = {0}; + const int num_huffman_tables = 2; + for (int i = 0; i < num_huffman_tables; i++) { + if (!dinfo.dc_huff_tbl_ptrs[i] || !dinfo.ac_huff_tbl_ptrs[i]) + continue; + huff.load_huffman_table[i] = 1; + memcpy(huff.huffman_table[i].num_dc_codes, &dinfo.dc_huff_tbl_ptrs[i]->bits[1], + sizeof(huff.huffman_table[i].num_dc_codes)); + memcpy(huff.huffman_table[i].dc_values, dinfo.dc_huff_tbl_ptrs[i]->huffval, + sizeof(huff.huffman_table[i].dc_values)); + memcpy(huff.huffman_table[i].num_ac_codes, &dinfo.ac_huff_tbl_ptrs[i]->bits[1], + sizeof(huff.huffman_table[i].num_ac_codes)); + memcpy(huff.huffman_table[i].ac_values, dinfo.ac_huff_tbl_ptrs[i]->huffval, + sizeof(huff.huffman_table[i].ac_values)); + } + huff_buf = createBuffer(VAHuffmanTableBufferType, sizeof(huff), &huff); + + /* Slice Parameter */ + VASliceParameterBufferJPEGBaseline *pslice; + slice_param_buf = createBuffer(VASliceParameterBufferType, sizeof(*pslice), NULL); + vaSafeCall(vaMapBuffer(display, slice_param_buf, (void**)&pslice)); + VASliceParameterBufferJPEGBaseline &slice = *pslice; + + slice.slice_data_offset = dinfo.src->next_input_byte - packet_buffer->back().data; + slice.slice_data_flag = VA_SLICE_DATA_FLAG_ALL; + for (int i = 0; i < dinfo.comps_in_scan; i++) { + slice.components[i].component_selector = dinfo.cur_comp_info[i]->component_id; + slice.components[i].dc_table_selector = dinfo.cur_comp_info[i]->dc_tbl_no; + slice.components[i].ac_table_selector = dinfo.cur_comp_info[i]->ac_tbl_no; + } + slice.num_components = dinfo.comps_in_scan; + slice.restart_interval = dinfo.restart_interval; + unsigned int mcu_h_size = dinfo.max_h_samp_factor * DCTSIZE; + unsigned int mcu_v_size = dinfo.max_v_samp_factor * DCTSIZE; + unsigned int mcus_per_row = (WIDTH + mcu_h_size - 1) / mcu_h_size; + unsigned int mcu_rows_in_scan = (HEIGHT + mcu_v_size - 1) / mcu_v_size; + slice.num_mcus = mcus_per_row * mcu_rows_in_scan; + + vaSafeCall(vaUnmapBuffer(display, slice_param_buf)); + } + + void decompress(unsigned char *buf, size_t len) + { + if (jpeg_first_packet) + { + jpeg_mem_src(&dinfo, buf, len); + int header_status = jpeg_read_header(&dinfo, true); + if (header_status != JPEG_HEADER_OK) + throw std::runtime_error("jpeg not ready for decompression"); + + if (dinfo.image_width != WIDTH || dinfo.image_height != HEIGHT) + throw std::runtime_error("image dimensions do not match preset"); + + jpeg_first_packet = false; + + createParameters(dinfo); + + jpeg_header_size = len - dinfo.src->bytes_in_buffer; + jpeg_abort_decompress(&dinfo); + } + /* Grab packet buffer for server */ + vaSafeCall(vaUnmapBuffer(display, packet_buffer->back_id())); + + /* The only parameter that changes after the first packet */ + VASliceParameterBufferJPEGBaseline *slice; + vaSafeCall(vaMapBuffer(display, slice_param_buf, (void**)&slice)); + slice->slice_data_size = len - jpeg_header_size; + vaSafeCall(vaUnmapBuffer(display, slice_param_buf)); + + /* Commit buffers */ + vaSafeCall(vaBeginPicture(display, context, surface)); + VABufferID va_bufs[5] = { + pic_param_buf, iq_buf, huff_buf, slice_param_buf, packet_buffer->back_id() + }; + vaSafeCall(vaRenderPicture(display, context, va_bufs, 5)); + vaSafeCall(vaEndPicture(display, context)); + + /* Sync surface */ + vaSafeCall(vaSyncSurface(display, surface)); + + frame->draw(surface); + + /* Release packet buffer back to parser */ + vaSafeCall(vaMapBuffer(display, packet_buffer->back_id(), (void**)&packet_buffer->back().data)); + } +}; + +VaapiJpegRgbPacketProcessor::VaapiJpegRgbPacketProcessor() : + impl_(new VaapiJpegRgbPacketProcessorImpl()) +{ +} + +VaapiJpegRgbPacketProcessor::~VaapiJpegRgbPacketProcessor() +{ + delete impl_; +} + +void VaapiJpegRgbPacketProcessor::process(const RgbPacket &packet) +{ + if (listener_ == 0) + return; + + impl_->startTiming(); + + impl_->frame->timestamp = packet.timestamp; + impl_->frame->sequence = packet.sequence; + + try + { + impl_->decompress(packet.jpeg_buffer, packet.jpeg_buffer_length); + if (listener_->onNewFrame(Frame::Color, impl_->frame)) + { + impl_->newFrame(); + } + } + catch (const std::runtime_error &err) + { + std::cerr << "[VaapiJpegRgbPacketProcessor::doProcess] Failed to decompress: " << err.what() << std::endl; + } + + impl_->stopTiming(); +} + +libfreenect2::DoubleBuffer *VaapiJpegRgbPacketProcessor::getPacketBuffer() +{ + return impl_->packet_buffer; +} + +} /* namespace libfreenect2 */