Medical Imaging Interaction Toolkit  2018.4.99-dfa0c14e
Medical Imaging Interaction Toolkit
mitkToFCameraPMDRawDataDevice.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 (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
13 #include "mitkRealTimeClock.h"
14 
15 #ifdef MITK_USE_TOF_PMDCAMBOARD
17 #endif
18 
19 #ifdef MITK_USE_TOF_PMDCAMCUBE
21 #endif
22 
23 #ifdef MITK_USE_TOF_PMDO3
25 #endif
26 
27 #include "itkMultiThreader.h"
28 #include <itksys/SystemTools.hxx>
29 
30 namespace mitk
31 {
33  m_SourceDataBuffer(nullptr), m_SourceDataArray(nullptr), m_ShortSourceData(nullptr),
34  m_OriginControllerWidth(0), m_OriginControllerHeight(0)
35  {
37  }
38 
40  {
41  this->CleanUpSourceData();
42  }
43 
45  {
46  bool ok = false;
47  if (m_Controller.IsNotNull())
48  {
49  ok = m_Controller->OpenCameraConnection();
50  if (ok)
51  {
52  this->m_CaptureWidth = m_Controller->GetInternalCaptureWidth();
53  this->m_CaptureHeight = m_Controller->GetInternalCaptureHeight();
56  this->m_SourceDataSize = m_Controller->GetSourceDataStructSize();
57  this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight;
58 
59  // allocate buffers
60  this->AllocatePixelArrays();
61  this->AllocateSourceData();
62 
63  this->m_CameraConnected = true;
64  }
65  }
66  return ok;
67  }
68 
70  {
71  bool ok = false;
72  if (m_Controller)
73  {
74  ok = m_Controller->CloseCameraConnection();
75  if (ok)
76  {
77  m_CameraConnected = false;
78  }
79  }
80  return ok;
81  }
82 
84  {
86  {
87  // get the first image
88  this->m_Controller->UpdateCamera();
89 
90  this->m_ImageMutex->Lock();
91  this->m_Controller->GetSourceData(this->m_SourceDataBuffer[this->m_FreePos]);
92  this->m_FreePos = (this->m_FreePos+1) % this->m_BufferSize;
93  this->m_CurrentPos = (this->m_CurrentPos+1) % this->m_BufferSize;
94  this->m_ImageSequence++;
95  this->m_ImageMutex->Unlock();
96 
97  this->m_CameraActiveMutex->Lock();
98  this->m_CameraActive = true;
99  this->m_CameraActiveMutex->Unlock();
100 
101  this->m_ThreadID = this->m_MultiThreader->SpawnThread(this->Acquire, this);
102  // wait a little to make sure that the thread is started
103  itksys::SystemTools::Delay(100);
104  }
105  else
106  {
107  MITK_INFO<<"Camera not connected";
108  }
109  }
110 
112  {
113  m_CameraActiveMutex->Lock();
114  m_CameraActive = false;
115  m_CameraActiveMutex->Unlock();
116 
117  itksys::SystemTools::Delay(100);
118  if (m_MultiThreader.IsNotNull())
119  {
120  m_MultiThreader->TerminateThread(m_ThreadID);
121  }
122  // wait a little to make sure that the thread is terminated
123  itksys::SystemTools::Delay(10);
124  }
125 
127  {
128  m_CameraActiveMutex->Lock();
129  bool ok = m_CameraActive;
130  m_CameraActiveMutex->Unlock();
131  return ok;
132  }
133 
135  {
136  if (m_Controller)
137  {
138  m_Controller->UpdateCamera();
139  }
140  }
141 
142  ITK_THREAD_RETURN_TYPE ToFCameraPMDRawDataDevice::Acquire(void* pInfoStruct)
143  {
144  /* extract this pointer from Thread Info structure */
145  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
146  if (pInfo == nullptr)
147  {
148  return ITK_THREAD_RETURN_VALUE;
149  }
150  if (pInfo->UserData == nullptr)
151  {
152  return ITK_THREAD_RETURN_VALUE;
153  }
154  ToFCameraPMDRawDataDevice* toFCameraDevice = (ToFCameraPMDRawDataDevice*)pInfo->UserData;
155  if (toFCameraDevice!=nullptr)
156  {
157  mitk::RealTimeClock::Pointer realTimeClock = mitk::RealTimeClock::New();
158  double t1, t2;
159  t1 = realTimeClock->GetCurrentStamp();
160  int n = 100;
161  bool overflow = false;
162  bool printStatus = false;
163 
164  while (toFCameraDevice->IsCameraActive())
165  {
166  // update the ToF camera
167  toFCameraDevice->UpdateCamera();
168  // get the source data from the camera and write it at the next free position in the buffer
169  // call modified to indicate that cameraDevice was modified
170  toFCameraDevice->Modified();
171 
172  vtkShortArray* channelData = vtkShortArray::New();
173  //toFCameraDevice->m_ImageMutex->Lock();
174  toFCameraDevice->m_Controller->GetShortSourceData(toFCameraDevice->m_ShortSourceData);
175  toFCameraDevice->GetChannelSourceData( toFCameraDevice->m_ShortSourceData, channelData );
176  //toFCameraDevice->m_ImageMutex->Unlock();
177 
178  if(!toFCameraDevice->m_RawDataSource->GetInit())
179  {
180  toFCameraDevice->m_RawDataSource->Initialize(toFCameraDevice->m_OriginControllerWidth,
181  toFCameraDevice->m_OriginControllerHeight,
182  toFCameraDevice->m_Controller->GetModulationFrequency(),
183  toFCameraDevice->GetChannelSize());
184  }
185  toFCameraDevice->m_RawDataSource->SetChannelData(channelData);
186  toFCameraDevice->m_RawDataSource->Update();
187  toFCameraDevice->m_ImageMutex->Lock();
188  toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataArray);
189  toFCameraDevice->m_RawDataSource->GetAllData(toFCameraDevice->m_DistanceArray,
190  toFCameraDevice->m_AmplitudeArray, toFCameraDevice->m_IntensityArray);
191  toFCameraDevice->m_ImageMutex->Unlock();
192 
193  toFCameraDevice->m_FreePos = (toFCameraDevice->m_FreePos+1) % toFCameraDevice->m_BufferSize;
194  toFCameraDevice->m_CurrentPos = (toFCameraDevice->m_CurrentPos+1) % toFCameraDevice->m_BufferSize;
195  toFCameraDevice->m_ImageSequence++;
196  if (toFCameraDevice->m_FreePos == toFCameraDevice->m_CurrentPos)
197  {
198  overflow = true;
199  }
200  if (toFCameraDevice->m_ImageSequence % n == 0)
201  {
202  printStatus = true;
203  }
204  channelData->Delete();
205 
206  if (overflow)
207  {
208  overflow = false;
209  }
210 
211  // print current frame rate
212  if (printStatus)
213  {
214  t2 = realTimeClock->GetCurrentStamp() - t1;
215  MITK_INFO << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence;
216  t1 = realTimeClock->GetCurrentStamp();
217  printStatus = false;
218  }
219 
220  } // end of while loop
221  }
222  return ITK_THREAD_RETURN_VALUE;
223  }
224 
225  void ToFCameraPMDRawDataDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence)
226  {
227  if (m_CameraActive)
228  {
229  memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float));
230  imageSequence = this->m_ImageSequence;
231  }
232  else
233  {
234  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
235  }
236  }
237 
238  void ToFCameraPMDRawDataDevice::GetIntensities(float* intensityArray, int& imageSequence)
239  {
240  if (m_CameraActive)
241  {
242  memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float));
243  imageSequence = this->m_ImageSequence;
244  }
245  else
246  {
247  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
248  }
249  }
250 
251  void ToFCameraPMDRawDataDevice::GetDistances(float* distanceArray, int& imageSequence)
252  {
253  if (m_CameraActive)
254  {
255  memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float));
256  imageSequence = this->m_ImageSequence;
257  }
258  else
259  {
260  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
261  }
262  }
263 
264  void ToFCameraPMDRawDataDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray,
265  int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray)
266  {
267  if (m_CameraActive)
268  {
269  // check for empty buffer
270  if (this->m_ImageSequence < 0)
271  {
272  // buffer empty
273  MITK_INFO << "Buffer empty!! ";
274  capturedImageSequence = this->m_ImageSequence;
275  return;
276  }
277 
278  // determine position of image in buffer
279  int pos = 0;
280  if ((requiredImageSequence < 0) || (requiredImageSequence > this->m_ImageSequence))
281  {
282  capturedImageSequence = this->m_ImageSequence;
283  pos = this->m_CurrentPos;
284  //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence;
285  }
286  else if (requiredImageSequence <= this->m_ImageSequence - this->m_BufferSize)
287  {
288  capturedImageSequence = (this->m_ImageSequence - this->m_BufferSize) + 1;
289  pos = (this->m_CurrentPos + 1) % this->m_BufferSize;
290  //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence;
291  }
292  else // (requiredImageSequence > this->m_ImageSequence - this->m_BufferSize) && (requiredImageSequence <= this->m_ImageSequence)
293 
294  {
295  capturedImageSequence = requiredImageSequence;
296  pos = (this->m_CurrentPos + (10-(this->m_ImageSequence - requiredImageSequence))) % this->m_BufferSize;
297  }
298 
299  memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float));
300  memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float));
301  memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float));
302  memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize);
303  }
304  else
305  {
306  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
307  }
308  }
309 
310  void ToFCameraPMDRawDataDevice::XYAxisFlipImage( float* imageData, float* &flippedData, int xAxis, int yAxis, int dimension )
311  {
312  int captureWidth = this->GetCaptureWidth();
313  int captureHeight = this->GetCaptureHeight();
314 
315  //
316  //flips image around x- axis (horizontal axis)
317  //
318 
319  if(xAxis == 1 && yAxis != 1)
320  {
321  for (int i=0; i<captureHeight; i++)
322  {
323  for (int j=0; j<captureWidth; j++)
324  {
325  for (int k=0; k<dimension; k++)
326  {
327  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[((captureHeight-1-i)*captureWidth*dimension)+((j)*dimension+k)];
328  }
329  }
330  }
331  }
332  //
333  // flips image around y- axis (vertical axis)
334  //
335 
336  if(xAxis!=1 && yAxis == 1)
337  {
338  for (int i=0; i<captureHeight; i++)
339  {
340  for (int j=0; j<captureWidth; j++)
341  {
342  for (int k=0; k<dimension; k++)
343  {
344  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[(i+1)*captureWidth*dimension-1-j*dimension-k];
345  }
346  }
347  }
348  }
349 
350  //
351  //flips image around x- and y- axis
352  //
353 
354  else if(xAxis == 1 && yAxis == 1)
355  {
356  for (int i=0; i<captureHeight; i++)
357  {
358  for (int j=0; j<captureWidth; j++)
359  {
360  for (int k=0; k<dimension; k++)
361  {
362  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[((captureHeight-i)*(captureWidth)*dimension-1)-(j*dimension-k)];
363  }
364  }
365  }
366  }
367  }
368 
369  ToFCameraPMDController::Pointer ToFCameraPMDRawDataDevice::GetController()
370  {
371  return this->m_Controller;
372  }
373 
374  void ToFCameraPMDRawDataDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue )
375  {
376  ToFCameraDevice::SetProperty(propertyKey,propertyValue);
377  this->m_PropertyList->SetProperty(propertyKey, propertyValue);
378  if (strcmp(propertyKey, "ModulationFrequency") == 0)
379  {
380  int modulationFrequency = 0;
381  GetIntProperty(propertyKey, modulationFrequency);
382  m_Controller->SetModulationFrequency(modulationFrequency);
383  }
384  else if (strcmp(propertyKey, "IntegrationTime") == 0)
385  {
386  int integrationTime = 0;
387  GetIntProperty(propertyKey, integrationTime);
388  m_Controller->SetIntegrationTime(integrationTime);
389  }
390  }
391 
393  {
394  if (m_IntensityArray)
395  {
396  delete [] m_IntensityArray;
397  }
398  if (m_DistanceArray)
399  {
400  delete [] m_DistanceArray;
401  }
402  if (m_AmplitudeArray)
403  {
404  delete [] m_AmplitudeArray;
405  }
406  if (m_ShortSourceData)
407  {
408  delete [] m_ShortSourceData;
409  }
410  }
411 
413  {
414  // free memory if it was already allocated
416  // allocate buffer
417  this->m_IntensityArray = new float[this->m_PixelNumber];
418  for(int i=0; i<this->m_PixelNumber; i++) {this->m_IntensityArray[i]=0.0;}
419  this->m_DistanceArray = new float[this->m_PixelNumber];
420  for(int i=0; i<this->m_PixelNumber; i++) {this->m_DistanceArray[i]=0.0;}
421  this->m_AmplitudeArray = new float[this->m_PixelNumber];
422  for(int i=0; i<this->m_PixelNumber; i++) {this->m_AmplitudeArray[i]=0.0;}
423  this->m_ShortSourceData = new short[this->m_SourceDataSize];
424  for(int i=0; i<this->m_SourceDataSize; i++) {this->m_ShortSourceData[i]=0.0;}
425 
426  }
427 
429  {
430  // clean up if array and data have already been allocated
432  // (re-) allocate memory
433  this->m_SourceDataArray = new char[this->m_SourceDataSize];
434  for(int i=0; i<this->m_SourceDataSize; i++) {this->m_SourceDataArray[i]=0;}
435 
436  this->m_SourceDataBuffer = new char*[this->m_MaxBufferSize];
437  for(int i=0; i<this->m_MaxBufferSize; i++)
438  {
439  this->m_SourceDataBuffer[i] = new char[this->m_SourceDataSize];
440  }
441  }
442 
444  {
445  if (m_SourceDataArray)
446  {
447  delete[] m_SourceDataArray;
448  }
449  if (m_SourceDataBuffer)
450  {
451  for(int i=0; i<this->m_MaxBufferSize; i++)
452  {
453  delete[] this->m_SourceDataBuffer[i];
454  }
455  delete[] this->m_SourceDataBuffer;
456  }
457  }
458 }
static ITK_THREAD_RETURN_TYPE Acquire(void *pInfoStruct)
Thread method continuously acquiring images from the ToF hardware.
virtual void SetProperty(const char *propertyKey, BaseProperty *propertyValue)
set a BaseProperty
float k(1.0)
virtual int GetCaptureHeight()
get the currently set capture height
char * m_SourceDataArray
array holding the current PMD source data
virtual bool OnConnectCamera()
opens a connection to the ToF camera
#define MITK_INFO
Definition: mitkLogMacros.h:18
virtual void CleanUpSourceData()
method for cleaning up memory allocated for m_SourceDataArray and m_SourceDataBuffer ...
char ** m_SourceDataBuffer
buffer holding the last acquired images
virtual bool DisconnectCamera()
closes the connection to the camera
int m_SourceDataSize
size of the PMD source data
float * m_IntensityArray
float array holding the intensity image
unsigned int m_OriginControllerHeight
holds the original controller height
int m_PixelNumber
number of pixels in the range image (m_CaptureWidth*m_CaptureHeight)
DataCollection - Class to facilitate loading/accessing structured data.
ToFCameraPMDController::Pointer GetController()
returns the corresponding camera controller
PropertyList::Pointer m_PropertyList
a list of the corresponding properties
Interface for all representations of PMD ToF devices. ToFCameraPMDDevice internally holds an instance...
float * m_DistanceArray
float array holding the distance image
int m_ThreadID
ID of the started thread.
bool GetIntProperty(const char *propertyKey, int &integer)
get an int from the property list
virtual void CleanupPixelArrays()
method for cleanup memory allocated for pixel arrays m_IntensityArray, m_DistanceArray and m_Amplitud...
virtual void UpdateCamera()
updates the camera for image acquisition
virtual void GetDistances(float *distanceArray, int &imageSequence)
gets the distance data from the ToF camera measuring the distance between the camera and the differen...
itk::FastMutexLock::Pointer m_CameraActiveMutex
mutex for the cameraActive flag
itk::MultiThreader::Pointer m_MultiThreader
itk::MultiThreader used for thread handling
int m_FreePos
current position in the buffer which will be filled with data acquired from the hardware ...
Abstract base class for properties.
virtual void SetProperty(const char *propertyKey, BaseProperty *propertyValue)
set a BaseProperty property in the property list
virtual void GetChannelSourceData(short *, vtkShortArray *)
#define MITK_WARN
Definition: mitkLogMacros.h:19
bool m_CameraActive
flag indicating if the camera is currently active or not. Caution: thread safe access only! ...
int m_CurrentPos
current position in the buffer which will be retrieved by the Get methods
bool m_CameraConnected
flag indicating if the camera is successfully connected or not. Caution: thread safe access only! ...
virtual void AllocateSourceData()
method for allocating m_SourceDataArray and m_SourceDataBuffer
ThreadedToFRawDataReconstruction::Pointer m_RawDataSource
bool t2(false)
int m_CaptureWidth
width of the range image (x dimension)
virtual void StartCamera()
starts the continuous updating of the camera. A separate thread updates the source data...
virtual void GetAmplitudes(float *amplitudeArray, int &imageSequence)
gets the amplitude data from the ToF camera as the strength of the active illumination of every pixel...
itk::FastMutexLock::Pointer m_ImageMutex
mutex for images provided by the range camera
virtual bool IsCameraActive()
returns whether the camera is currently active or not
int m_BufferSize
buffer size of the image buffer needed for loss-less acquisition of range data
int m_MaxBufferSize
maximal buffer size needed for initialization of data arrays. Default value is 100.
ToFCameraPMDController::Pointer m_Controller
corresponding CameraController
int m_CaptureHeight
height of the range image (y dimension)
void XYAxisFlipImage(float *imageData, float *&flippedData, int xAxis, int yAxis, int dimension=1)
gets the image data and flips it according to user needs Caution! The user is responsible for allocat...
short * m_ShortSourceData
array holding the current PMD raw data
virtual void GetAllImages(float *distanceArray, float *amplitudeArray, float *intensityArray, char *sourceDataArray, int requiredImageSequence, int &capturedImageSequence, unsigned char *rgbDataArray=nullptr)
gets the 3 images (distance, amplitude, intensity) from the ToF camera. Caution! The user is responsi...
virtual void AllocatePixelArrays()
method for allocating memory for pixel arrays m_IntensityArray, m_DistanceArray and m_AmplitudeArray ...
float * m_AmplitudeArray
float array holding the amplitude image
static Pointer New(void)
instanciates a new, operating-system dependant, instance of mitk::RealTimeClock.
unsigned int m_OriginControllerWidth
holds the original controller width
virtual void StopCamera()
stops the continuous updating of the camera
virtual int GetCaptureWidth()
get the currently set capture width
int m_ImageSequence
counter for acquired images
virtual void GetIntensities(float *intensityArray, int &imageSequence)
gets the intensity data from the ToF camera as a greyscale image. Caution! The user is responsible fo...