Add VA-API JPEG decoder

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

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

examples/protonect/include/libfreenect2/async_packet_processor.h

@@ -76,6 +76,11 @@ class AsyncPacketProcessor : public PacketProcessor<PacketT>
     }
     packet_condition_.notify_one();
   }
+
+  virtual libfreenect2::DoubleBuffer *getPacketBuffer()
+  {
+    return processor_->getPacketBuffer();
+  }
 private:
   PacketProcessorPtr processor_;
   bool current_packet_available_;

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

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

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_;

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_;
 

examples/protonect/include/libfreenect2/frame_listener.hpp

@@ -28,6 +28,7 @@
 #define FRAME_LISTENER_HPP_
 
 #include <cstddef>
+#include <stdint.h>
 #include <libfreenect2/config.h>
 
 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;
   }

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

examples/protonect/include/libfreenect2/packet_processor.h

@@ -26,6 +26,7 @@
 
 #ifndef PACKET_PROCESSOR_H_
 #define PACKET_PROCESSOR_H_
+#include <libfreenect2/double_buffer.h>
 
 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<typename PacketT>

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_ */

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

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<float>();

examples/protonect/src/depth_packet_stream_parser.cpp

@@ -27,7 +27,6 @@
 #include <libfreenect2/depth_packet_stream_parser.h>
 #include <iostream>
 #include <memory.h>
-#include <algorithm>
 
 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<DepthPacket>();
+
+  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<size_t>(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<DepthSubPacketFooter *>(ptr_in);
-
-      if(footer->magic0 == 0x0 && footer->magic1 == 0x9)
-      {
-        footer_found = true;
-        break;
-      }
+      in_length -= sizeof(DepthSubPacketFooter);
+      footer = reinterpret_cast<DepthSubPacketFooter *>(&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,14 +109,15 @@ 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;
 
               processor_->process(packet);
             }
             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;
     }
   }
 }

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

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;