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