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,36 @@ 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();
+
+  const float* getXTable();
+  const float* getZTable();
+
+  const short* getLookupTable();
+
+ protected:
+  unsigned char* p0table_;
+
+  float* xtable_;
+  float* ztable_;
+  
+  short* lut_;
+};
+
 } /* namespace libfreenect2 */
 #endif /* DEPTH_PACKET_PROCESSOR_H_ */

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,63 @@ void DepthPacketProcessor::setFrameListener(libfreenect2::FrameListener *listene
   listener_ = listener;
 }
 
+DumpDepthPacketProcessor::DumpDepthPacketProcessor()
+  : p0table_(NULL), xtable_(NULL), ztable_(NULL), lut_(NULL) {
+}
+
+DumpDepthPacketProcessor::~DumpDepthPacketProcessor(){
+  delete[] p0table_;
+  delete[] xtable_;
+  delete[] ztable_;
+  delete[] lut_;
+}
+
+void DumpDepthPacketProcessor::process(const DepthPacket &packet) {
+  Frame* depth_frame = new Frame(1, 1, packet.buffer_length);
+  
+  depth_frame->timestamp = packet.timestamp;
+  depth_frame->sequence = packet.sequence;
+  depth_frame->format = Frame::Raw;
+  std::memcpy(depth_frame->data, packet.buffer, packet.buffer_length);
+
+  Frame* ir_frame = new Frame(1, 1, packet.buffer_length, depth_frame->data);
+  ir_frame->timestamp = packet.timestamp;
+  ir_frame->sequence = packet.sequence;
+  ir_frame->data = packet.buffer;
+  ir_frame->format = Frame::Raw;
+
+  if (!listener_->onNewFrame(Frame::Ir, ir_frame)) {
+    delete ir_frame;
+  }
+  ir_frame = NULL;
+  if (!listener_->onNewFrame(Frame::Depth, depth_frame)) {
+    delete depth_frame;
+  }
+  depth_frame = NULL;
+}
+  
+const unsigned char* DumpDepthPacketProcessor::getP0Tables() { return p0table_; }
+  
+const float* DumpDepthPacketProcessor::getXTable() { return xtable_; }
+const float* DumpDepthPacketProcessor::getZTable() { return ztable_; }
+
+const 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

@@ -29,9 +29,12 @@
 #include <libfreenect2/rgb_packet_processor.h>
 #include <libfreenect2/async_packet_processor.h>
 
+#include <cstring>
 #include <fstream>
+#include <sstream>
 #include <string>
 
+
 namespace libfreenect2
 {
 
@@ -49,22 +52,25 @@ void RgbPacketProcessor::setFrameListener(libfreenect2::FrameListener *listener)
   listener_ = listener;
 }
 
-DumpRgbPacketProcessor::DumpRgbPacketProcessor()
-{
-}
-
-DumpRgbPacketProcessor::~DumpRgbPacketProcessor()
-{
-}
+DumpRgbPacketProcessor::DumpRgbPacketProcessor() {}
+DumpRgbPacketProcessor::~DumpRgbPacketProcessor() {}
 
 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 *frame = new Frame(1, 1, 1920*1080*4);
+  frame->sequence = packet.sequence;
+  frame->timestamp = packet.timestamp;
+  frame->exposure = packet.exposure;
+  frame->gain = packet.gain;
+  frame->gamma = packet.gamma;
+  frame->format = Frame::Raw;
+
+  std::memcpy(frame->data, packet.jpeg_buffer, packet.jpeg_buffer_length);
+
+  if (!listener_->onNewFrame(Frame::Color, frame)) {
+    delete frame;
+  }
+  frame = NULL;
 }
 
 } /* namespace libfreenect2 */