Add a dump depth processor. Reactivate the RGB dump processor.

Add a dump pipeline.

Author: brendandburns

include/internal/libfreenect2/depth_packet_processor.h

@@ -175,6 +175,40 @@ class OpenCLDepthPacketProcessor : public DepthPacketProcessor
 private:
   OpenCLDepthPacketProcessorImpl *impl_;
 };
+
 #endif // LIBFREENECT2_WITH_OPENCL_SUPPORT
+
+class DumpDepthPacketProcessor : public DepthPacketProcessor
+{
+ public:
+  DumpDepthPacketProcessor();
+  virtual ~DumpDepthPacketProcessor();
+
+  virtual void process(const DepthPacket &packet);
+
+  virtual void loadP0TablesFromCommandResponse(unsigned char* buffer, size_t buffer_length);
+  virtual void loadXZTables(const float *xtable, const float *ztable);
+  virtual void loadLookupTable(const short *lut);
+
+  const unsigned char* getP0Tables();
+
+  float* getXTable();
+  float* getZTable();
+
+  short* getLookupTable();
+
+ protected:
+  Frame* depth_frame;
+  Frame* ir_frame;
+
+  unsigned char* p0table_;
+  size_t p0table_length_;
+
+  float* xtable_;
+  float* ztable_;
+  
+  short* lut_;
+};
+
 } /* namespace libfreenect2 */
 #endif /* DEPTH_PACKET_PROCESSOR_H_ */

include/internal/libfreenect2/rgb_packet_processor.h

@@ -75,6 +75,7 @@ class DumpRgbPacketProcessor : public RgbPacketProcessor
   virtual ~DumpRgbPacketProcessor();
 protected:
   virtual void process(const libfreenect2::RgbPacket &packet);
+  Frame *frame;
 };
 
 class TurboJpegRgbPacketProcessorImpl;

include/libfreenect2/frame_listener.hpp

@@ -58,12 +58,14 @@ class LIBFREENECT2_API Frame
     BGRX,
     RGBX,
     Gray,
-    Float
+    Float,
+    // Note that if format is 'raw' then 'bytes_per_pixel' actually contains the number of bytes
+    Raw
   };
 
   size_t width;           ///< Length of a line (in pixels).
   size_t height;          ///< Number of lines in the frame.
-  size_t bytes_per_pixel; ///< Number of bytes in a pixel.
+  size_t bytes_per_pixel; ///< Number of bytes in a pixel. If frame format is 'Raw' this is the buffer size.
   unsigned char* data;    ///< Data of the frame (aligned). @see See Frame::Type for pixel format.
   uint32_t timestamp;     ///< Unit: roughly or exactly 0.1 millisecond
   uint32_t sequence;      ///< Increasing frame sequence number

include/libfreenect2/packet_pipeline.h

@@ -66,6 +66,13 @@ class LIBFREENECT2_API PacketPipeline
   PacketPipelineComponents *comp_;
 };
 
+ class LIBFREENECT2_API DumpPacketPipeline: public PacketPipeline
+ {
+ public:
+   DumpPacketPipeline();
+   virtual ~DumpPacketPipeline();
+ };
+
 /** Pipeline with CPU depth processing. */
 class LIBFREENECT2_API CpuPacketPipeline : public PacketPipeline
 {

src/depth_packet_processor.cpp

@@ -29,6 +29,8 @@
 #include <libfreenect2/depth_packet_processor.h>
 #include <libfreenect2/async_packet_processor.h>
 
+#include <cstring>
+
 namespace libfreenect2
 {
 
@@ -98,4 +100,64 @@ void DepthPacketProcessor::setFrameListener(libfreenect2::FrameListener *listene
   listener_ = listener;
 }
 
+  DumpDepthPacketProcessor::DumpDepthPacketProcessor() : depth_frame(new Frame(512, 424, 4)), ir_frame(new Frame(512, 424,4)) {
+}
+
+DumpDepthPacketProcessor::~DumpDepthPacketProcessor(){
+  delete depth_frame;
+  delete ir_frame;
+  delete[] p0table_;
+  delete[] xtable_;
+  delete[] ztable_;
+  delete[] lut_;
+}
+
+void DumpDepthPacketProcessor::process(const DepthPacket &packet) {
+    ir_frame->height = 1;
+    ir_frame->width = 1;
+    ir_frame->bytes_per_pixel = packet.buffer_length;
+    ir_frame->timestamp = packet.timestamp;
+    ir_frame->sequence = packet.sequence;
+    ir_frame->data = packet.buffer;
+
+    depth_frame->height = 1;
+    depth_frame->width = 1;
+    depth_frame->bytes_per_pixel = packet.buffer_length;
+    depth_frame->timestamp = packet.timestamp;
+    depth_frame->sequence = packet.sequence;
+    depth_frame->data = packet.buffer;
+
+    if (listener_->onNewFrame(Frame::Ir, ir_frame)) {
+      ir_frame = new Frame(512, 424,4);
+    }
+    if (listener_->onNewFrame(Frame::Depth, depth_frame)) {
+      depth_frame = new Frame(512, 424, 4);
+    }
+  }
+
+  
+  const unsigned char* DumpDepthPacketProcessor::getP0Tables() { return p0table_; }
+
+  float* DumpDepthPacketProcessor::getXTable() { return xtable_; }
+  float* DumpDepthPacketProcessor::getZTable() { return ztable_; }
+
+  short* DumpDepthPacketProcessor::getLookupTable() { return lut_; }
+
+  void DumpDepthPacketProcessor::loadP0TablesFromCommandResponse(unsigned char* buffer, size_t buffer_length) {
+    p0table_ = new unsigned char[buffer_length];
+    std::memcpy(p0table_, buffer, buffer_length);
+  }
+  
+  void DumpDepthPacketProcessor::loadXZTables(const float *xtable, const float *ztable) {
+    xtable_ = new float[TABLE_SIZE];
+    std::memcpy(xtable_, xtable, TABLE_SIZE * sizeof(float));
+
+    ztable_ = new float[TABLE_SIZE];
+    std::memcpy(ztable_, ztable, TABLE_SIZE * sizeof(float));
+  }
+  
+  void DumpDepthPacketProcessor::loadLookupTable(const short *lut) {
+    lut_ = new short[LUT_SIZE];
+    std::memcpy(lut_, lut, LUT_SIZE * sizeof(short));
+  }
 } /* namespace libfreenect2 */

src/packet_pipeline.cpp

@@ -129,4 +129,13 @@ OpenCLPacketPipeline::OpenCLPacketPipeline(const int deviceId) : deviceId(device
 OpenCLPacketPipeline::~OpenCLPacketPipeline() { }
 #endif // LIBFREENECT2_WITH_OPENCL_SUPPORT
 
+DumpPacketPipeline::DumpPacketPipeline()
+{
+  RgbPacketProcessor *rgb = new DumpRgbPacketProcessor();
+  DepthPacketProcessor *depth = new DumpDepthPacketProcessor();
+  comp_->initialize(rgb, depth);
+}
+
+DumpPacketPipeline::~DumpPacketPipeline() {}
+
 } /* namespace libfreenect2 */

src/rgb_packet_processor.cpp

@@ -30,8 +30,10 @@
 #include <libfreenect2/async_packet_processor.h>
 
 #include <fstream>
+#include <sstream>
 #include <string>
 
+
 namespace libfreenect2
 {
 
@@ -50,21 +52,31 @@ void RgbPacketProcessor::setFrameListener(libfreenect2::FrameListener *listener)
 }
 
 DumpRgbPacketProcessor::DumpRgbPacketProcessor()
+  : frame(new Frame(1920, 1080, 4))
 {
 }
 
 DumpRgbPacketProcessor::~DumpRgbPacketProcessor()
 {
+  delete frame;
 }
 
 void DumpRgbPacketProcessor::process(const RgbPacket &packet)
 {
-  //std::stringstream name;
-  //name << packet->sequence << "_" << packet->unknown0 << "_" << jpeg_buffer_length << ".jpeg";
-  //
-  //std::ofstream file(name.str().c_str());
-  //file.write(reinterpret_cast<char *>(packet->jpeg_buffer), jpeg_buffer_length);
-  //file.close();
+  frame->sequence = packet.sequence;
+  frame->timestamp = packet.timestamp;
+  frame->width = 1;
+  frame->height = 1;
+  frame->bytes_per_pixel = packet.jpeg_buffer_length;
+  frame->data = packet.jpeg_buffer;
+
+  frame->exposure = packet.exposure;
+  frame->gain = packet.gain;
+  frame->gamma = packet.gamma;
+
+  if (listener_->onNewFrame(Frame::Color, frame)) {
+    frame = new Frame(1920, 1080, 4);
+  }
 }
 
 } /* namespace libfreenect2 */