Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkToFCameraPMDDevice.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 "mitkToFCameraPMDDevice.h"
17 #include "mitkRealTimeClock.h"
18 
19 #include "itkMultiThreader.h"
20 #include <itksys/SystemTools.hxx>
21 
22 namespace mitk
23 {
25  m_SourceDataBuffer(NULL), m_SourceDataArray(NULL)
26  {
27  }
28 
30  {
31  this->CleanUpSourceData();
33  }
34 
36  {
37  bool ok = false;
38  if (m_Controller)
39  {
40  ok = m_Controller->OpenCameraConnection();
41  if (ok)
42  {
43  this->m_CaptureWidth = m_Controller->GetInternalCaptureWidth();
44  this->m_CaptureHeight = m_Controller->GetInternalCaptureHeight();
45  this->m_SourceDataSize = m_Controller->GetSourceDataStructSize();
46  this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight;
47 
48  // allocate buffers
49  this->AllocatePixelArrays();
50  this->AllocateSourceData();
51 
52  this->m_CameraConnected = true;
53  }
54  }
55  return ok;
56  }
57 
59  {
60  bool ok = false;
61  if (m_Controller)
62  {
63  ok = m_Controller->CloseCameraConnection();
64 
65  if (ok)
66  {
67  m_CameraConnected = false;
68  }
69 
70  }
71  return ok;
72  }
73 
75  {
77  {
78  // get the first image
79  this->m_Controller->UpdateCamera();
80  this->m_ImageMutex->Lock();
81  //this->m_Controller->GetSourceData(this->m_SourceDataArray);
82  this->m_Controller->GetSourceData(this->m_SourceDataBuffer[this->m_FreePos]);
83  this->m_FreePos = (this->m_FreePos+1) % this->m_BufferSize;
84  this->m_CurrentPos = (this->m_CurrentPos+1) % this->m_BufferSize;
85  this->m_ImageSequence++;
86  this->m_ImageMutex->Unlock();
87 
88  this->m_CameraActiveMutex->Lock();
89  this->m_CameraActive = true;
90  this->m_CameraActiveMutex->Unlock();
91  this->m_ThreadID = this->m_MultiThreader->SpawnThread(this->Acquire, this);
92  // wait a little to make sure that the thread is started
93  itksys::SystemTools::Delay(10);
94  }
95  else
96  {
97  MITK_INFO<<"Camera not connected";
98  }
99  }
100 
102  {
103  if (m_Controller)
104  {
105  m_Controller->UpdateCamera();
106  }
107  }
108 
109  ITK_THREAD_RETURN_TYPE ToFCameraPMDDevice::Acquire(void* pInfoStruct)
110  {
111  /* extract this pointer from Thread Info structure */
112  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
113  if (pInfo == NULL)
114  {
115  return ITK_THREAD_RETURN_VALUE;
116  }
117  if (pInfo->UserData == NULL)
118  {
119  return ITK_THREAD_RETURN_VALUE;
120  }
121  ToFCameraPMDDevice* toFCameraDevice = (ToFCameraPMDDevice*)pInfo->UserData;
122  if (toFCameraDevice!=NULL)
123  {
124  mitk::RealTimeClock::Pointer realTimeClock;
125  realTimeClock = mitk::RealTimeClock::New();
126  double t1, t2;
127  t1 = realTimeClock->GetCurrentStamp();
128  int n = 100;
129  bool overflow = false;
130  bool printStatus = false;
131  while (toFCameraDevice->IsCameraActive())
132  {
133  // update the ToF camera
134  toFCameraDevice->UpdateCamera();
135  // get the source data from the camera and write it at the next free position in the buffer
136  toFCameraDevice->m_ImageMutex->Lock();
137  toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataBuffer[toFCameraDevice->m_FreePos]);
138 
139 
140  toFCameraDevice->m_ImageMutex->Unlock();
141  // call modified to indicate that cameraDevice was modified
142  toFCameraDevice->Modified();
143 
144  toFCameraDevice->m_FreePos = (toFCameraDevice->m_FreePos+1) % toFCameraDevice->m_BufferSize;
145  toFCameraDevice->m_CurrentPos = (toFCameraDevice->m_CurrentPos+1) % toFCameraDevice->m_BufferSize;
146  toFCameraDevice->m_ImageSequence++;
147  if (toFCameraDevice->m_FreePos == toFCameraDevice->m_CurrentPos)
148  {
149  overflow = true;
150  }
151  if (toFCameraDevice->m_ImageSequence % n == 0)
152  {
153  printStatus = true;
154  }
155  if (overflow)
156  {
157  overflow = false;
158  }
159  if (printStatus)
160  {
161  t2 = realTimeClock->GetCurrentStamp() - t1;
162  MITK_INFO << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence;
163  t1 = realTimeClock->GetCurrentStamp();
164  printStatus = false;
165  }
166  } // end of while loop
167  }
168  return ITK_THREAD_RETURN_VALUE;
169  }
170 
171  void ToFCameraPMDDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence)
172  {
173  if (m_CameraActive)
174  {
175  // 1) copy the image buffer
176  // 2) Flip around y- axis (vertical axis)
177  m_ImageMutex->Lock();
178  this->m_Controller->GetAmplitudes(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_AmplitudeArray);
179  m_ImageMutex->Unlock();
180 
181  for (int i=0; i<this->m_CaptureHeight; i++)
182  {
183  for (int j=0; j<this->m_CaptureWidth; j++)
184  {
185  amplitudeArray[i*this->m_CaptureWidth+j] = this->m_AmplitudeArray[(i+1)*this->m_CaptureWidth-1-j];
186  }
187  }
188  imageSequence = this->m_ImageSequence;
189  }
190  else
191  {
192  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
193  }
194  }
195 
196  void ToFCameraPMDDevice::GetIntensities(float* intensityArray, int& imageSequence)
197  {
198  if (m_CameraActive)
199  {
200  // 1) copy the image buffer
201  // 2) Flip around y- axis (vertical axis)
202 
203  m_ImageMutex->Lock();
204  this->m_Controller->GetIntensities(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_IntensityArray);
205  m_ImageMutex->Unlock();
206 
207  for (int i=0; i<this->m_CaptureHeight; i++)
208  {
209  for (int j=0; j<this->m_CaptureWidth; j++)
210  {
211  intensityArray[i*this->m_CaptureWidth+j] = this->m_IntensityArray[(i+1)*this->m_CaptureWidth-1-j];
212  }
213  }
214  imageSequence = this->m_ImageSequence;
215  }
216  else
217  {
218  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
219  }
220  m_ImageMutex->Unlock();
221  }
222 
223  void ToFCameraPMDDevice::GetDistances(float* distanceArray, int& imageSequence)
224  {
225  if (m_CameraActive)
226  {
227  // 1) copy the image buffer
228  // 2) convert the distance values from m to mm
229  // 3) Flip around y- axis (vertical axis)
230  m_ImageMutex->Lock();
231  this->m_Controller->GetDistances(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_DistanceArray);
232  m_ImageMutex->Unlock();
233 
234  for (int i=0; i<this->m_CaptureHeight; i++)
235  {
236  for (int j=0; j<this->m_CaptureWidth; j++)
237  {
238  distanceArray[i*this->m_CaptureWidth+j] = this->m_DistanceArray[(i+1)*this->m_CaptureWidth-1-j];
239  }
240  }
241  imageSequence = this->m_ImageSequence;
242  }
243  else
244  {
245  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
246  }
247  }
248 
249  void ToFCameraPMDDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray,
250  int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray)
251  {
252  if (m_CameraActive)
253  {
254  // 1) copy the image buffer
255  // 2) convert the distance values from m to mm
256  // 3) Flip around y- axis (vertical axis)
257 
258  // check for empty buffer
259  if (this->m_ImageSequence < 0)
260  {
261  // buffer empty
262  MITK_INFO << "Buffer empty!! ";
263  capturedImageSequence = this->m_ImageSequence;
264  return;
265  }
266  // determine position of image in buffer
267  int pos = 0;
268  if ((requiredImageSequence < 0) || (requiredImageSequence > this->m_ImageSequence))
269  {
270  capturedImageSequence = this->m_ImageSequence;
271  pos = this->m_CurrentPos;
272  }
273  else if (requiredImageSequence <= this->m_ImageSequence - this->m_BufferSize)
274  {
275  capturedImageSequence = (this->m_ImageSequence - this->m_BufferSize) + 1;
276  pos = (this->m_CurrentPos + 1) % this->m_BufferSize;
277  }
278  else // (requiredImageSequence > this->m_ImageSequence - this->m_BufferSize) && (requiredImageSequence <= this->m_ImageSequence)
279 
280  {
281  capturedImageSequence = requiredImageSequence;
282  pos = (this->m_CurrentPos + (10-(this->m_ImageSequence - requiredImageSequence))) % this->m_BufferSize;
283  }
284 
285  m_ImageMutex->Lock();
286  this->m_Controller->GetDistances(this->m_SourceDataBuffer[pos], this->m_DistanceArray);
287  this->m_Controller->GetAmplitudes(this->m_SourceDataBuffer[pos], this->m_AmplitudeArray);
288  this->m_Controller->GetIntensities(this->m_SourceDataBuffer[pos], this->m_IntensityArray);
289  memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize);
290  m_ImageMutex->Unlock();
291 
292  memcpy(distanceArray, this->m_DistanceArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float));
293  memcpy(intensityArray, this->m_IntensityArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float));
294  memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float));
295  }
296  else
297  {
298  MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active.";
299  }
300  }
301 
303  {
304  return this->m_Controller;
305  }
306 
307  void ToFCameraPMDDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue )
308  {
309  ToFCameraDevice::SetProperty(propertyKey,propertyValue);
310  if (strcmp(propertyKey, "ModulationFrequency") == 0)
311  {
312  int modulationFrequency = 0;
313  GetIntProperty(propertyKey, modulationFrequency);
314  modulationFrequency = m_Controller->SetModulationFrequency(modulationFrequency);
315  static_cast<mitk::IntProperty*>(propertyValue)->SetValue(modulationFrequency);
316  this->m_PropertyList->SetProperty(propertyKey, propertyValue );
317  }
318  else if (strcmp(propertyKey, "IntegrationTime") == 0)
319  {
320  int integrationTime = 0;
321  GetIntProperty(propertyKey, integrationTime);
322  integrationTime = m_Controller->SetIntegrationTime(integrationTime);
323  static_cast<mitk::IntProperty*>(propertyValue)->SetValue(integrationTime);
324  this->m_PropertyList->SetProperty(propertyKey, propertyValue );
325 
326  }
327  }
328 
330  {
331  // clean up if array and data have already been allocated
333  // (re-) allocate memory
334  this->m_SourceDataArray = new char[this->m_SourceDataSize];
335  for(int i=0; i<this->m_SourceDataSize; i++) {this->m_SourceDataArray[i]=0;}
336 
337  this->m_SourceDataBuffer = new char*[this->m_MaxBufferSize];
338  for(int i=0; i<this->m_MaxBufferSize; i++)
339  {
340  this->m_SourceDataBuffer[i] = new char[this->m_SourceDataSize];
341  }
342  }
343 
345  {
346  if (m_SourceDataArray)
347  {
348  delete[] m_SourceDataArray;
349  }
350  if (m_SourceDataBuffer)
351  {
352  for(int i=0; i<this->m_MaxBufferSize; i++)
353  {
354  delete[] this->m_SourceDataBuffer[i];
355  }
356  delete[] this->m_SourceDataBuffer;
357  }
358  }
359 }
itk::SmartPointer< Self > Pointer
#define MITK_INFO
Definition: mitkLogMacros.h:22
char * m_SourceDataArray
array holding the current PMD source data
ToFCameraPMDController::Pointer GetController()
returns the corresponding camera controller
virtual void UpdateCamera()
updated the controller hold by this device
int m_SourceDataSize
size of the PMD source data
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...
float * m_IntensityArray
float array holding the intensity image
int m_PixelNumber
number of pixels in the range image (m_CaptureWidth*m_CaptureHeight)
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...
DataCollection - Class to facilitate loading/accessing structured data.
PropertyList::Pointer m_PropertyList
a list of the corresponding properties
char ** m_SourceDataBuffer
buffer holding the last acquired images
float * m_DistanceArray
float array holding the distance image
int m_ThreadID
ID of the started thread.
virtual void StartCamera()
starts the continuous updating of the camera. A separate thread updates the source data...
bool GetIntProperty(const char *propertyKey, int &integer)
get an int from the property list
itk::FastMutexLock::Pointer m_CameraActiveMutex
mutex for the cameraActive flag
itk::MultiThreader::Pointer m_MultiThreader
itk::MultiThreader used for thread handling
ToFCameraPMDController::Pointer m_Controller
corresponding CameraController
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
#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 GetAmplitudes(float *amplitudeArray, int &imageSequence)
gets the amplitude data from the ToF camera as the strength of the active illumination of every pixel...
virtual bool DisconnectCamera()
closes the connection to the camera
int m_CaptureWidth
width of the range image (x dimension)
virtual void SetProperty(const char *propertyKey, BaseProperty *propertyValue)
set a BaseProperty
itk::FastMutexLock::Pointer m_ImageMutex
mutex for images provided by the range camera
virtual void AllocatePixelArrays()
method for allocating memory for pixel arrays m_IntensityArray, m_DistanceArray and m_AmplitudeArray ...
virtual void CleanUpSourceData()
method for cleaning up memory allocated for m_SourceDataArray and m_SourceDataBuffer ...
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.
virtual void GetDistances(float *distanceArray, int &imageSequence)
gets the distance data from the ToF camera measuring the distance between the camera and the differen...
int m_CaptureHeight
height of the range image (y dimension)
virtual void AllocateSourceData()
method for allocating m_SourceDataArray and m_SourceDataBuffer
static ITK_THREAD_RETURN_TYPE Acquire(void *pInfoStruct)
Thread method continuously acquiring images from the ToF hardware.
float * m_AmplitudeArray
float array holding the amplitude image
static Pointer New(void)
instanciates a new, operating-system dependant, instance of mitk::RealTimeClock.
virtual void CleanupPixelArrays()
method for cleanup memory allocated for pixel arrays m_IntensityArray, m_DistanceArray and m_Amplitud...
Interface for all representations of PMD ToF devices. ToFCameraPMDDevice internally holds an instance...
int m_ImageSequence
counter for acquired images
virtual bool IsCameraActive()
returns true if the camera is connected and started
virtual bool OnConnectCamera()
opens a connection to the ToF camera