Improve RGB and depth stream parsers

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/frame_listener.hpp

@@ -28,6 +28,7 @@
 #define FRAME_LISTENER_HPP_
 
 #include <cstddef>
+#include <stdint.h>
 #include <libfreenect2/config.h>
 
 namespace libfreenect2
@@ -42,6 +43,8 @@ struct LIBFREENECT2_API Frame
     Depth = 4
   };
 
+  uint32_t timestamp;
+  uint32_t sequence;
   size_t width, height, bytes_per_pixel;
   unsigned char* data;
 

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

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
 {
@@ -43,8 +42,8 @@ DepthPacketStreamParser::DepthPacketStreamParser() :
   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;
 }
 
@@ -59,43 +58,38 @@ void DepthPacketStreamParser::setPacketProcessor(libfreenect2::BaseDepthPacketPr
 
 void DepthPacketStreamParser::onDataReceived(unsigned char* buffer, size_t in_length)
 {
-  // TODO: simplify this crap (so code, such unreadable, wow ;)
   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 +105,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 +142,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;

examples/protonect/src/rgb_packet_stream_parser.cpp

@@ -36,11 +36,29 @@ 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<RgbPacket>())
 {
@@ -71,12 +89,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<RgbPacketFooter *>(&fb.data[fb.length - sizeof(RgbPacketFooter)]);
+
+    if (footer->magic_header == 0x39393939 && footer->magic_footer == 0x42424242)
     {
+      RawRgbPacket *raw_packet = reinterpret_cast<RawRgbPacket *>(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 +146,9 @@ void RgbPacketStreamParser::onDataReceived(unsigned char* buffer, size_t length)
         RawRgbPacket *raw_packet = reinterpret_cast<RawRgbPacket *>(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);

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)