Medical Imaging Interaction Toolkit  2016.11.0
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,
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 ===================================================================*/
17 #include "mitkRealTimeClock.h"
18 
19 #ifdef MITK_USE_TOF_PMDCAMBOARD
21 #endif
22 
23 #ifdef MITK_USE_TOF_PMDCAMCUBE
25 #endif
26 
27 #ifdef MITK_USE_TOF_PMDO3
29 #endif
30 
31 #include "itkMultiThreader.h"
32 #include <itksys/SystemTools.hxx>
33 
34 namespace mitk
35 {
37  m_SourceDataBuffer(NULL), m_SourceDataArray(NULL), m_ShortSourceData(NULL),
38  m_OriginControllerWidth(0), m_OriginControllerHeight(0)
39  {
41  }
42 
44  {
45  this->CleanUpSourceData();
46  }
47 
49  {
50  bool ok = false;
51  if (m_Controller.IsNotNull())
52  {
53  ok = m_Controller->OpenCameraConnection();
54  if (ok)
55  {
56  this->m_CaptureWidth = m_Controller->GetInternalCaptureWidth();
57  this->m_CaptureHeight = m_Controller->GetInternalCaptureHeight();
60  this->m_SourceDataSize = m_Controller->GetSourceDataStructSize();
61  this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight;
62 
63  // allocate buffers
64  this->AllocatePixelArrays();
65  this->AllocateSourceData();
66 
67  this->m_CameraConnected = true;
68  }
69  }
70  return ok;
71  }
72 
74  {
75  bool ok = false;
76  if (m_Controller)
77  {
78  ok = m_Controller->CloseCameraConnection();
79  if (ok)
80  {
81  m_CameraConnected = false;
82  }
83  }
84  return ok;
85  }
86 
88  {
90  {
91  // get the first image
92  this->m_Controller->UpdateCamera();
93 
94  this->m_ImageMutex->Lock();
95  this->m_Controller->GetSourceData(this->m_SourceDataBuffer[this->m_FreePos]);
96  this->m_FreePos = (this->m_FreePos+1) % this->m_BufferSize;
97  this->m_CurrentPos = (this->m_CurrentPos+1) % this->m_BufferSize;
98  this->m_ImageSequence++;
99  this->m_ImageMutex->Unlock();
100 
101  this->m_CameraActiveMutex->Lock();
102  this->m_CameraActive = true;
103  this->m_CameraActiveMutex->Unlock();
104 
105  this->m_ThreadID = this->m_MultiThreader->SpawnThread(this->Acquire, this);
106  // wait a little to make sure that the thread is started
107  itksys::SystemTools::Delay(100);
108  }
109  else
110  {
111  MITK_INFO<<"Camera not connected";
112  }
113  }
114 
116  {
117  m_CameraActiveMutex->Lock();
118  m_CameraActive = false;
119  m_CameraActiveMutex->Unlock();
120 
121  itksys::SystemTools::Delay(100);
122  if (m_MultiThreader.IsNotNull())
123  {
124  m_MultiThreader->TerminateThread(m_ThreadID);
125  }
126  // wait a little to make sure that the thread is terminated
127  itksys::SystemTools::Delay(10);
128  }
129 
131  {
132  m_CameraActiveMutex->Lock();
133  bool ok = m_CameraActive;
134  m_CameraActiveMutex->Unlock();
135  return ok;
136  }
137 
139  {
140  if (m_Controller)
141  {
142  m_Controller->UpdateCamera();
143  }
144  }
145 
146  ITK_THREAD_RETURN_TYPE ToFCameraPMDRawDataDevice::Acquire(void* pInfoStruct)
147  {
148  /* extract this pointer from Thread Info structure */
149  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
150  if (pInfo == NULL)
151  {
152  return ITK_THREAD_RETURN_VALUE;
153  }
154  if (pInfo->UserData == NULL)
155  {
156  return ITK_THREAD_RETURN_VALUE;
157  }
158  ToFCameraPMDRawDataDevice* toFCameraDevice = (ToFCameraPMDRawDataDevice*)pInfo->UserData;
159  if (toFCameraDevice!=NULL)
160  {
162  double t1, t2;
163  t1 = realTimeClock->GetCurrentStamp();
164  int n = 100;
165  bool overflow = false;
166  bool printStatus = false;
167 
168  while (toFCameraDevice->IsCameraActive())
169  {
170  // update the ToF camera
171  toFCameraDevice->UpdateCamera();
172  // get the source data from the camera and write it at the next free position in the buffer
173  // call modified to indicate that cameraDevice was modified
174  toFCameraDevice->Modified();
175 
176  vtkShortArray* channelData = vtkShortArray::New();
177  //toFCameraDevice->m_ImageMutex->Lock();
178  toFCameraDevice->m_Controller->GetShortSourceData(toFCameraDevice->m_ShortSourceData);
179  toFCameraDevice->GetChannelSourceData( toFCameraDevice->m_ShortSourceData, channelData );
180  //toFCameraDevice->m_ImageMutex->Unlock();
181 
182  if(!toFCameraDevice->m_RawDataSource->GetInit())
183  {
184  toFCameraDevice->m_RawDataSource->Initialize(toFCameraDevice->m_OriginControllerWidth,
185  toFCameraDevice->m_OriginControllerHeight,
186  toFCameraDevice->m_Controller->GetModulationFrequency(),
187  toFCameraDevice->GetChannelSize());
188  }
189  toFCameraDevice->m_RawDataSource->SetChannelData(channelData);
190  toFCameraDevice->m_RawDataSource->Update();
191  toFCameraDevice->m_ImageMutex->Lock();
192  toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataArray);
193  toFCameraDevice->m_RawDataSource->GetAllData(toFCameraDevice->m_DistanceArray,
194  toFCameraDevice->m_AmplitudeArray, toFCameraDevice->m_IntensityArray);
195  toFCameraDevice->m_ImageMutex->Unlock();
196 
197  toFCameraDevice->m_FreePos = (toFCameraDevice->m_FreePos+1) % toFCameraDevice->m_BufferSize;
198  toFCameraDevice->m_CurrentPos = (toFCameraDevice->m_CurrentPos+1) % toFCameraDevice->m_BufferSize;
199  toFCameraDevice->m_ImageSequence++;
200  if (toFCameraDevice->m_FreePos == toFCameraDevice->m_CurrentPos)
201  {
202  overflow = true;
203  }
204  if (toFCameraDevice->m_ImageSequence % n == 0)
205  {
206  printStatus = true;
207  }
208  channelData->Delete();
209 
210  if (overflow)
211  {
212  overflow = false;
213  }
214 
215  // print current frame rate
216  if (printStatus)
217  {
218  t2 = realTimeClock->GetCurrentStamp() - t1;
219  MITK_INFO << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence;
220  t1 = realTimeClock->GetCurrentStamp();
221  printStatus = false;
222  }
223 
224  } // end of while loop
225  }
226  return ITK_THREAD_RETURN_VALUE;
227  }
228 
229  void ToFCameraPMDRawDataDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence)
230  {
231  if (m_CameraActive)
232  {
233  memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float));
234  imageSequence = this->m_ImageSequence;
235  }
236  else
237  {
238  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
239  }
240  }
241 
242  void ToFCameraPMDRawDataDevice::GetIntensities(float* intensityArray, int& imageSequence)
243  {
244  if (m_CameraActive)
245  {
246  memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float));
247  imageSequence = this->m_ImageSequence;
248  }
249  else
250  {
251  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
252  }
253  }
254 
255  void ToFCameraPMDRawDataDevice::GetDistances(float* distanceArray, int& imageSequence)
256  {
257  if (m_CameraActive)
258  {
259  memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float));
260  imageSequence = this->m_ImageSequence;
261  }
262  else
263  {
264  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
265  }
266  }
267 
268  void ToFCameraPMDRawDataDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray,
269  int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray)
270  {
271  if (m_CameraActive)
272  {
273  // check for empty buffer
274  if (this->m_ImageSequence < 0)
275  {
276  // buffer empty
277  MITK_INFO << "Buffer empty!! ";
278  capturedImageSequence = this->m_ImageSequence;
279  return;
280  }
281 
282  // determine position of image in buffer
283  int pos = 0;
284  if ((requiredImageSequence < 0) || (requiredImageSequence > this->m_ImageSequence))
285  {
286  capturedImageSequence = this->m_ImageSequence;
287  pos = this->m_CurrentPos;
288  //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence;
289  }
290  else if (requiredImageSequence <= this->m_ImageSequence - this->m_BufferSize)
291  {
292  capturedImageSequence = (this->m_ImageSequence - this->m_BufferSize) + 1;
293  pos = (this->m_CurrentPos + 1) % this->m_BufferSize;
294  //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence;
295  }
296  else // (requiredImageSequence > this->m_ImageSequence - this->m_BufferSize) && (requiredImageSequence <= this->m_ImageSequence)
297 
298  {
299  capturedImageSequence = requiredImageSequence;
300  pos = (this->m_CurrentPos + (10-(this->m_ImageSequence - requiredImageSequence))) % this->m_BufferSize;
301  }
302 
303  memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float));
304  memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float));
305  memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float));
306  memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize);
307  }
308  else
309  {
310  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
311  }
312  }
313 
314  void ToFCameraPMDRawDataDevice::XYAxisFlipImage( float* imageData, float* &flippedData, int xAxis, int yAxis, int dimension )
315  {
316  int captureWidth = this->GetCaptureWidth();
317  int captureHeight = this->GetCaptureHeight();
318 
319  //
320  //flips image around x- axis (horizontal axis)
321  //
322 
323  if(xAxis == 1 && yAxis != 1)
324  {
325  for (int i=0; i<captureHeight; i++)
326  {
327  for (int j=0; j<captureWidth; j++)
328  {
329  for (int k=0; k<dimension; k++)
330  {
331  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[((captureHeight-1-i)*captureWidth*dimension)+((j)*dimension+k)];
332  }
333  }
334  }
335  }
336  //
337  // flips image around y- axis (vertical axis)
338  //
339 
340  if(xAxis!=1 && yAxis == 1)
341  {
342  for (int i=0; i<captureHeight; i++)
343  {
344  for (int j=0; j<captureWidth; j++)
345  {
346  for (int k=0; k<dimension; k++)
347  {
348  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[(i+1)*captureWidth*dimension-1-j*dimension-k];
349  }
350  }
351  }
352  }
353 
354  //
355  //flips image around x- and y- axis
356  //
357 
358  else if(xAxis == 1 && yAxis == 1)
359  {
360  for (int i=0; i<captureHeight; i++)
361  {
362  for (int j=0; j<captureWidth; j++)
363  {
364  for (int k=0; k<dimension; k++)
365  {
366  flippedData[i*captureWidth*dimension+j*dimension+k] = imageData[((captureHeight-i)*(captureWidth)*dimension-1)-(j*dimension-k)];
367  }
368  }
369  }
370  }
371  }
372 
374  {
375  return this->m_Controller;
376  }
377 
378  void ToFCameraPMDRawDataDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue )
379  {
380  ToFCameraDevice::SetProperty(propertyKey,propertyValue);
381  this->m_PropertyList->SetProperty(propertyKey, propertyValue);
382  if (strcmp(propertyKey, "ModulationFrequency") == 0)
383  {
384  int modulationFrequency = 0;
385  GetIntProperty(propertyKey, modulationFrequency);
386  m_Controller->SetModulationFrequency(modulationFrequency);
387  }
388  else if (strcmp(propertyKey, "IntegrationTime") == 0)
389  {
390  int integrationTime = 0;
391  GetIntProperty(propertyKey, integrationTime);
392  m_Controller->SetIntegrationTime(integrationTime);
393  }
394  }
395 
397  {
398  if (m_IntensityArray)
399  {
400  delete [] m_IntensityArray;
401  }
402  if (m_DistanceArray)
403  {
404  delete [] m_DistanceArray;
405  }
406  if (m_AmplitudeArray)
407  {
408  delete [] m_AmplitudeArray;
409  }
410  if (m_ShortSourceData)
411  {
412  delete [] m_ShortSourceData;
413  }
414  }
415 
417  {
418  // free memory if it was already allocated
420  // allocate buffer
421  this->m_IntensityArray = new float[this->m_PixelNumber];
422  for(int i=0; i<this->m_PixelNumber; i++) {this->m_IntensityArray[i]=0.0;}
423  this->m_DistanceArray = new float[this->m_PixelNumber];
424  for(int i=0; i<this->m_PixelNumber; i++) {this->m_DistanceArray[i]=0.0;}
425  this->m_AmplitudeArray = new float[this->m_PixelNumber];
426  for(int i=0; i<this->m_PixelNumber; i++) {this->m_AmplitudeArray[i]=0.0;}
427  this->m_ShortSourceData = new short[this->m_SourceDataSize];
428  for(int i=0; i<this->m_SourceDataSize; i++) {this->m_ShortSourceData[i]=0.0;}
429 
430  }
431 
433  {
434  // clean up if array and data have already been allocated
436  // (re-) allocate memory
437  this->m_SourceDataArray = new char[this->m_SourceDataSize];
438  for(int i=0; i<this->m_SourceDataSize; i++) {this->m_SourceDataArray[i]=0;}
439 
440  this->m_SourceDataBuffer = new char*[this->m_MaxBufferSize];
441  for(int i=0; i<this->m_MaxBufferSize; i++)
442  {
443  this->m_SourceDataBuffer[i] = new char[this->m_SourceDataSize];
444  }
445  }
446 
448  {
449  if (m_SourceDataArray)
450  {
451  delete[] m_SourceDataArray;
452  }
453  if (m_SourceDataBuffer)
454  {
455  for(int i=0; i<this->m_MaxBufferSize; i++)
456  {
457  delete[] this->m_SourceDataBuffer[i];
458  }
459  delete[] this->m_SourceDataBuffer;
460  }
461  }
462 }
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
virtual int GetCaptureHeight()
get the currently set capture height
itk::SmartPointer< Self > Pointer
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:22
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 GetAllImages(float *distanceArray, float *amplitudeArray, float *intensityArray, char *sourceDataArray, int requiredImageSequence, int &capturedImageSequence, unsigned char *rgbDataArray=NULL)
gets the 3 images (distance, amplitude, intensity) from the ToF camera. Caution! The user is responsi...
virtual void GetChannelSourceData(short *, vtkShortArray *)
#define MITK_WARN
Definition: mitkLogMacros.h:23
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
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 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...
static itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.