Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkKinectV2Controller.cpp
Go to the documentation of this file.
1 /*===================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 #include "mitkKinectV2Controller.h"
17 
18 //Kinect V2 SDK
19 #include <Kinect.h>
20 
21 //VTK
22 #include <vtkPolyData.h>
23 #include <vtkCellArray.h>
24 #include <vtkPoints.h>
25 #include <vtkSmartPointer.h>
26 #include <vtkFloatArray.h>
27 #include <vtkPointData.h>
28 #include <vtkMath.h>
29 
30 #include <mitkVector.h>
31 
32 //Taken from official Microsoft SDK samples. Should never be public or part of the class,
33 //because it is just for cleaning up.
34 // Safe release for interfaces
35 template<class Interface>
36 inline void SafeRelease(Interface *& pInterfaceToRelease)
37 {
38  if (pInterfaceToRelease != NULL)
39  {
40  pInterfaceToRelease->Release();
41  pInterfaceToRelease = NULL;
42  }
43 }
44 
45 namespace mitk
46 {
47  class KinectV2Controller::KinectV2ControllerPrivate
48  {
49  public:
50  KinectV2ControllerPrivate();
51  ~KinectV2ControllerPrivate();
52 
53  //bool ErrorText(unsigned int error);
54  IKinectSensor* m_pKinectSensor;
55  IMultiSourceFrameReader* m_pMultiSourceFrameReader;
56  ICoordinateMapper* m_pCoordinateMapper;
57  RGBQUAD* m_pColorRGBX;
58 
59  bool m_ConnectionCheck;
60 
61  int m_DepthCaptureWidth;
62  int m_DepthCaptureHeight;
63 
64  int m_RGBCaptureWidth;
65  int m_RGBCaptureHeight;
66  size_t m_RGBBufferSize;
67  size_t m_DepthBufferSize;
68 
69  CameraSpacePoint* m_CameraCoordinates;
70  ColorSpacePoint* m_ColorPoints;
71  vtkSmartPointer<vtkPolyData> m_PolyData;
72 
73  double m_TriangulationThreshold;
74  bool m_GenerateTriangularMesh;
75  };
76 
77  KinectV2Controller::KinectV2ControllerPrivate::KinectV2ControllerPrivate():
78  m_pKinectSensor(NULL),
79  m_pMultiSourceFrameReader(NULL),
80  m_pCoordinateMapper(NULL),
81  m_pColorRGBX(NULL),
82  m_ConnectionCheck(false),
83  m_DepthCaptureWidth(512),
84  m_DepthCaptureHeight(424),
85  m_RGBCaptureWidth(1920),
86  m_RGBCaptureHeight(1080),
87  m_RGBBufferSize(1920*1080*3),
88  m_DepthBufferSize(sizeof(float)*512*424),
89  m_CameraCoordinates(NULL),
90  m_ColorPoints(NULL),
91  m_PolyData(NULL),
92  m_TriangulationThreshold(0.0),
93  m_GenerateTriangularMesh(false)
94  {
95  // create heap storage for color pixel data in RGBX format
96  m_pColorRGBX = new RGBQUAD[m_RGBCaptureWidth * m_RGBCaptureHeight];
97  //initialize 3D world coordinates and texture coordinates
98  m_CameraCoordinates = new CameraSpacePoint[m_DepthCaptureWidth * m_DepthCaptureHeight];
99  m_ColorPoints = new ColorSpacePoint[m_DepthCaptureWidth * m_DepthCaptureHeight];
100  m_PolyData = vtkSmartPointer<vtkPolyData>::New();
101  }
102 
103  KinectV2Controller::KinectV2ControllerPrivate::~KinectV2ControllerPrivate()
104  {
105  MITK_INFO << "~KinectV2ControllerPrivate";
106  }
107 
108  KinectV2Controller::KinectV2Controller(): d(new KinectV2ControllerPrivate)
109  {
110  }
111 
113  {
114  MITK_INFO << "~KinectV2Controller";
115  delete d;
116  }
117 
119  {
120  if (!d->m_ConnectionCheck)
121  {
122  HRESULT hr;
123  d->m_ConnectionCheck = true;
124 
125  hr = GetDefaultKinectSensor(&d->m_pKinectSensor);
126 
127  if (FAILED(hr))
128  {
129  d->m_ConnectionCheck = false;
130  }
131  else
132  {
133  hr = d->m_pKinectSensor->get_CoordinateMapper(&d->m_pCoordinateMapper);
134  if (FAILED(hr))
135  {
136  d->m_ConnectionCheck = false;
137  }
138  hr = d->m_pKinectSensor->Open();
139  }
140 
141  if (!d->m_pKinectSensor || FAILED(hr))
142  {
143  d->m_ConnectionCheck = false;
144  MITK_WARN << "No Kinect 2 ready!";
145  }
146  else
147  {
148  MITK_INFO << "Kinect 2 succesfully connected";
149  }
150  }
151  return d->m_ConnectionCheck;
152  }
153 
155  {
156  //check if it is already initialized
157  if((d->m_ConnectionCheck) && (d->m_pMultiSourceFrameReader))
158  {
159  return true;
160  }
161  else //initialize the frame reader
162  {
163  HRESULT hr = d->m_pKinectSensor->OpenMultiSourceFrameReader(
164  FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color | FrameSourceTypes::FrameSourceTypes_Infrared,
165  &d->m_pMultiSourceFrameReader);
166  if (SUCCEEDED(hr) && (d->m_pMultiSourceFrameReader))
167  {
168  MITK_INFO << "KinectV2 MultiFrameReader initialized";
169  return true;
170  }
171  }
172  return false;
173  }
174 
175 
177  {
178  // done with depth frame reader
179  MITK_INFO << "CloseConnection";
180  SafeRelease(d->m_pMultiSourceFrameReader);
181 
182  // close the Kinect Sensor
183  if(d->m_pKinectSensor)
184  {
185  d->m_pKinectSensor->Close();
186  }
187 
188  SafeRelease(d->m_pKinectSensor);
189  d->m_ConnectionCheck = false;
190  return true;
191  }
192 
194  {
195  //Acquire lastest frame updates the camera and for
196  //unknown reasons I cannot use it here in UpdateCamera()
197  //without resulting in random crashes of the app.
198  return true;
199  }
200 
201  void KinectV2Controller::GetDistances(float* distances)
202  {
204  {
205  MITK_ERROR << "Unable to initialize MultiFrameReader";
206  return;
207  }
208  IMultiSourceFrame* pMultiSourceFrame = NULL;
209  IDepthFrame* pDepthFrame = NULL;
210 
211  HRESULT hr = -1; //SDK error format
212 
213  static DWORD lastTime = 0;
214  DWORD currentTime = GetTickCount();
215  //Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS.
216  if( unsigned int(currentTime - lastTime) > 33 )
217  {
218  hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
219  lastTime = currentTime;
220  }
221 
222  if (SUCCEEDED(hr))
223  {
224  IDepthFrameReference* pDepthFrameReference = NULL;
225 
226  hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
227  if (SUCCEEDED(hr))
228  {
229  hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
230  }
231  SafeRelease(pDepthFrameReference);
232  }
233 
234  if (SUCCEEDED(hr))
235  {
236  UINT nDepthBufferSize = 0;
237  UINT16 *pDepthBuffer = NULL;
238 
239  if (SUCCEEDED(hr))
240  {
241  hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
242  }
243  if (SUCCEEDED(hr))
244  {
245  UINT pointCount = d->m_DepthCaptureWidth * d->m_DepthCaptureHeight;
246  d->m_pCoordinateMapper->MapDepthFrameToCameraSpace(pointCount, pDepthBuffer, pointCount, d->m_CameraCoordinates);
247  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
248  vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
249  vtkSmartPointer<vtkFloatArray> textureCoordinates = vtkSmartPointer<vtkFloatArray>::New();
250  textureCoordinates->SetNumberOfComponents(2);
251  textureCoordinates->Allocate(pointCount);
252 
253  d->m_pCoordinateMapper->MapDepthFrameToColorSpace(pointCount, pDepthBuffer, pointCount, d->m_ColorPoints);
254 
255  for(int i = 0; i < d->m_DepthCaptureHeight*d->m_DepthCaptureWidth; ++i)
256  {
257  vtkIdType id = points->InsertNextPoint(d->m_CameraCoordinates[i].X, d->m_CameraCoordinates[i].Y, d->m_CameraCoordinates[i].Z);
258  vertices->InsertNextCell(1);
259  vertices->InsertCellPoint(id);
260  distances[i] = static_cast<float>(*pDepthBuffer);
261  ++pDepthBuffer;
262 
263  ColorSpacePoint colorPoint = d->m_ColorPoints[i];
264  // retrieve the depth to color mapping for the current depth pixel
265  int colorInDepthX = (int)(floor(colorPoint.X + 0.5));
266  int colorInDepthY = (int)(floor(colorPoint.Y + 0.5));
267 
268  float xNorm = static_cast<float>(colorInDepthX)/d->m_RGBCaptureWidth;
269  float yNorm = static_cast<float>(colorInDepthY)/d->m_RGBCaptureHeight;
270 
271  // make sure the depth pixel maps to a valid point in color space
272  if ( colorInDepthX >= 0 && colorInDepthX < d->m_RGBCaptureWidth && colorInDepthY >= 0 && colorInDepthY < d->m_RGBCaptureHeight )
273  {
274  textureCoordinates->InsertTuple2(id, xNorm, yNorm);
275  }
276  }
277  d->m_PolyData = vtkSmartPointer<vtkPolyData>::New();
278  d->m_PolyData->SetPoints(points);
279  d->m_PolyData->SetVerts(vertices);
280  d->m_PolyData->GetPointData()->SetTCoords(textureCoordinates);
281  }
282  else
283  {
284  MITK_ERROR << "AccessUnderlyingBuffer";
285  }
286  }
287  SafeRelease(pDepthFrame);
288  SafeRelease(pMultiSourceFrame);
289 
290  if( hr != -1 && !SUCCEEDED(hr) )
291  {
292  //The thread gets here, if the data is requested faster than the device can deliver it.
293  //This may happen from time to time.
294  MITK_DEBUG << "HR result false in KinectV2Controller::GetDistances()";
295  return;
296  }
297  }
298 
299  void KinectV2Controller::GetRgb(unsigned char* rgb)
300  {
302  {
303  MITK_ERROR << "Unable to initialize MultiFrameReader";
304  return;
305  }
306  IMultiSourceFrame* pMultiSourceFrame = NULL;
307  IColorFrame* pColorFrame = NULL;
308 
309  HRESULT hr = -1;
310 
311  static DWORD lastTime = 0;
312  DWORD currentTime = GetTickCount();
313  //Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS.
314  if( unsigned int(currentTime - lastTime) > 33 )
315  {
316  hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
317  lastTime = currentTime;
318  }
319 
320  ColorImageFormat imageFormat = ColorImageFormat_None;
321  UINT nColorBufferSize = 0;
322  RGBQUAD *pColorBuffer = NULL;
323  // get color frame data
324  if (SUCCEEDED(hr))
325  {
326  hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
327  }
328 
329  if (SUCCEEDED(hr))
330  {
331  if (imageFormat == ColorImageFormat_Bgra)
332  {
333  hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
334  }
335  else if (d->m_pColorRGBX)
336  {
337  pColorBuffer = d->m_pColorRGBX;
338  nColorBufferSize = d->m_RGBCaptureWidth * d->m_RGBCaptureHeight * sizeof(RGBQUAD);
339  hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
340  }
341  else
342  {
343  hr = E_FAIL;
344  }
345  if (SUCCEEDED(hr))
346  {
347  for(int i = 0; i < d->m_RGBBufferSize; i+=3)
348  {
349  //convert from BGR to RGB
350  rgb[i+0] = pColorBuffer->rgbRed;
351  rgb[i+1] = pColorBuffer->rgbGreen;
352  rgb[i+2] = pColorBuffer->rgbBlue;
353  ++pColorBuffer;
354  }
355  }
356  }
357  SafeRelease(pColorFrame);
358  SafeRelease(pMultiSourceFrame);
359 
360  if( hr != -1 && !SUCCEEDED(hr) )
361  {
362  //The thread gets here, if the data is requested faster than the device can deliver it.
363  //This may happen from time to time.
364  MITK_DEBUG << "HR result false in KinectV2Controller::GetRgb()";
365  }
366  }
367 
368  void KinectV2Controller::GetAllData(float* distances, float* amplitudes, unsigned char* rgb)
369  {
371  {
372  MITK_ERROR << "Unable to initialize MultiFrameReader";
373  return;
374  }
375 
376  IMultiSourceFrame* pMultiSourceFrame = NULL;
377  IDepthFrame* pDepthFrame = NULL;
378  IColorFrame* pColorFrame = NULL;
379  IInfraredFrame* pInfraRedFrame = NULL;
380 
381  HRESULT hr = -1;
382 
383  static DWORD lastTime = 0;
384  DWORD currentTime = GetTickCount();
385  //Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS.
386  if( unsigned int(currentTime - lastTime) > 33 )
387  {
388  hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
389  lastTime = currentTime;
390  }
391 
392  if (SUCCEEDED(hr))
393  {
394  IDepthFrameReference* pDepthFrameReference = NULL;
395 
396  hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
397  if (SUCCEEDED(hr))
398  {
399  hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
400  }
401  SafeRelease(pDepthFrameReference);
402  }
403 
404  if (SUCCEEDED(hr))
405  {
406  IColorFrameReference* pColorFrameReference = NULL;
407 
408  hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
409  if (SUCCEEDED(hr))
410  {
411  hr = pColorFrameReference->AcquireFrame(&pColorFrame);
412  }
413  SafeRelease(pColorFrameReference);
414  }
415 
416  if (SUCCEEDED(hr))
417  {
418  IInfraredFrameReference* pInfraredFrameReference = NULL;
419 
420  hr = pMultiSourceFrame->get_InfraredFrameReference(&pInfraredFrameReference);
421  if (SUCCEEDED(hr))
422  {
423  hr = pInfraredFrameReference->AcquireFrame(&pInfraRedFrame);
424  }
425  SafeRelease(pInfraredFrameReference);
426  }
427 
428  if (SUCCEEDED(hr))
429  {
430  UINT nDepthBufferSize = 0;
431  UINT16 *pDepthBuffer = NULL;
432  UINT16 *pInfraRedBuffer = NULL;
433 
434  ColorImageFormat imageFormat = ColorImageFormat_None;
435  UINT nColorBufferSize = 0;
436  RGBQUAD *pColorBuffer = NULL;
437 
438  if (SUCCEEDED(hr))
439  {
440  hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
441  }
442  if (SUCCEEDED(hr))
443  {
444  hr = pInfraRedFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pInfraRedBuffer);
445  }
446  if (SUCCEEDED(hr))
447  {
448  UINT pointCount = d->m_DepthCaptureWidth * d->m_DepthCaptureHeight;
449  d->m_pCoordinateMapper->MapDepthFrameToCameraSpace(pointCount, pDepthBuffer, pointCount, d->m_CameraCoordinates);
450  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
451  vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
452  vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New();
453 
454  const double meterfactor = 1000.0;
455 
456  vtkSmartPointer<vtkFloatArray> textureCoordinates = vtkSmartPointer<vtkFloatArray>::New();
457  vtkSmartPointer<vtkIdList> vertexIdList = vtkSmartPointer<vtkIdList>::New();
458  vertexIdList->Allocate(pointCount);
459  vertexIdList->SetNumberOfIds(pointCount);
460  for(unsigned int i = 0; i < pointCount; ++i)
461  {
462  vertexIdList->SetId(i, 0);
463  }
464 
465  std::vector<bool> isPointValid;
466  isPointValid.resize(pointCount);
467  //Allocate the object once else it would automatically allocate new memory
468  //for every vertex and perform a copy which is expensive.
469  vertexIdList->Allocate(pointCount);
470  vertexIdList->SetNumberOfIds(pointCount);
471  textureCoordinates->SetNumberOfComponents(2);
472  textureCoordinates->Allocate(pointCount);
473 
474  d->m_pCoordinateMapper->MapDepthFrameToColorSpace(pointCount, pDepthBuffer, pointCount, d->m_ColorPoints);
475 
476  for(int j = 0; j < d->m_DepthCaptureHeight; ++j)
477  {
478  for(int i = 0; i < d->m_DepthCaptureWidth; ++i)
479  {
480  unsigned int pixelID = i+j*d->m_DepthCaptureWidth;
481  unsigned int inverseid = (d->m_DepthCaptureWidth - i - 1) + j*d->m_DepthCaptureWidth;
482 
483  distances[inverseid] = static_cast<float>(*pDepthBuffer);
484  amplitudes[inverseid] = static_cast<float>(*pInfraRedBuffer);
485  ++pDepthBuffer;
486  ++pInfraRedBuffer;
487 
488  if (d->m_CameraCoordinates[pixelID].Z<=mitk::eps)
489  {
490  isPointValid[pixelID] = false;
491  }
492  else
493  {
494  isPointValid[pixelID] = true;
495 
496  //VTK would insert empty points into the polydata if we use
497  //points->InsertPoint(pixelID, cartesianCoordinates.GetDataPointer()).
498  //If we use points->InsertNextPoint(...) instead, the ID's do not
499  //correspond to the image pixel ID's. Thus, we have to save them
500  //in the vertexIdList.
501  //Kinect SDK delivers world coordinates in meters, so we have to
502  //convert to mm for MITK.
503  vertexIdList->SetId(pixelID, points->InsertNextPoint(-d->m_CameraCoordinates[pixelID].X*meterfactor, -d->m_CameraCoordinates[pixelID].Y*meterfactor, d->m_CameraCoordinates[pixelID].Z*meterfactor));
504 
505  if (d->m_GenerateTriangularMesh)
506  {
507  if((i >= 1) && (j >= 1))
508  {
509  //This little piece of art explains the ID's:
510  //
511  // P(x_1y_1)---P(xy_1)
512  // | |
513  // | |
514  // | |
515  // P(x_1y)-----P(xy)
516  //
517  //We can only start triangulation if we are at vertex (1,1),
518  //because we need the other 3 vertices near this one.
519  //To go one pixel line back in the image array, we have to
520  //subtract 1x xDimension.
521  vtkIdType xy = pixelID;
522  vtkIdType x_1y = pixelID-1;
523  vtkIdType xy_1 = pixelID-d->m_DepthCaptureWidth;
524  vtkIdType x_1y_1 = xy_1-1;
525 
526  //Find the corresponding vertex ID's in the saved vertexIdList:
527  vtkIdType xyV = vertexIdList->GetId(xy);
528  vtkIdType x_1yV = vertexIdList->GetId(x_1y);
529  vtkIdType xy_1V = vertexIdList->GetId(xy_1);
530  vtkIdType x_1y_1V = vertexIdList->GetId(x_1y_1);
531 
532  if (isPointValid[xy]&&isPointValid[x_1y]&&isPointValid[x_1y_1]&&isPointValid[xy_1]) // check if points of cell are valid
533  {
534  double pointXY[3], pointX_1Y[3], pointXY_1[3], pointX_1Y_1[3];
535 
536  points->GetPoint(xyV, pointXY);
537  points->GetPoint(x_1yV, pointX_1Y);
538  points->GetPoint(xy_1V, pointXY_1);
539  points->GetPoint(x_1y_1V, pointX_1Y_1);
540 
541 
542  if( (mitk::Equal(d->m_TriangulationThreshold, 0.0)) || ((vtkMath::Distance2BetweenPoints(pointXY, pointX_1Y) <= d->m_TriangulationThreshold)
543  && (vtkMath::Distance2BetweenPoints(pointXY, pointXY_1) <= d->m_TriangulationThreshold)
544  && (vtkMath::Distance2BetweenPoints(pointX_1Y, pointX_1Y_1) <= d->m_TriangulationThreshold)
545  && (vtkMath::Distance2BetweenPoints(pointXY_1, pointX_1Y_1) <= d->m_TriangulationThreshold)))
546  {
547  polys->InsertNextCell(3);
548  polys->InsertCellPoint(x_1yV);
549  polys->InsertCellPoint(xyV);
550  polys->InsertCellPoint(x_1y_1V);
551 
552  polys->InsertNextCell(3);
553  polys->InsertCellPoint(x_1y_1V);
554  polys->InsertCellPoint(xyV);
555  polys->InsertCellPoint(xy_1V);
556  }
557  else
558  {
559  //We dont want triangulation, but we want to keep the vertex
560  vertices->InsertNextCell(1);
561  vertices->InsertCellPoint(xyV);
562  }
563  }
564  }
565  }
566  else
567  {
568  //We dont want triangulation, we only want vertices
569  vertices->InsertNextCell(1);
570  vertices->InsertCellPoint(vertexIdList->GetId(pixelID));
571  }
572 
573  ColorSpacePoint colorPoint = d->m_ColorPoints[pixelID];
574  // retrieve the depth to color mapping for the current depth pixel
575  int colorInDepthX = (int)(floor(colorPoint.X + 0.5));
576  int colorInDepthY = (int)(floor(colorPoint.Y + 0.5));
577 
578  float xNorm = -static_cast<float>(colorInDepthX)/d->m_RGBCaptureWidth;
579  float yNorm = static_cast<float>(colorInDepthY)/d->m_RGBCaptureHeight;
580 
581  // make sure the depth pixel maps to a valid point in color space
582  if ( colorInDepthX >= 0 && colorInDepthX < d->m_RGBCaptureWidth && colorInDepthY >= 0 && colorInDepthY < d->m_RGBCaptureHeight )
583  {
584  textureCoordinates->InsertTuple2(vertexIdList->GetId(pixelID), xNorm, yNorm);
585  }
586  else
587  {
588  textureCoordinates->InsertTuple2(vertexIdList->GetId(pixelID), 0, 0);
589  }
590  }
591  }
592  }
593 
594  d->m_PolyData = vtkSmartPointer<vtkPolyData>::New();
595  d->m_PolyData->SetPoints(points);
596  d->m_PolyData->SetVerts(vertices);
597  d->m_PolyData->SetPolys(polys);
598  d->m_PolyData->GetPointData()->SetTCoords(textureCoordinates);
599  }
600  else
601  {
602  MITK_ERROR << "AccessUnderlyingBuffer";
603  }
604 
605  // get color frame data
606  if (SUCCEEDED(hr))
607  {
608  hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
609  }
610 
611  if (SUCCEEDED(hr))
612  {
613  if (imageFormat == ColorImageFormat_Bgra)
614  {
615  hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
616  }
617  else if (d->m_pColorRGBX)
618  {
619  pColorBuffer = d->m_pColorRGBX;
620  nColorBufferSize = d->m_RGBCaptureWidth * d->m_RGBCaptureHeight * sizeof(RGBQUAD);
621  hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
622  }
623  else
624  {
625  hr = E_FAIL;
626  }
627  if (SUCCEEDED(hr))
628  {
629  for(int j = 0; j < d->m_RGBCaptureHeight; ++j)
630  {
631  for(int i = 0; i < d->m_RGBCaptureWidth; ++i)
632  {
633  //the buffer has the size of 3*ResolutionX/Y (one for each color value)
634  //thats why die id is multiplied by 3.
635  unsigned int id = ((d->m_RGBCaptureWidth - i - 1) + j*d->m_RGBCaptureWidth)*3;
636  //convert from BGR to RGB
637  rgb[id+0] = pColorBuffer->rgbRed;
638  rgb[id+1] = pColorBuffer->rgbGreen;
639  rgb[id+2] = pColorBuffer->rgbBlue;
640  ++pColorBuffer;
641  }
642  }
643  }
644  }
645  }
646 
647  SafeRelease(pDepthFrame);
648  SafeRelease(pColorFrame);
649  SafeRelease(pInfraRedFrame);
650  SafeRelease(pMultiSourceFrame);
651 
652  if( hr != -1 && !SUCCEEDED(hr) )
653  {
654  //The thread gets here, if the data is requested faster than the device can deliver it.
655  //This may happen from time to time.
656  MITK_DEBUG << "HR result false in KinectV2Controller::GetAllData()";
657  }
658  }
659 
660  void KinectV2Controller::GetAmplitudes( float* amplitudes )
661  {
663  {
664  MITK_ERROR << "Unable to initialize MultiFrameReader";
665  return;
666  }
667 
668  IMultiSourceFrame* pMultiSourceFrame = NULL;
669  IInfraredFrame* pInfraRedFrame = NULL;
670 
671  HRESULT hr = -1;
672 
673  static DWORD lastTime = 0;
674  DWORD currentTime = GetTickCount();
675  //Check if we do not request data faster than 30 FPS. Kinect V2 can only deliver 30 FPS.
676  if( unsigned int(currentTime - lastTime) > 33 )
677  {
678  hr = d->m_pMultiSourceFrameReader->AcquireLatestFrame(&pMultiSourceFrame);
679  lastTime = currentTime;
680  }
681 
682  if (SUCCEEDED(hr))
683  {
684  IInfraredFrameReference* pInfraredFrameReference = NULL;
685 
686  hr = pMultiSourceFrame->get_InfraredFrameReference(&pInfraredFrameReference);
687  if (SUCCEEDED(hr))
688  {
689  hr = pInfraredFrameReference->AcquireFrame(&pInfraRedFrame);
690  }
691  SafeRelease(pInfraredFrameReference);
692  }
693 
694  if (SUCCEEDED(hr))
695  {
696  UINT nDepthBufferSize = 0;
697  UINT16 *pInfraRedBuffer = NULL;
698 
699  if (SUCCEEDED(hr))
700  {
701  hr = pInfraRedFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pInfraRedBuffer);
702  }
703  if (SUCCEEDED(hr))
704  {
705  for(int i = 0; i < d->m_DepthCaptureHeight*d->m_DepthCaptureWidth; ++i)
706  {
707  amplitudes[i] = static_cast<float>(*pInfraRedBuffer);
708  ++pInfraRedBuffer;
709  }
710  }
711  else
712  {
713  MITK_ERROR << "AccessUnderlyingBuffer";
714  }
715  }
716  SafeRelease(pInfraRedFrame);
717  SafeRelease(pMultiSourceFrame);
718 
719  if( hr != -1 && !SUCCEEDED(hr) )
720  {
721  //The thread gets here, if the data is requested faster than the device can deliver it.
722  //This may happen from time to time.
723  MITK_DEBUG << "HR result false in KinectV2Controller::GetAmplitudes()";
724  }
725  }
726 
728  {
729  return d->m_RGBCaptureWidth;
730  }
731 
733  {
734  return d->m_RGBCaptureHeight;
735  }
736 
738  {
739  return d->m_DepthCaptureWidth;
740  }
741 
743  {
744  return d->m_DepthCaptureHeight;
745  }
746 
747  vtkSmartPointer<vtkPolyData> KinectV2Controller::GetVtkPolyData()
748  {
749  return d->m_PolyData;
750  }
751 
753  {
754  d->m_GenerateTriangularMesh = flag;
755  }
756 
757  void KinectV2Controller::SetTriangulationThreshold(double triangulationThreshold)
758  {
759  this->d->m_TriangulationThreshold = triangulationThreshold * triangulationThreshold;
760  }
761 }
virtual bool UpdateCamera()
updates the camera. The update function of the hardware interface is called only when new data is ava...
void SetTriangulationThreshold(double triangulationThreshold)
void GetAllData(float *distances, float *amplitudes, unsigned char *rgb)
convenience method for faster access to distance and rgb data
#define MITK_INFO
Definition: mitkLogMacros.h:22
void SafeRelease(Interface *&pInterfaceToRelease)
#define MITK_ERROR
Definition: mitkLogMacros.h:24
virtual bool OpenCameraConnection()
opens a connection to the Kinect V2 camera.
void GetAmplitudes(float *amplitudes)
#define MITK_DEBUG
Definition: mitkLogMacros.h:26
void GetDistances(float *distances)
acquire new distance data from the Kinect camera
DataCollection - Class to facilitate loading/accessing structured data.
bool InitializeMultiFrameReader()
Setup MultiFrameReader of Kinect V2. This reader can acquire different types of data. Here it is used to acquire depth, RGB and infrared images.
void GetRgb(unsigned char *rgb)
acquire new rgb data from the Kinect camera
#define MITK_WARN
Definition: mitkLogMacros.h:23
vtkSmartPointer< vtkPolyData > GetVtkPolyData()
MITKNEWMODULE_EXPORT bool Equal(mitk::ExampleDataStructure *leftHandSide, mitk::ExampleDataStructure *rightHandSide, mitk::ScalarType eps, bool verbose)
Returns true if the example data structures are considered equal.
virtual bool CloseCameraConnection()
closes the connection to the camera
MITKCORE_EXPORT const ScalarType eps
static itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.