Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
VideoSource_Kinect2.cpp
1 #include "VideoSource_Kinect2.hh"
2 #include <iostream>
3 #include <Base/Image/ImageIO.hh>
4 
5 #include <libfreenect2/frame_listener.hpp>
6 #include <libfreenect2/packet_pipeline.h>
7 #include <libfreenect2/async_packet_processor.h>
8 #include <libfreenect2/response.h>
9 #include <libfreenect2/resource.h>
10 #include <iostream>
11 #include <OpenCLFramework/Algorithm/clfAlgorithm.hh>
12 
13 #ifdef FREENECT_HAVE_TURBOJPEG
14 #include <turbojpeg.h>
15 #else
16 #ifdef BIAS_HAVE_LIBJPEG
17 #include <jpeglib.h>
18 #else
19 // ImageMagick support
20 #ifdef BIAS_HAVE_IMAGEMAGICKLIB
21 # include <Base/Common/BIASpragmaStart.hh>
22 # include <Magick++.h>
23 # include <Base/Common/BIASpragmaEnd.hh>
24 # if (MagickLibVersion >= 0x650)
25 # define MagickLib MagickCore
26 # endif
27 #endif
28 #endif
29 #endif
30 
31 using namespace std;
32 using namespace BIAS;
33 using namespace libfreenect2;
34 
35 ///////////////////////FREENECT INTERFACES/////////////////////////////////
36 
37 #ifdef FREENECT_HAVE_TURBOJPEG
38 class FreenectJpegRgbPacketProcessor : public RgbPacketProcessor
39 {
40 public:
41  FreenectJpegRgbPacketProcessor() {
42  frame_ = new Frame(1920,1080,tjPixelSize[TJPF_RGB]);
43  flippedframe_ = new Frame(1920,1080,tjPixelSize[TJPF_RGB]);
44  decompressor_ = tjInitDecompress();
45  if(decompressor_ == 0)
46  {
47  BIASERR("[TurboJpegRgbPacketProcessor] Failed to initialize TurboJPEG decompressor! TurboJPEG error: '" << tjGetErrorStr() << "'");
48  }
49  }
50  virtual ~FreenectJpegRgbPacketProcessor() {
51  delete frame_;
52  delete flippedframe_;
53  if (decompressor_ != 0) {
54  tjDestroy(decompressor_);
55  }
56  }
57 protected:
58  virtual void process(const libfreenect2::RgbPacket &packet) {
59  if (listener_ != 0 && decompressor_ != 0) {
60  tjDecompress2(decompressor_, packet.jpeg_buffer, packet.jpeg_buffer_length, flippedframe_->data, 1920, 1920 * tjPixelSize[TJPF_RGB], 1080, TJPF_RGB, 0);
61  unsigned char *out = frame_->data;
62  unsigned char *in = flippedframe_->data;
63  unsigned int stride = 1920*3;
64  register int istart = 0;
65  register int ostart = 0;
66  for (unsigned int y=0;y<1080;y++) {
67  istart = y*stride;
68  ostart = y*stride+1919*3;
69  for (unsigned int x=0;x<1920;x++) {
70  memcpy( out+ostart, in+istart, 3);
71  istart += 3;
72  ostart -= 3;
73  }
74  }
75  if(listener_->onNewFrame(Frame::Color, frame_))
76  {
77  // yay new frame
78  }
79  }
80  }
81 private:
82  Frame *frame_;
83  Frame *flippedframe_;
84  tjhandle decompressor_;
85 };
86 #else
87 #ifdef BIAS_HAVE_LIBJPEG
88 class FreenectJpegRgbPacketProcessor : public RgbPacketProcessor
89 {
90 public:
91  FreenectJpegRgbPacketProcessor() {
92  frame_ = new Frame(1920,1080,3);
93  flippedframe_ = new Frame(1920,1080,3);
94  jpeg_create_decompress(&cinfo_);
95  cinfo_.err = jpeg_std_error(&err_);
96  }
97  virtual ~FreenectJpegRgbPacketProcessor() {
98  delete frame_;
99  delete flippedframe_;
100  jpeg_destroy_decompress(&cinfo_);
101  }
102 protected:
103  virtual void process(const libfreenect2::RgbPacket &packet) {
104  if (listener_ != 0) {
105  jpeg_mem_src(&cinfo_, packet.jpeg_buffer, packet.jpeg_buffer_length);
106  jpeg_read_header(&cinfo_, true);
107  jpeg_start_decompress(&cinfo_);
108  int row_stride = cinfo_.output_width * cinfo_.output_components;
109  unsigned char *data = flippedframe_->data;
110  while (cinfo_.output_scanline < cinfo_.output_height) {
111  jpeg_read_scanlines(&cinfo_, &data, 1);
112  data+=row_stride;
113  }
114  jpeg_finish_decompress(&cinfo_);
115  unsigned char *out = frame_->data;
116  unsigned char *in = flippedframe_->data;
117  unsigned int stride = 1920*3;
118  register int istart = 0;
119  register int ostart = 0;
120  for (unsigned int y=0;y<1080;y++) {
121  istart = y*stride;
122  ostart = y*stride+1919*3;
123  for (unsigned int x=0;x<1920;x++) {
124  memcpy( out+ostart, in+istart, 3);
125  istart += 3;
126  ostart -= 3;
127  }
128  }
129  if(listener_->onNewFrame(Frame::Color, frame_))
130  {
131  // yay new frame
132  }
133  }
134  }
135 private:
136  Frame *frame_;
137  Frame *flippedframe_;
138  jpeg_decompress_struct cinfo_;
139  jpeg_error_mgr err_;
140 };
141 #else
142 class FreenectJpegRgbPacketProcessor : public RgbPacketProcessor
143 {
144 public:
145  FreenectJpegRgbPacketProcessor() {
146  frame_ = new Frame(1920,1080,3);
147  flippedframe_ = new Frame(1920,1080,3);
148  }
149  virtual ~FreenectJpegRgbPacketProcessor() {
150  delete frame_;
151  delete flippedframe_;
152  }
153 protected:
154  virtual void process(const libfreenect2::RgbPacket &packet) {
155  if (listener_ != 0) {
156 #ifdef BIAS_HAVE_IMAGEMAGICKLIB
157  Magick::Image image;
158  Magick::Blob data(packet.jpeg_buffer, packet.jpeg_buffer_length);
159  Magick::Geometry size(1920,1080);
160  image.read( data, size, 1, "JPEG" );
161  image.write(0, 0, 1920, 1080, "RGB", MagickLib::CharPixel, flippedframe_->data);
162  unsigned char *out = frame_->data;
163  unsigned char *in = flippedframe_->data;
164  unsigned int stride = 1920*3;
165  register int istart = 0;
166  register int ostart = 0;
167  for (unsigned int y=0;y<1080;y++) {
168  istart = y*stride;
169  ostart = y*stride+1919*3;
170  for (unsigned int x=0;x<1920;x++) {
171  memcpy( out+ostart, in+istart, 3);
172  istart += 3;
173  ostart -= 3;
174  }
175  }
176 #else
177  BIASERR("could not find any suitable lib to decompress jpeg!");
178 #endif
179  if(listener_->onNewFrame(Frame::Color, frame_))
180  {
181  //std::cout << "YAY" << std::endl;
182  }
183  }
184  }
185 private:
186  Frame *frame_;
187  Frame *flippedframe_;
188 };
189 #endif
190 #endif
191 
192 class FreenectDepthPacketProcessor : public DepthPacketProcessor, public BIAS::clfAlgorithm
193 {
194 public:
195  FreenectDepthPacketProcessor(BIAS::clfContext *context);
196  virtual ~FreenectDepthPacketProcessor();
197  virtual void setConfiguration(const libfreenect2::DepthPacketProcessor::Config &config);
198 
199  virtual void loadP0TablesFromCommandResponse(unsigned char* buffer, size_t buffer_length);
200 
201  void loadXTableFromFile(const std::string &filename);
202  void loadZTableFromFile(const std::string &filename);
203  void load11To16LutFromFile(const std::string &filename);
204 
205  virtual void process(const DepthPacket &packet);
206 
207 protected:
208  void initialize_();
209  void generateOptions_(std::string &options);
210  bool loadBufferFromResources_(const std::string &filename, unsigned char *buffer, const size_t n);
211 private:
212  Frame *ir_frame_;
213  Frame *depth_frame_;
214 
215  cl_short lut11to16[2048];
216  cl_float x_table[512 * 424];
217  cl_float z_table[512 * 424];
218  cl_float3 p0_table[512 * 424];
219  libfreenect2::DepthPacketProcessor::Config config_;
220  DepthPacketProcessor::Parameters params_;
221  size_t image_size;
222  bool initialized_;
223 
224 private: // opencl
225  // Read only buffers
226  BIAS::clfBuffer *buf_lut11to16;
227  BIAS::clfBuffer *buf_p0_table;
228  BIAS::clfBuffer *buf_x_table;
229  BIAS::clfBuffer *buf_z_table;
230  BIAS::clfBuffer *buf_packet;
231 
232  // Read-Write buffers
233  BIAS::clfBuffer *buf_a;
234  BIAS::clfBuffer *buf_b;
235  BIAS::clfBuffer *buf_n;
236  BIAS::clfBuffer *buf_ir;
237  BIAS::clfBuffer *buf_a_filtered;
238  BIAS::clfBuffer *buf_b_filtered;
239  BIAS::clfBuffer *buf_edge_test;
240  BIAS::clfBuffer *buf_depth;
241  BIAS::clfBuffer *buf_ir_sum;
242  BIAS::clfBuffer *buf_filtered;
243 };
244 
245 void FreenectDepthPacketProcessor::generateOptions_(std::string &options) {
246  std::ostringstream oss;
247  oss.precision(16);
248  oss << std::scientific;
249 
250  oss << " -D BFI_BITMASK=" << "0x180";
251 
252  oss << " -D AB_MULTIPLIER=" << params_.ab_multiplier << "f";
253  oss << " -D AB_MULTIPLIER_PER_FRQ0=" << params_.ab_multiplier_per_frq[0] << "f";
254  oss << " -D AB_MULTIPLIER_PER_FRQ1=" << params_.ab_multiplier_per_frq[1] << "f";
255  oss << " -D AB_MULTIPLIER_PER_FRQ2=" << params_.ab_multiplier_per_frq[2] << "f";
256  oss << " -D AB_OUTPUT_MULTIPLIER=" << params_.ab_output_multiplier << "f";
257 
258  oss << " -D PHASE_IN_RAD0=" << params_.phase_in_rad[0] << "f";
259  oss << " -D PHASE_IN_RAD1=" << params_.phase_in_rad[1] << "f";
260  oss << " -D PHASE_IN_RAD2=" << params_.phase_in_rad[2] << "f";
261 
262  oss << " -D JOINT_BILATERAL_AB_THRESHOLD=" << params_.joint_bilateral_ab_threshold << "f";
263  oss << " -D JOINT_BILATERAL_MAX_EDGE=" << params_.joint_bilateral_max_edge << "f";
264  oss << " -D JOINT_BILATERAL_EXP=" << params_.joint_bilateral_exp << "f";
265  oss << " -D JOINT_BILATERAL_THRESHOLD=" << (params_.joint_bilateral_ab_threshold * params_.joint_bilateral_ab_threshold) / (params_.ab_multiplier * params_.ab_multiplier) << "f";
266  oss << " -D GAUSSIAN_KERNEL_0=" << params_.gaussian_kernel[0] << "f";
267  oss << " -D GAUSSIAN_KERNEL_1=" << params_.gaussian_kernel[1] << "f";
268  oss << " -D GAUSSIAN_KERNEL_2=" << params_.gaussian_kernel[2] << "f";
269  oss << " -D GAUSSIAN_KERNEL_3=" << params_.gaussian_kernel[3] << "f";
270  oss << " -D GAUSSIAN_KERNEL_4=" << params_.gaussian_kernel[4] << "f";
271  oss << " -D GAUSSIAN_KERNEL_5=" << params_.gaussian_kernel[5] << "f";
272  oss << " -D GAUSSIAN_KERNEL_6=" << params_.gaussian_kernel[6] << "f";
273  oss << " -D GAUSSIAN_KERNEL_7=" << params_.gaussian_kernel[7] << "f";
274  oss << " -D GAUSSIAN_KERNEL_8=" << params_.gaussian_kernel[8] << "f";
275 
276  oss << " -D PHASE_OFFSET=" << params_.phase_offset << "f";
277  oss << " -D UNAMBIGIOUS_DIST=" << params_.unambigious_dist << "f";
278  oss << " -D INDIVIDUAL_AB_THRESHOLD=" << params_.individual_ab_threshold << "f";
279  oss << " -D AB_THRESHOLD=" << params_.ab_threshold << "f";
280  oss << " -D AB_CONFIDENCE_SLOPE=" << params_.ab_confidence_slope << "f";
281  oss << " -D AB_CONFIDENCE_OFFSET=" << params_.ab_confidence_offset << "f";
282  oss << " -D MIN_DEALIAS_CONFIDENCE=" << params_.min_dealias_confidence << "f";
283  oss << " -D MAX_DEALIAS_CONFIDENCE=" << params_.max_dealias_confidence << "f";
284 
285  oss << " -D EDGE_AB_AVG_MIN_VALUE=" << params_.edge_ab_avg_min_value << "f";
286  oss << " -D EDGE_AB_STD_DEV_THRESHOLD=" << params_.edge_ab_std_dev_threshold << "f";
287  oss << " -D EDGE_CLOSE_DELTA_THRESHOLD=" << params_.edge_close_delta_threshold << "f";
288  oss << " -D EDGE_FAR_DELTA_THRESHOLD=" << params_.edge_far_delta_threshold << "f";
289  oss << " -D EDGE_MAX_DELTA_THRESHOLD=" << params_.edge_max_delta_threshold << "f";
290  oss << " -D EDGE_AVG_DELTA_THRESHOLD=" << params_.edge_avg_delta_threshold << "f";
291  oss << " -D MAX_EDGE_COUNT=" << params_.max_edge_count << "f";
292 
293  oss << " -D MIN_DEPTH=" << config_.MinDepth * 1000.0f << "f";
294  oss << " -D MAX_DEPTH=" << config_.MaxDepth * 1000.0f << "f";
295  options = oss.str();
296 }
297 
298 BIAS::clfBuffer* createbuffer( BIAS::clfContext *ctx, bool readonly, bool writeonly, size_t size) {
299  BIAS::clfBuffer *ret = ctx->CreateBuffer();
300  ret->SetCached(true);
301  ret->Allocate(size, readonly, writeonly);
302  return ret;
303 }
304 
305 
306 void FreenectDepthPacketProcessor::initialize_() {
307  if (initialized_) return;
308  std::string options;
309  generateOptions_(options);
310  programCL_->AddSource("/OpenCLFramework/cl/opencl_freenect2.cl");
311  programCL_->Build(0, options);
312  programCL_->AddKernel("processPixelStage1");
313  programCL_->AddKernel("filterPixelStage1");
314  programCL_->AddKernel("processPixelStage2");
315  programCL_->AddKernel("filterPixelStage2");
316  programCL_->AddKernel("flipImage");
317  // read only
318 
319  buf_lut11to16 = createbuffer( context_, true, false, 2048 * sizeof(cl_short));
320  buf_p0_table = createbuffer( context_, true, false, image_size * sizeof(cl_float3));
321  buf_x_table = createbuffer( context_, true, false, image_size * sizeof(cl_float));
322  buf_z_table = createbuffer( context_, true, false, image_size * sizeof(cl_float));
323  buf_packet = createbuffer( context_, true, false, ((image_size * 11) / 16) * 10 * sizeof(cl_ushort));
324 
325  // read write
326  buf_a = createbuffer( context_, false, false, image_size * sizeof(cl_float3));
327  buf_b = createbuffer( context_, false, false, image_size * sizeof(cl_float3));
328  buf_n = createbuffer( context_, false, false, image_size * sizeof(cl_float3));
329  buf_ir = createbuffer( context_, false, false, image_size * sizeof(cl_float));
330  buf_a_filtered = createbuffer( context_, false, false, image_size * sizeof(cl_float3));
331  buf_b_filtered = createbuffer( context_, false, false, image_size * sizeof(cl_float3));
332  buf_edge_test = createbuffer( context_, false, false, image_size * sizeof(cl_uchar));
333  buf_depth = createbuffer( context_, false, false, image_size * sizeof(cl_float));
334  buf_ir_sum = createbuffer( context_, false, false, image_size * sizeof(cl_float));
335  buf_filtered = createbuffer( context_, false, false, image_size * sizeof(cl_float));
336 
337  programCL_->KernelSetArgument("processPixelStage1", 0, *buf_lut11to16);
338  programCL_->KernelSetArgument("processPixelStage1", 1, *buf_z_table);
339  programCL_->KernelSetArgument("processPixelStage1", 2, *buf_p0_table);
340  programCL_->KernelSetArgument("processPixelStage1", 3, *buf_packet);
341  programCL_->KernelSetArgument("processPixelStage1", 4, *buf_a);
342  programCL_->KernelSetArgument("processPixelStage1", 5, *buf_b);
343  programCL_->KernelSetArgument("processPixelStage1", 6, *buf_n);
344  programCL_->KernelSetArgument("processPixelStage1", 7, *buf_ir);
345 
346  programCL_->KernelSetArgument("filterPixelStage1", 0, *buf_a);
347  programCL_->KernelSetArgument("filterPixelStage1", 1, *buf_b);
348  programCL_->KernelSetArgument("filterPixelStage1", 2, *buf_n);
349  programCL_->KernelSetArgument("filterPixelStage1", 3, *buf_a_filtered);
350  programCL_->KernelSetArgument("filterPixelStage1", 4, *buf_b_filtered);
351  programCL_->KernelSetArgument("filterPixelStage1", 5, *buf_edge_test);
352 
353  if (config_.EnableBilateralFilter) {
354  programCL_->KernelSetArgument("processPixelStage2", 0, *buf_a_filtered);
355  programCL_->KernelSetArgument("processPixelStage2", 1, *buf_b_filtered);
356  } else {
357  programCL_->KernelSetArgument("processPixelStage2", 0, *buf_a);
358  programCL_->KernelSetArgument("processPixelStage2", 1, *buf_b);
359  }
360  programCL_->KernelSetArgument("processPixelStage2", 2, *buf_x_table);
361  programCL_->KernelSetArgument("processPixelStage2", 3, *buf_z_table);
362  programCL_->KernelSetArgument("processPixelStage2", 4, *buf_depth);
363  programCL_->KernelSetArgument("processPixelStage2", 5, *buf_ir_sum);
364 
365  programCL_->KernelSetArgument("filterPixelStage2", 0, *buf_depth);
366  programCL_->KernelSetArgument("filterPixelStage2", 1, *buf_ir_sum);
367  programCL_->KernelSetArgument("filterPixelStage2", 2, *buf_edge_test);
368  programCL_->KernelSetArgument("filterPixelStage2", 3, *buf_filtered);
369 
370  // reuse buffer for flipping
371  programCL_->KernelSetArgument("flipImage", 1, *buf_ir_sum);
372 
373  buf_lut11to16->WriteToBuffer( lut11to16 );
374  buf_p0_table->WriteToBuffer( p0_table );
375  buf_x_table->WriteToBuffer( x_table );
376  buf_z_table->WriteToBuffer( z_table );
377 
378  initialized_ = true;
379 }
380 
381 FreenectDepthPacketProcessor::FreenectDepthPacketProcessor(BIAS::clfContext *context) : BIAS::clfAlgorithm(context, true) {
382  image_size = 512 * 424;
383  load11To16LutFromFile("11to16.bin");
384  loadXTableFromFile("xTable.bin");
385  loadZTableFromFile("zTable.bin");
386  ir_frame_ = new Frame(526,424,4);
387  depth_frame_ = new Frame(526,424,4);
388  initialized_ = false;
389  config_.MaxDepth = 9.0f;
390 }
391 
392 #define OUT_NAME(FUNCTION) "[OpenCLDepthPacketProcessor::" FUNCTION "] "
393 
394 FreenectDepthPacketProcessor::~FreenectDepthPacketProcessor() {}
395 void FreenectDepthPacketProcessor::setConfiguration(const libfreenect2::DepthPacketProcessor::Config &config) {
396  DepthPacketProcessor::setConfiguration(config);
397  config_ = config;
398 }
399 
400 void FreenectDepthPacketProcessor::loadP0TablesFromCommandResponse(unsigned char* buffer, size_t buffer_length) {
401  libfreenect2::protocol::P0TablesResponse *p0table = (libfreenect2::protocol::P0TablesResponse *)buffer;
402 
403  if(buffer_length < sizeof(libfreenect2::protocol::P0TablesResponse))
404  {
405  std::cerr << OUT_NAME("loadP0TablesFromCommandResponse") "P0Table response too short!" << std::endl;
406  return;
407  }
408  for(int r = 0; r < 424; ++r)
409  {
410  cl_float3 *it = &p0_table[r * 512];
411  const uint16_t *it0 = &p0table->p0table0[r * 512];
412  const uint16_t *it1 = &p0table->p0table1[r * 512];
413  const uint16_t *it2 = &p0table->p0table2[r * 512];
414  for(int c = 0; c < 512; ++c, ++it, ++it0, ++it1, ++it2)
415  {
416  it->s[0] = -((float) * it0) * 0.000031 * M_PI;
417  it->s[1] = -((float) * it1) * 0.000031 * M_PI;
418  it->s[2] = -((float) * it2) * 0.000031 * M_PI;
419  it->s[3] = 0.0f;
420  }
421  }
422 }
423 
424 
425 
426 void FreenectDepthPacketProcessor::loadXTableFromFile(const std::string &filename) {
427  if(!loadBufferFromResources_(filename, (unsigned char *)x_table, image_size * sizeof(float)))
428  {
429  std::cerr << OUT_NAME("loadXTableFromFile") "could not load x table from: " << filename << std::endl;
430  }
431 }
432 void FreenectDepthPacketProcessor::loadZTableFromFile(const std::string &filename) {
433  if(!loadBufferFromResources_(filename, (unsigned char *)z_table, image_size * sizeof(float)))
434  {
435  std::cerr << OUT_NAME("loadZTableFromFile") "could not load z table from: " << filename << std::endl;
436  }
437 }
438 void FreenectDepthPacketProcessor::load11To16LutFromFile(const std::string &filename) {
439  if(!loadBufferFromResources_(filename, (unsigned char *)lut11to16, 2048 * sizeof(cl_ushort)))
440  {
441  std::cerr << OUT_NAME("load11To16LutFromFile") "could not load lut table from: " << filename << std::endl;
442  }
443 }
444 
445 void FreenectDepthPacketProcessor::process(const DepthPacket &packet) {
446  bool has_listener = this->listener_ != 0;
447 
448  initialize_();
449 
450  buf_packet->WriteToBuffer( packet.buffer );
451  context_->RunOn1DRange(*programCL_, "processPixelStage1", image_size, 0);
452  if (config_.EnableBilateralFilter) {
453  context_->RunOn1DRange(*programCL_, "filterPixelStage1", image_size, 0);
454  }
455  context_->RunOn1DRange(*programCL_, "processPixelStage2", image_size, 0);
456  if (config_.EnableEdgeAwareFilter) {
457  context_->RunOn1DRange(*programCL_, "filterPixelStage2", image_size, 0);
458  programCL_->KernelSetArgument("flipImage", 0, *buf_filtered);
459  } else {
460  programCL_->KernelSetArgument("flipImage", 0, *buf_depth);
461  }
462  context_->RunOn1DRange(*programCL_, "flipImage", image_size, 0);
463  buf_ir_sum->ReadFromBuffer( depth_frame_->data );
464  programCL_->KernelSetArgument("flipImage", 0, *buf_ir);
465  context_->RunOn1DRange(*programCL_, "flipImage", image_size, 0);
466  buf_ir_sum->ReadFromBuffer( ir_frame_->data );
467  if(has_listener)
468  {
469  if (listener_->onNewFrame(Frame::Ir, ir_frame_))
470  {
471  //std::cout << "yay, ir frame!" << std::endl;
472  }
473 
474  if (listener_->onNewFrame(Frame::Depth, depth_frame_))
475  {
476  //std::cout << "yay, depth frame!" << std::endl;
477  }
478  }
479 }
480 
481 bool FreenectDepthPacketProcessor::loadBufferFromResources_(const std::string &filename, unsigned char *buffer, const size_t n)
482 {
483  size_t length = 0;
484  const unsigned char *data = NULL;
485 
486  if(!loadResource(filename, &data, &length))
487  {
488  std::cerr << OUT_NAME("loadBufferFromResources") "failed to load resource: " << filename << std::endl;
489  return false;
490  }
491 
492  if(length != n)
493  {
494  std::cerr << OUT_NAME("loadBufferFromResources") "wrong size of resource: " << filename << std::endl;
495  return false;
496  }
497 
498  memcpy(buffer, data, length);
499  return true;
500 }
501 
502 
503 
504 
505 class FreenectPacketPipeline : public PacketPipeline
506 {
507 public:
508  FreenectPacketPipeline();
509  virtual ~FreenectPacketPipeline();
510  virtual void initialize();
511  virtual PacketParser *getRgbPacketParser() const;
512  virtual PacketParser *getIrPacketParser() const;
513 
514  virtual RgbPacketProcessor *getRgbPacketProcessor() const;
515  virtual DepthPacketProcessor *getDepthPacketProcessor() const;
516 protected:
517  RgbPacketStreamParser *rgb_parser_;
518  DepthPacketStreamParser *depth_parser_;
519 
520  RgbPacketProcessor *rgb_processor_;
521  BaseRgbPacketProcessor *async_rgb_processor_;
522  DepthPacketProcessor *depth_processor_;
523  BaseDepthPacketProcessor *async_depth_processor_;
524  BIAS::clfContext ctx_;
525 
526 };
527 
528 FreenectPacketPipeline::FreenectPacketPipeline() {
529  rgb_parser_ = 0;
530  depth_parser_ = 0;
531  rgb_processor_ = 0;
532  async_rgb_processor_ = 0;
533  depth_processor_ = 0;
534  async_depth_processor_ = 0;
535  initialize();
536 }
537 
538 FreenectPacketPipeline::~FreenectPacketPipeline() {
539  delete rgb_parser_;
540  delete depth_parser_;
541  delete rgb_processor_;
542  delete async_rgb_processor_;
543  delete depth_processor_;
544  delete async_depth_processor_;
545 }
546 
547 void FreenectPacketPipeline::initialize() {
548  rgb_parser_ = new RgbPacketStreamParser();
549  depth_parser_ = new DepthPacketStreamParser();
550 
551  rgb_processor_ = new FreenectJpegRgbPacketProcessor();
552  depth_processor_ = new FreenectDepthPacketProcessor(&ctx_);
553 
554  async_rgb_processor_ = new AsyncPacketProcessor<RgbPacket>(rgb_processor_);
555  async_depth_processor_ = new AsyncPacketProcessor<DepthPacket>(depth_processor_);
556 
557  rgb_parser_->setPacketProcessor(async_rgb_processor_);
558  depth_parser_->setPacketProcessor(async_depth_processor_);
559 }
560 
561 PacketPipeline::PacketParser* FreenectPacketPipeline::getRgbPacketParser() const {
562  return rgb_parser_;
563 }
564 
565 PacketPipeline::PacketParser* FreenectPacketPipeline::getIrPacketParser() const {
566  return depth_parser_;
567 }
568 
569 RgbPacketProcessor *FreenectPacketPipeline::getRgbPacketProcessor() const {
570  return rgb_processor_;
571 }
572 
573 DepthPacketProcessor *FreenectPacketPipeline::getDepthPacketProcessor() const {
574  return depth_processor_;
575 }
576 
577 
578 
579 ///////////////////////VideoSource_Kinect2////////////////////////////////
580 /////////////////////////////////////////////////////////////////////////
581 
582 VideoSource_Kinect2::VideoSource_Kinect2(){
583  dev_ = NULL;
584  pipeline_ = NULL;
585  grabMode_ = Kinect2Mode::all;
586  numDevices_ = 0;
587 }
588 
589 VideoSource_Kinect2::~VideoSource_Kinect2(){
590  delete pipeline_;
591  delete dev_;
592  delete freenect2_;
593 }
594 
595 void VideoSource_Kinect2::SetMode(Kinect2Mode mode) {
596  grabMode_ = mode;
597 }
598 
599 bool VideoSource_Kinect2::onNewFrame(Frame::Type type, Frame *frame) {
600  timespec ts;
601  clock_gettime(CLOCK_REALTIME, &ts);
602  ts.tv_sec += 2;
603  if (type == Frame::Color) {
604  int semOk = sem_timedwait(&colorbuffer_.sProd, &ts);
605  if (semOk != 0) {
606  std::cout << "dropping frames!" << std::endl;
607  return true;
608  }
609  colorbuffer_.buffer[ colorbuffer_.curProd ].CopyIn_NoInit ( frame->data );
610  colorbuffer_.curProd = (colorbuffer_.curProd+1) % buffersize_;
611  sem_post(&colorbuffer_.sCons);
612  }
613  if (type == Frame::Depth) {
614  int semOk = sem_timedwait(&depthbuffer_.sProd, &ts);
615  if (semOk != 0) {
616  std::cout << "dropping frames!" << std::endl;
617  return true;
618  }
619  depthbuffer_.buffer[ depthbuffer_.curProd ].CopyIn_NoInit ( frame->data );
620  depthbuffer_.curProd = (depthbuffer_.curProd+1) % buffersize_;
621  sem_post(&depthbuffer_.sCons);
622  }
623  if (type == Frame::Ir) {
624  int semOk = sem_timedwait(&irbuffer_.sProd, &ts);
625  if (semOk != 0) {
626  std::cout << "dropping frames!" << std::endl;
627  return true;
628  }
629  irbuffer_.buffer[ irbuffer_.curProd ].CopyIn_NoInit ( frame->data );
630  irbuffer_.curProd = (irbuffer_.curProd+1) % buffersize_;
631  sem_post(&irbuffer_.sCons);
632  }
633  return true;
634 }
635 
636 int VideoSource_Kinect2::OpenDevice(){
637  return OpenDevice(0);
638 }
639 
640 libfreenect2::Freenect2 *VideoSource_Kinect2::freenect2_ = NULL;
641 
642 int VideoSource_Kinect2::OpenDevice(int device){
643  pipeline_ = new FreenectPacketPipeline();
644  if (freenect2_ == NULL) {
645  freenect2_ = new Freenect2;
646  }
647  GetNumDevices(); // sets numDevices_
648  if (numDevices_ <= device) {
649  BIASERR("could not find device with index " << device);
650  return -1;
651  }
652  dev_ = freenect2_->openDevice(device, pipeline_);
653  return 0;
654 }
655 
656 int VideoSource_Kinect2::CloseDevice(){
657  BIASWARN("todo: CloseDevice() free resources here!");
658 // delete pipeline_;
659 // delete freenect2_;
660 // delete dev_;
661 // freenect2_ = NULL;
662  dev_ = NULL;
663  pipeline_ = NULL;
664  return 0;
665 }
666 
667 
668 int VideoSource_Kinect2::PreGrab(){
669  if (grabMode_ == Kinect2Mode::all || grabMode_ == Kinect2Mode::color) {
670  dev_->setColorFrameListener(this);
671  }
672  if (grabMode_ == Kinect2Mode::all || grabMode_ == Kinect2Mode::depthAndIr) {
673  dev_->setIrAndDepthFrameListener(this);
674  }
675  dev_->start();
676  return 0;
677 }
678 
679 int VideoSource_Kinect2::PostGrab(){
680  dev_->stop();
681  dev_->close();
682  return 0;
683 }
684 
685 int VideoSource_Kinect2::InitImage(ImageBase &Image){
686  Image.Init(1920,1080,3,ImageBase::ST_unsignedchar);
687  return 0;
688 }
689 
690 int VideoSource_Kinect2::InitDepthImage(ImageBase &Image){
691  Image.Init(512,424,1,ImageBase::ST_float);
692  return 0;
693 }
694 
695 int VideoSource_Kinect2::InitIrImage(ImageBase &Image){
696  Image.Init(512,424,1,ImageBase::ST_float);
697  return 0;
698 }
699 
700 int VideoSource_Kinect2::InitIrFullImage(ImageBase &Image){
701  Image.Init(512,424,9,ImageBase::ST_float);
702  return 0;
703 }
704 
705 
706 int VideoSource_Kinect2::GrabSingle(Camera <unsigned char> &image){
707  sem_wait(&colorbuffer_.sCons);
708  image.CopyIn_NoInit( colorbuffer_.buffer[ colorbuffer_.curCons ].GetImageData() );
709  colorbuffer_.curCons = (colorbuffer_.curCons+1) % buffersize_;
710  sem_post(&colorbuffer_.sProd);
711  return 0;
712 }
713 
714 
715 int VideoSource_Kinect2::GrabSingleIR(Camera <float> &image){
716  sem_wait(&irbuffer_.sCons);
717  image.CopyIn_NoInit( irbuffer_.buffer[ irbuffer_.curCons ].GetImageData() );
718  irbuffer_.curCons = (irbuffer_.curCons+1) % buffersize_;
719  sem_post(&irbuffer_.sProd);
720  return 0;
721 }
722 
723 
724 int VideoSource_Kinect2::GrabSingleIRFull(Camera <float> &image){
725  // todo
726  return 0;
727 }
728 
729 int VideoSource_Kinect2::GrabSingleDepth(Camera <float> &image){
730  sem_wait(&depthbuffer_.sCons);
731  image.CopyIn_NoInit( depthbuffer_.buffer[ depthbuffer_.curCons ].GetImageData() );
732  depthbuffer_.curCons = (depthbuffer_.curCons+1) % buffersize_;
733  sem_post(&depthbuffer_.sProd);
734  return 0;
735 }
736 
737 
738 int VideoSource_Kinect2::GetNumDevices(){
739  if (freenect2_ == NULL) {
740  freenect2_ = new Freenect2;
741  }
742  numDevices_ = freenect2_->enumerateDevices();
743  return numDevices_;
744 }
745 
746 int VideoSource_Kinect2::GetCapabilities(VideoSourceCapabilities &caps){
747  return 0;
748 }
749 
750 
751 int VideoSource_Kinect2::GetCapabilities(const char *device, VideoSourceCapabilities &caps){
752  return 0;
753 }
BIAS::VideoSource_Kinect2::SetMode
void SetMode(Kinect2Mode mode)
Definition: VideoSource_Kinect2.cpp:595
BIAS::VideoSource_Kinect2::PostGrab
virtual int PostGrab()
Stop grabbing.
Definition: VideoSource_Kinect2.cpp:679
BIAS::clfContext::CreateBuffer
clfBuffer * CreateBuffer()
create buffer object
Definition: clfContext.hh:79
BIAS::VideoSource_Kinect2::GrabSingleDepth
virtual int GrabSingleDepth(BIAS::Camera< float > &image)
grab the depth image (slow).
Definition: VideoSource_Kinect2.cpp:729
BIAS::clfBuffer::SetCached
void SetCached(bool cache)
Definition: clfBuffer.cpp:109
BIAS::VideoSource_Kinect2::~VideoSource_Kinect2
virtual ~VideoSource_Kinect2()
Definition: VideoSource_Kinect2.cpp:589
BIAS::ImageBase::CopyIn_NoInit
void CopyIn_NoInit(void *data)
Take some data and fill it into the Image.
Definition: ImageBase.cpp:827
BIAS::VideoSource_Kinect2::InitDepthImage
virtual int InitDepthImage(BIAS::ImageBase &Image)
kinect 2 depth image will be 512x424, float in Millimeters
Definition: VideoSource_Kinect2.cpp:690
BIAS::VideoSource_Kinect2::GrabSingleIR
virtual int GrabSingleIR(BIAS::Camera< float > &image)
grab the raw ir data dump (very fast).
Definition: VideoSource_Kinect2.cpp:715
BIAS::VideoSource_Kinect2::GrabSingleIRFull
virtual int GrabSingleIRFull(BIAS::Camera< float > &image)
grab all raw ir images (fast) as a 9 channel float image.
Definition: VideoSource_Kinect2.cpp:724
BIAS::VideoSource_Kinect2::GrabSingle
virtual int GrabSingle(BIAS::Camera< unsigned char > &image)
grab single color image. Note: This image has already been jpeg compressed
Definition: VideoSource_Kinect2.cpp:706
BIAS::ImageBase::ST_float
float image storage type
Definition: ImageBase.hh:118
BIAS::clfBuffer
OpenCL Buffer wrapper.
Definition: clfBuffer.hh:43
BIAS::VideoSource_Kinect2::VideoSource_Kinect2
VideoSource_Kinect2()
Definition: VideoSource_Kinect2.cpp:582
BIAS::VideoSource_Kinect2::InitIrImage
virtual int InitIrImage(BIAS::ImageBase &Image)
kinect 2 ir image will be 512x424, single channel unsigned char
Definition: VideoSource_Kinect2.cpp:695
BIAS::VideoSource_Kinect2::onNewFrame
virtual bool onNewFrame(libfreenect2::Frame::Type type, libfreenect2::Frame *frame)
Definition: VideoSource_Kinect2.cpp:599
BIAS::clfContext
OpenCL Context wrapper.
Definition: clfContext.hh:49
BIAS::VideoSource_Kinect2::Kinect2Mode
Kinect2Mode
Definition: VideoSource_Kinect2.hh:27
BIAS::clfBuffer::Allocate
void Allocate(unsigned int bufsize, bool readonly=false, bool writeonly=false, void *hostptr=NULL, bool copy=false)
Allocation of a memory buffer A memory buffer can be created on device or host, it can be initialized...
Definition: clfBuffer.cpp:45
BIAS::VideoSource_Kinect2::GetCapabilities
virtual int GetCapabilities(VideoSourceCapabilities &caps)
No standard capabilities here.
Definition: VideoSource_Kinect2.cpp:746
BIAS::VideoSource_Kinect2::PreGrab
virtual int PreGrab()
Start grabbing.
Definition: VideoSource_Kinect2.cpp:668
BIAS::Image
The image template class for specific storage types.
Definition: Image.hh:78
BIAS::VideoSource_Kinect2::OpenDevice
virtual int OpenDevice()
Opens first device found, OpenDevice(0)
Definition: VideoSource_Kinect2.cpp:636
BIAS::VideoSource_Kinect2::CloseDevice
virtual int CloseDevice()
Close device, if no other kinect cameras are in use, context is deleted (i.e. close usb bus connectio...
Definition: VideoSource_Kinect2.cpp:656
BIAS::ImageBase::Init
void Init(unsigned int width, unsigned int height, unsigned int nChannels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
Initialize image size and channels.
Definition: ImageBase.cpp:229
BIAS::ImageBase::ST_unsignedchar
(8bit) unsigned char image storage type
Definition: ImageBase.hh:112
BIAS::VideoSourceCapabilities
Checks for VideoSource capabilities.
Definition: VideoSourceCapabilities.hh:17
BIAS::VideoSource_Kinect2::InitIrFullImage
virtual int InitIrFullImage(ImageBase &Image)
all kinect 2 ir phase images in 512x242, nine channel float
Definition: VideoSource_Kinect2.cpp:700
BIAS::ImageBase
This is the base class for images in BIAS.
Definition: ImageBase.hh:102
cl_float4
Definition: cl_platform.h:1035
BIAS::VideoSource_Kinect2::InitImage
virtual int InitImage(BIAS::ImageBase &Image)
kinect 2 color image will be 1920x1080, RGB.
Definition: VideoSource_Kinect2.cpp:685
BIAS::VideoSource_Kinect2::GetNumDevices
virtual int GetNumDevices()
Return the number of available Kinect 2 devices.
Definition: VideoSource_Kinect2.cpp:738
Generated on Tue Feb 6 2018 02:01:46 for Basic Image AlgorithmS Library by   doxygen 1.8.5