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