update rgb streamer.

inspecting to find magic markers.
copy data in chunks.
Verify by packetlength and sequence of footer and header.

Author: larshg

examples/protonect/include/libfreenect2/frame_listener.hpp

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

examples/protonect/include/libfreenect2/rgb_packet_processor.h

@@ -39,7 +39,7 @@ namespace libfreenect2
 struct RgbPacket
 {
   uint32_t sequence;
-
+  uint32_t timestamp;
   unsigned char *jpeg_buffer;
   size_t jpeg_buffer_length;
 };

examples/protonect/src/rgb_packet_stream_parser.cpp

@@ -28,18 +28,34 @@
 #include <memory.h>
 #include <iostream>
 
+
 namespace libfreenect2
 {
 
 
 struct RawRgbPacket
 {
   uint32_t sequence;
-  uint32_t unknown0;
+  uint32_t magicHeader; // is 'BBBB' equal 0x42424242
 
   unsigned char jpeg_buffer[0];
 };
 
+struct RgbPacketFooter {
+    uint32_t magicHeader; // is '9999' equal 0x39393939
+    uint32_t sequence;
+    int32_t unknown1; // varies from 0 to 60 with steps of 4
+    int32_t unknown2; // seems 0 always
+    int32_t unknown3; // 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 magicFooter; // is 'BBBB' equal 0x42424242
+    uint32_t packet_size;
+    float version; // ? this seems to be consistent 1.0
+    uint8_t unknown4[12]; // seems to be 0 all the time.
+};
+
 RgbPacketStreamParser::RgbPacketStreamParser() :
     processor_(noopProcessor<RgbPacket>())
 {
@@ -57,49 +73,65 @@ void RgbPacketStreamParser::setPacketProcessor(BaseRgbPacketProcessor *processor
 
 void RgbPacketStreamParser::onDataReceived(unsigned char* buffer, size_t length)
 {
+  // if no data return
+  if (length == 0)
+    return;
+
   Buffer &fb = buffer_.front();
 
-  // package containing data
-  if(length > 0)
+  // iterate through each byte to detect footer
+  for (size_t i = 0; i < length; i++)
   {
-    if(fb.length + length <= fb.capacity)
-    {
-      memcpy(fb.data + fb.length, buffer, length);
+    RgbPacketFooter* footer = reinterpret_cast<RgbPacketFooter *>(&buffer[i]);
+
+    // if magic markers match
+    if (length - i >= sizeof(RgbPacketFooter) && footer->magicHeader == 0x39393939 && footer->magicFooter == 0x42424242)
+    {  
+      memcpy(&fb.data[fb.length], buffer, length);
       fb.length += length;
-    }
-    else
-    {
-      std::cerr << "[RgbPacketStreamParser::handleNewData] buffer overflow!" << std::endl;
-    }
 
-    // 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))
-    {
-      // can the processor handle the next image?
-      if(processor_->ready())
-      {
-        buffer_.swap();
-        Buffer &bb = buffer_.back();
+      buffer_.swap();
+      Buffer &bb = buffer_.back();
+      RawRgbPacket *raw_packet = reinterpret_cast<RawRgbPacket *>(bb.data);
 
-        RawRgbPacket *raw_packet = reinterpret_cast<RawRgbPacket *>(bb.data);
-        RgbPacket rgb_packet;
-        rgb_packet.sequence = raw_packet->sequence;
-        rgb_packet.jpeg_buffer = raw_packet->jpeg_buffer;
-        rgb_packet.jpeg_buffer_length = bb.length - sizeof(RawRgbPacket);
+      if (bb.length == footer->packet_size && raw_packet->sequence == footer->sequence)
+      {
+        if (processor_->ready())
+        {
+          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);
 
-        // call the processor
-        processor_->process(rgb_packet);
+          // call the processor
+          processor_->process(rgb_packet);
+        }
+        else
+        {
+          std::cerr << "[RgbPacketStreamParser::handleNewData] skipping rgb packet!" << std::endl;
+        }
+        // clear buffer and return
+        buffer_.front().length = 0;
+        return;
       }
       else
       {
-        std::cerr << "[RgbPacketStreamParser::handleNewData] skipping rgb packet!" << std::endl;
+        std::cerr << "[RgbPacketStreamParser::handleNewData] packetsize or sequence doesn't match!" << std::endl;
+        fb.length = 0;
+        return;
       }
-
-      // reset front buffer
-      buffer_.front().length = 0;
     }
   }
+  // clear buffer if overflow of data
+  if (fb.length + length > fb.capacity)
+  {
+    std::cerr << "[RgbPacketStreamParser::handleNewData] Buffer overflow - resetting." << std::endl;
+    fb.length = 0;
+  }
+
+  memcpy(&fb.data[fb.length], buffer, length);
+  fb.length += length;
 }
 
 } /* namespace libfreenect2 */

examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp

@@ -112,6 +112,8 @@ void TurboJpegRgbPacketProcessor::process(const RgbPacket &packet)
   {
     impl_->startTiming();
 
+    impl_->frame->timestamp = packet.timestamp;
+
     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)