diff --git a/Modules/ToFHardware/mitkToFCameraMITKPlayerController.cpp b/Modules/ToFHardware/mitkToFCameraMITKPlayerController.cpp index e3b07eca7b..62363ebc48 100644 --- a/Modules/ToFHardware/mitkToFCameraMITKPlayerController.cpp +++ b/Modules/ToFHardware/mitkToFCameraMITKPlayerController.cpp @@ -1,331 +1,336 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include #include // mitk includes #include "mitkItkImageFileReader.h" #include "mitkIOUtil.h" #include #include "mitkImageReadAccessor.h" namespace mitk { ToFCameraMITKPlayerController::ToFCameraMITKPlayerController() : m_PixelNumber(0), m_RGBPixelNumber(0), m_NumberOfBytes(0), m_NumberOfRGBBytes(0), m_CaptureWidth(0), m_CaptureHeight(0), m_RGBCaptureWidth(0), m_RGBCaptureHeight(0), m_ConnectionCheck(false), m_InputFileName(""), m_ToFImageType(ToFImageType3D), m_DistanceImage(0), m_AmplitudeImage(0), m_IntensityImage(0), m_RGBImage(0), m_DistanceInfile(NULL), m_AmplitudeInfile(NULL), m_IntensityInfile(NULL), m_RGBInfile(NULL), m_IntensityArray(NULL), m_DistanceArray(NULL), m_AmplitudeArray(NULL), m_RGBArray(NULL), m_DistanceImageFileName(""), m_AmplitudeImageFileName(""), m_IntensityImageFileName(""), m_RGBImageFileName(""), m_PixelStartInFile(0), m_CurrentFrame(-1), m_NumOfFrames(0) { m_ImageStatus = std::vector(4,true); } ToFCameraMITKPlayerController::~ToFCameraMITKPlayerController() { this->CleanUp(); } void ToFCameraMITKPlayerController::CleanUp() { if(m_DistanceImage.IsNotNull()) { m_DistanceImage->ReleaseData(); m_DistanceImage = NULL; } if(m_AmplitudeImage.IsNotNull()) { m_AmplitudeImage->ReleaseData(); m_AmplitudeImage = NULL; } if(m_IntensityImage.IsNotNull()) { m_IntensityImage->ReleaseData(); m_IntensityImage = NULL; } if(m_RGBImage.IsNotNull()) { m_RGBImage->ReleaseData(); m_RGBImage = NULL; } delete[] this->m_DistanceArray; this->m_DistanceArray = NULL; delete[] this->m_AmplitudeArray; this->m_AmplitudeArray = NULL; delete[] this->m_IntensityArray; this->m_IntensityArray = NULL; delete[] this->m_RGBArray; this->m_RGBArray = NULL; + + this->m_DistanceImageFileName = ""; + this->m_AmplitudeImageFileName = ""; + this->m_IntensityImageFileName = ""; + this->m_RGBImageFileName = ""; } bool ToFCameraMITKPlayerController::OpenCameraConnection() { if(!this->m_ConnectionCheck) { try { if (this->m_DistanceImageFileName.empty() && this->m_AmplitudeImageFileName.empty() && this->m_IntensityImageFileName.empty() && this->m_RGBImageFileName.empty()) { throw std::logic_error("No image data file names set"); } if (!this->m_DistanceImageFileName.empty()) { m_DistanceImage = mitk::IOUtil::LoadImage(this->m_DistanceImageFileName); } else { MITK_ERROR << "ToF distance image data file empty"; } if (!this->m_AmplitudeImageFileName.empty()) { m_AmplitudeImage = mitk::IOUtil::LoadImage(this->m_AmplitudeImageFileName); } else { MITK_WARN << "ToF amplitude image data file empty"; } if (!this->m_IntensityImageFileName.empty()) { m_IntensityImage = mitk::IOUtil::LoadImage(this->m_IntensityImageFileName); } else { MITK_WARN << "ToF intensity image data file empty"; } if (!this->m_RGBImageFileName.empty()) { m_RGBImage = mitk::IOUtil::LoadImage(this->m_RGBImageFileName); } else { MITK_WARN << "ToF RGB image data file empty"; } // check if the opened files contained data if(m_DistanceImage.IsNull()) { m_ImageStatus.at(0) = false; } if(m_AmplitudeImage.IsNull()) { m_ImageStatus.at(1) = false; } if(m_IntensityImage.IsNull()) { m_ImageStatus.at(2) = false; } if(m_RGBImage.IsNull()) { m_ImageStatus.at(3) = false; } // Check for dimension type mitk::Image::Pointer infoImage = NULL; if(m_ImageStatus.at(0)) { infoImage = m_DistanceImage; } else if (m_ImageStatus.at(1)) { infoImage = m_AmplitudeImage; } else if(m_ImageStatus.at(2)) { infoImage = m_IntensityImage; } else if(m_ImageStatus.at(3)) { infoImage = m_RGBImage; } if (infoImage->GetDimension() == 2) this->m_ToFImageType = ToFImageType2DPlusT; else if (infoImage->GetDimension() == 3) this->m_ToFImageType = ToFImageType3D; else if (infoImage->GetDimension() == 4) this->m_ToFImageType = ToFImageType2DPlusT; else throw std::logic_error("Error opening ToF data file: Invalid dimension."); this->m_CaptureWidth = infoImage->GetDimension(0); this->m_CaptureHeight = infoImage->GetDimension(1); this->m_PixelNumber = this->m_CaptureWidth*this->m_CaptureHeight; this->m_NumberOfBytes = this->m_PixelNumber * sizeof(float); if(m_RGBImage) { m_RGBCaptureWidth = m_RGBImage->GetDimension(0); m_RGBCaptureHeight = m_RGBImage->GetDimension(1); m_RGBPixelNumber = m_RGBCaptureWidth * m_RGBCaptureHeight; m_NumberOfRGBBytes = m_RGBPixelNumber * 3; } if (this->m_ToFImageType == ToFImageType2DPlusT) { this->m_NumOfFrames = infoImage->GetDimension(3); } else { this->m_NumOfFrames = infoImage->GetDimension(2); } // allocate buffer this->m_DistanceArray = new float[this->m_PixelNumber]; for(int i=0; im_PixelNumber; i++) {this->m_DistanceArray[i]=0.0;} this->m_AmplitudeArray = new float[this->m_PixelNumber]; for(int i=0; im_PixelNumber; i++) {this->m_AmplitudeArray[i]=0.0;} this->m_IntensityArray = new float[this->m_PixelNumber]; for(int i=0; im_PixelNumber; i++) {this->m_IntensityArray[i]=0.0;} this->m_RGBArray = new unsigned char[m_NumberOfRGBBytes]; for(int i=0; im_RGBArray[i]=0.0;} MITK_INFO << "NumOfFrames: " << this->m_NumOfFrames; this->m_ConnectionCheck = true; return this->m_ConnectionCheck; } catch(std::exception& e) { MITK_ERROR << "Error opening ToF data file " << this->m_InputFileName << " " << e.what(); throw std::logic_error("Error opening ToF data file"); return false; } } else return this->m_ConnectionCheck; } bool ToFCameraMITKPlayerController::CloseCameraConnection() { if (this->m_ConnectionCheck) { this->CleanUp(); this->m_ConnectionCheck = false; return true; } return false; } void ToFCameraMITKPlayerController::UpdateCamera() { this->m_CurrentFrame++; if(this->m_CurrentFrame >= this->m_NumOfFrames) { this->m_CurrentFrame = 0; } if(this->m_ImageStatus.at(0)) { this->AccessData(this->m_CurrentFrame, this->m_DistanceImage, this->m_DistanceArray); } if(this->m_ImageStatus.at(1)) { this->AccessData(this->m_CurrentFrame, this->m_AmplitudeImage, this->m_AmplitudeArray); } if(this->m_ImageStatus.at(2)) { this->AccessData(this->m_CurrentFrame, this->m_IntensityImage, this->m_IntensityArray); } if(this->m_ImageStatus.at(3)) { if(!this->m_ToFImageType) { ImageReadAccessor rgbAcc(m_RGBImage, m_RGBImage->GetSliceData(m_CurrentFrame)); memcpy(m_RGBArray, rgbAcc.GetData(), m_NumberOfRGBBytes ); } else if(this->m_ToFImageType) { ImageReadAccessor rgbAcc(m_RGBImage, m_RGBImage->GetVolumeData(m_CurrentFrame)); memcpy(m_RGBArray, rgbAcc.GetData(), m_NumberOfRGBBytes); } } itksys::SystemTools::Delay(50); } void ToFCameraMITKPlayerController::AccessData(int frame, Image::Pointer image, float* &data) { if(!this->m_ToFImageType) { ImageReadAccessor imgAcc(image, image->GetSliceData(frame)); memcpy(data, imgAcc.GetData(), this->m_NumberOfBytes ); } else if(this->m_ToFImageType) { ImageReadAccessor imgAcc(image, image->GetVolumeData(frame)); memcpy(data, imgAcc.GetData(), this->m_NumberOfBytes); } } void ToFCameraMITKPlayerController::GetAmplitudes(float* amplitudeArray) { memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_NumberOfBytes); } void ToFCameraMITKPlayerController::GetIntensities(float* intensityArray) { memcpy(intensityArray, this->m_IntensityArray, this->m_NumberOfBytes); } void ToFCameraMITKPlayerController::GetDistances(float* distanceArray) { memcpy(distanceArray, this->m_DistanceArray, this->m_NumberOfBytes); } void ToFCameraMITKPlayerController::GetRgb(unsigned char* rgbArray) { memcpy(rgbArray, this->m_RGBArray, m_NumberOfRGBBytes); } void ToFCameraMITKPlayerController::SetInputFileName(std::string inputFileName) { this->m_InputFileName = inputFileName; } } diff --git a/Modules/ToFHardware/mitkToFCameraMITKPlayerDevice.cpp b/Modules/ToFHardware/mitkToFCameraMITKPlayerDevice.cpp index ac6431bbbd..5abc2b1011 100644 --- a/Modules/ToFHardware/mitkToFCameraMITKPlayerDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraMITKPlayerDevice.cpp @@ -1,380 +1,388 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkToFCameraMITKPlayerDevice.h" #include "mitkToFCameraMITKPlayerController.h" #include "mitkRealTimeClock.h" #include #include #include namespace mitk { ToFCameraMITKPlayerDevice::ToFCameraMITKPlayerDevice() : m_DistanceDataBuffer(NULL), m_AmplitudeDataBuffer(NULL), m_IntensityDataBuffer(NULL), m_RGBDataBuffer(NULL) { m_Controller = ToFCameraMITKPlayerController::New(); } ToFCameraMITKPlayerDevice::~ToFCameraMITKPlayerDevice() { DisconnectCamera(); CleanUpDataBuffers(); } bool ToFCameraMITKPlayerDevice::OnConnectCamera() { bool ok = m_Controller->OpenCameraConnection(); if (ok) { this->m_CaptureWidth = m_Controller->GetCaptureWidth(); this->m_CaptureHeight = m_Controller->GetCaptureHeight(); this->m_RGBImageWidth = m_Controller->GetRGBCaptureWidth(); this->m_RGBImageHeight = m_Controller->GetRGBCaptureHeight(); this->m_PixelNumber = m_Controller->GetPixelNumber(); this->m_RGBPixelNumber = m_Controller->GetRGBPixelNumber(); if(m_RGBPixelNumber != m_PixelNumber) this->SetBoolProperty("RGBImageHasDifferentResolution", true); AllocatePixelArrays(); AllocateDataBuffers(); m_CameraConnected = true; } return ok; } bool ToFCameraMITKPlayerDevice::DisconnectCamera() { bool ok = m_Controller->CloseCameraConnection(); if (ok) { m_CameraConnected = false; +// m_PropertyList->DeleteProperty("HasAmplitudeImage"); +// m_PropertyList->DeleteProperty("HasIntensityImage"); +// m_PropertyList->DeleteProperty("HasRGBImage"); +// m_PropertyList->DeleteProperty("DistanceImageFileName"); +// m_PropertyList->DeleteProperty("AmplitudeImageFileName"); +// m_PropertyList->DeleteProperty("IntensityImageFileName"); +// m_PropertyList->DeleteProperty("RGBImageFileName"); +// m_PropertyList->DeleteProperty("RGBImageHasDifferentResolution"); } return ok; } void ToFCameraMITKPlayerDevice::StartCamera() { if (m_CameraConnected) { // get the first image this->m_Controller->UpdateCamera(); this->m_ImageMutex->Lock(); this->m_Controller->GetDistances(this->m_DistanceDataBuffer[this->m_FreePos]); this->m_Controller->GetAmplitudes(this->m_AmplitudeDataBuffer[this->m_FreePos]); this->m_Controller->GetIntensities(this->m_IntensityDataBuffer[this->m_FreePos]); this->m_Controller->GetRgb(this->m_RGBDataBuffer[this->m_FreePos]); this->m_FreePos = (this->m_FreePos+1) % this->m_BufferSize; this->m_CurrentPos = (this->m_CurrentPos+1) % this->m_BufferSize; this->m_ImageSequence++; this->m_ImageMutex->Unlock(); this->m_CameraActiveMutex->Lock(); this->m_CameraActive = true; this->m_CameraActiveMutex->Unlock(); this->m_ThreadID = this->m_MultiThreader->SpawnThread(this->Acquire, this); // wait a little to make sure that the thread is started itksys::SystemTools::Delay(10); } else { MITK_INFO<<"Camera not connected"; } } void ToFCameraMITKPlayerDevice::UpdateCamera() { m_Controller->UpdateCamera(); } ITK_THREAD_RETURN_TYPE ToFCameraMITKPlayerDevice::Acquire(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } ToFCameraMITKPlayerDevice* toFCameraDevice = (ToFCameraMITKPlayerDevice*)pInfo->UserData; if (toFCameraDevice!=NULL) { mitk::RealTimeClock::Pointer realTimeClock; realTimeClock = mitk::RealTimeClock::New(); int n = 100; double t1, t2; t1 = realTimeClock->GetCurrentStamp(); bool overflow = false; bool printStatus = false; while (toFCameraDevice->IsCameraActive()) { // update the ToF camera toFCameraDevice->UpdateCamera(); // get image data from controller and write it to the according buffer toFCameraDevice->m_Controller->GetDistances(toFCameraDevice->m_DistanceDataBuffer[toFCameraDevice->m_FreePos]); toFCameraDevice->m_Controller->GetAmplitudes(toFCameraDevice->m_AmplitudeDataBuffer[toFCameraDevice->m_FreePos]); toFCameraDevice->m_Controller->GetIntensities(toFCameraDevice->m_IntensityDataBuffer[toFCameraDevice->m_FreePos]); toFCameraDevice->m_Controller->GetRgb(toFCameraDevice->m_RGBDataBuffer[toFCameraDevice->m_FreePos]); toFCameraDevice->Modified(); toFCameraDevice->m_ImageMutex->Lock(); toFCameraDevice->m_FreePos = (toFCameraDevice->m_FreePos+1) % toFCameraDevice->m_BufferSize; toFCameraDevice->m_CurrentPos = (toFCameraDevice->m_CurrentPos+1) % toFCameraDevice->m_BufferSize; toFCameraDevice->m_ImageSequence++; if (toFCameraDevice->m_FreePos == toFCameraDevice->m_CurrentPos) { overflow = true; } if (toFCameraDevice->m_ImageSequence % n == 0) { printStatus = true; } toFCameraDevice->m_ImageMutex->Unlock(); if (overflow) { overflow = false; } // print current framerate if (printStatus) { t2 = realTimeClock->GetCurrentStamp() - t1; MITK_INFO << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence; t1 = realTimeClock->GetCurrentStamp(); printStatus = false; } } // end of while loop } return ITK_THREAD_RETURN_VALUE; } void ToFCameraMITKPlayerDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence) { m_ImageMutex->Lock(); // write amplitude image data to float array for (int i=0; im_PixelNumber; i++) { amplitudeArray[i] = this->m_AmplitudeDataBuffer[this->m_CurrentPos][i]; } imageSequence = this->m_ImageSequence; m_ImageMutex->Unlock(); } void ToFCameraMITKPlayerDevice::GetIntensities(float* intensityArray, int& imageSequence) { m_ImageMutex->Lock(); // write intensity image data to float array for (int i=0; im_PixelNumber; i++) { intensityArray[i] = this->m_IntensityDataBuffer[this->m_CurrentPos][i]; } imageSequence = this->m_ImageSequence; m_ImageMutex->Unlock(); } void ToFCameraMITKPlayerDevice::GetDistances(float* distanceArray, int& imageSequence) { m_ImageMutex->Lock(); // write distance image data to float array for (int i=0; im_PixelNumber; i++) { distanceArray[i] = this->m_DistanceDataBuffer[this->m_CurrentPos][i]; } imageSequence = this->m_ImageSequence; m_ImageMutex->Unlock(); } void ToFCameraMITKPlayerDevice::GetRgb(unsigned char* rgbArray, int& imageSequence) { m_ImageMutex->Lock(); // write intensity image data to unsigned char array for (int i=0; im_RGBPixelNumber*3; i++) { rgbArray[i] = this->m_RGBDataBuffer[this->m_CurrentPos][i]; } imageSequence = this->m_ImageSequence; m_ImageMutex->Unlock(); } void ToFCameraMITKPlayerDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* /*sourceDataArray*/, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray) { m_ImageMutex->Lock(); //check for empty buffer if (this->m_ImageSequence < 0) { // buffer empty MITK_INFO << "Buffer empty!! "; capturedImageSequence = this->m_ImageSequence; m_ImageMutex->Unlock(); return; } // determine position of image in buffer int pos = 0; if ((requiredImageSequence < 0) || (requiredImageSequence > this->m_ImageSequence)) { capturedImageSequence = this->m_ImageSequence; pos = this->m_CurrentPos; } else if (requiredImageSequence <= this->m_ImageSequence - this->m_BufferSize) { capturedImageSequence = (this->m_ImageSequence - this->m_BufferSize) + 1; pos = (this->m_CurrentPos + 1) % this->m_BufferSize; } else // (requiredImageSequence > this->m_ImageSequence - this->m_BufferSize) && (requiredImageSequence <= this->m_ImageSequence) { capturedImageSequence = requiredImageSequence; pos = (this->m_CurrentPos + (10-(this->m_ImageSequence - requiredImageSequence))) % this->m_BufferSize; } if(this->m_DistanceDataBuffer&&this->m_AmplitudeDataBuffer&&this->m_IntensityDataBuffer&&this->m_RGBDataBuffer) { // write image data to float arrays memcpy(distanceArray, this->m_DistanceDataBuffer[pos], this->m_PixelNumber * sizeof(float)); memcpy(amplitudeArray, this->m_AmplitudeDataBuffer[pos], this->m_PixelNumber * sizeof(float)); memcpy(intensityArray, this->m_IntensityDataBuffer[pos], this->m_PixelNumber * sizeof(float)); memcpy(rgbDataArray, this->m_RGBDataBuffer[pos], this->m_RGBPixelNumber * 3 * sizeof(unsigned char)); } m_ImageMutex->Unlock(); } void ToFCameraMITKPlayerDevice::SetInputFileName(std::string inputFileName) { this->m_InputFileName = inputFileName; this->m_Controller->SetInputFileName(inputFileName); } void ToFCameraMITKPlayerDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { this->m_PropertyList->SetProperty(propertyKey, propertyValue); ToFCameraMITKPlayerController::Pointer myController = dynamic_cast(this->m_Controller.GetPointer()); std::string strValue; GetStringProperty(propertyKey, strValue); if (strcmp(propertyKey, "DistanceImageFileName") == 0) { myController->SetDistanceImageFileName(strValue); } else if (strcmp(propertyKey, "AmplitudeImageFileName") == 0) { std::ifstream amplitudeImage(strValue.c_str()); if(amplitudeImage) { this->m_PropertyList->SetBoolProperty("HasAmplitudeImage", true); myController->SetAmplitudeImageFileName(strValue); } else { MITK_WARN << "File " << strValue << " does not exist!"; } } else if (strcmp(propertyKey, "IntensityImageFileName") == 0) { std::ifstream intensityImage(strValue.c_str()); if(intensityImage) { this->m_PropertyList->SetBoolProperty("HasIntensityImage", true); myController->SetIntensityImageFileName(strValue); } else { MITK_WARN << "File " << strValue << " does not exist!"; } } else if (strcmp(propertyKey, "RGBImageFileName") == 0) { std::ifstream intensityImage(strValue.c_str()); if(intensityImage) { this->m_PropertyList->SetBoolProperty("HasRGBImage", true); myController->SetRGBImageFileName(strValue); } else { MITK_WARN << "File " << strValue << " does not exist!"; } } } void ToFCameraMITKPlayerDevice::CleanUpDataBuffers() { if (m_DistanceDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_DistanceDataBuffer[i]; } delete[] this->m_DistanceDataBuffer; } if (m_AmplitudeDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_AmplitudeDataBuffer[i]; } delete[] this->m_AmplitudeDataBuffer; } if (m_IntensityDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_IntensityDataBuffer[i]; } delete[] this->m_IntensityDataBuffer; } if (m_RGBDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_RGBDataBuffer[i]; } delete[] this->m_RGBDataBuffer; } } void ToFCameraMITKPlayerDevice::AllocateDataBuffers() { // free memory if it was already allocated this->CleanUpDataBuffers(); // allocate buffers this->m_DistanceDataBuffer = new float*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_DistanceDataBuffer[i] = new float[this->m_PixelNumber]; } this->m_AmplitudeDataBuffer = new float*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_AmplitudeDataBuffer[i] = new float[this->m_PixelNumber]; } this->m_IntensityDataBuffer = new float*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_IntensityDataBuffer[i] = new float[this->m_PixelNumber]; } this->m_RGBDataBuffer = new unsigned char*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_RGBDataBuffer[i] = new unsigned char[this->m_RGBPixelNumber*3]; } } }