diff --git a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp index e4002556a9..2f0c125ad5 100644 --- a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp +++ b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp @@ -1,334 +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. ===================================================================*/ // mitk includes #include "mitkThreadedToFRawDataReconstruction.h" #include "mitkITKImageImport.h" #include "mitkImageDataItem.h" // stl includes #include #include #include // vtk includes #include #include #include // itk includes #include #include #ifdef WIN32 #include #else #include #endif #include #define _USE_MATH_DEFINES #include #define PI M_PI; #define cAir 299704944; #define fMod 20000000; namespace mitk { ThreadedToFRawDataReconstruction::ThreadedToFRawDataReconstruction(): m_Threader(0), m_CISDist(0), m_CISAmpl(0), m_CISInten(0), m_ThreadedCISDist(0), m_ThreadedCISAmpl(0), m_ThreadedCISInten(0), m_Init(0), m_Width(0), m_Height(0), m_SourceDataSize(0), m_ImageSize(0), m_SourceData(0) { m_ThreadData = new ThreadDataStruct; m_ThreadData->m_ModulationFrequency = fMod; m_ThreadData->m_ImageDataMutex = itk::FastMutexLock::New(); m_ThreadData->m_ThreadDataMutex = itk::FastMutexLock::New(); m_StackSize = 1; } ThreadedToFRawDataReconstruction::~ThreadedToFRawDataReconstruction() { if(m_ThreadData != NULL) delete m_ThreadData; if(m_CISDist != NULL) delete[] m_CISDist; if(m_CISAmpl != NULL) delete[] m_CISAmpl; if(m_CISInten != NULL) delete[] m_CISInten; if(m_ThreadedCISInten != NULL) delete[] m_ThreadedCISInten; if(m_ThreadedCISAmpl != NULL) delete[] m_ThreadedCISAmpl; if(m_ThreadedCISDist != NULL) delete[] m_ThreadedCISDist; } -void ThreadedToFRawDataReconstruction::Initialize(int width, int height, int modulationFrequency, int sourceDataSize ) +void ThreadedToFRawDataReconstruction::Initialize(int width, int height, int modulationFrequency, + int sourceDataSize ) { m_Width = width; m_Height = height; m_SourceDataSize = sourceDataSize; m_ImageSize = width * height; m_ThreadData->m_ModulationFrequency = modulationFrequency * 1e6; if(!m_Init) { m_SourceData = vtkShortArray::New(); m_SourceData->SetNumberOfComponents(m_SourceDataSize); m_SourceData->SetNumberOfTuples(4); m_SourceData->Allocate(1); m_CISDist = new float[m_ImageSize]; m_CISAmpl = new float[m_ImageSize]; m_CISInten = new float[m_ImageSize]; m_ThreadedCISDist = new float[m_ImageSize]; m_ThreadedCISAmpl = new float[m_ImageSize]; m_ThreadedCISInten = new float[m_ImageSize]; m_ThreadData->m_OutputData.push_back( m_ThreadedCISDist ); m_ThreadData->m_OutputData.push_back( m_ThreadedCISAmpl ); m_ThreadData->m_OutputData.push_back( m_ThreadedCISInten ); m_Init = true; } } void ThreadedToFRawDataReconstruction::SetChannelData(vtkShortArray* sourceData) { m_SourceData->DeepCopy(sourceData); } void ThreadedToFRawDataReconstruction::GetDistances(float* dist) { memcpy(dist, m_CISDist, m_ImageSize*sizeof(float) ); } void ThreadedToFRawDataReconstruction::GetAmplitudes(float* ampl) { memcpy(ampl, m_CISAmpl, m_ImageSize*sizeof(float)); } void ThreadedToFRawDataReconstruction::GetIntensities(float* inten) { memcpy(inten, m_CISInten, m_ImageSize*sizeof(float)); } void ThreadedToFRawDataReconstruction::GetAllData(float* dist, float* ampl, float* inten) { memcpy(dist, m_CISDist, m_ImageSize*sizeof(float) ); memcpy(ampl, m_CISAmpl, m_ImageSize*sizeof(float)); memcpy(inten, m_CISInten, m_ImageSize*sizeof(float)); } void ThreadedToFRawDataReconstruction::GenerateData() { if(m_Init) { this->BeforeThreadedGenerateData(); } } void ThreadedToFRawDataReconstruction::BeforeThreadedGenerateData() { int sourceDataSize = m_SourceDataSize; int lineWidth = m_Width; int frameHeight = m_Height; int channelSize = lineWidth*frameHeight << 1; int quadChannelSize = channelSize * 0.25; std::vector quad = std::vector(quadChannelSize); // clean the thread data array m_ThreadData->m_InputData.erase(m_ThreadData->m_InputData.begin(),m_ThreadData->m_InputData.end()); int channelNo = 0; while(channelNo < m_SourceData->GetNumberOfTuples()) { short* sourceData = new short[channelSize]; m_SourceData->GetTupleValue(channelNo, sourceData); quad.insert(quad.begin(), sourceData, sourceData+channelSize); m_ThreadData->m_InputData.push_back(quad); delete[]sourceData; ++channelNo; } if(m_Threader.IsNull()) { m_Threader = this->GetMultiThreader(); } int maxThreadNr = 0; if(m_Threader->GetGlobalDefaultNumberOfThreads()> 5) { maxThreadNr = 5; } else if(m_Threader->GetGlobalMaximumNumberOfThreads()>5) { maxThreadNr = 5; } else { maxThreadNr = m_Threader->GetGlobalMaximumNumberOfThreads(); } if ( m_ThreadData->m_Barrier.IsNull()) { m_ThreadData->m_Barrier = itk::Barrier::New(); m_ThreadData->m_Barrier->Initialize(maxThreadNr); // } m_ThreadData->m_DataSize = quadChannelSize; m_ThreadData->m_LineWidth = lineWidth; m_ThreadData->m_FrameHeight = frameHeight * 0.25; std::vector threadIDVector; int threadcounter = 0; while(threadcounter != maxThreadNr-1) { if (m_Threader->GetNumberOfThreads() < m_Threader->GetGlobalMaximumNumberOfThreads()) { int threadID = m_Threader->SpawnThread(this->ThreadedGenerateDataCallbackFunction, m_ThreadData); threadIDVector.push_back(threadID); threadcounter++; } } m_ThreadData->m_Barrier->Wait(); int count = 0; while(count != threadIDVector.size()) { m_Threader->TerminateThread(threadIDVector.at(count)); count++; } m_ThreadData->m_ImageDataMutex->Lock(); memcpy(m_CISDist, m_ThreadData->m_OutputData.at(0), (channelSize * 0.5)*sizeof(float)); memcpy(m_CISAmpl, m_ThreadData->m_OutputData.at(1), (channelSize * 0.5)*sizeof(float)); memcpy(m_CISInten, m_ThreadData->m_OutputData.at(2), (channelSize * 0.5)*sizeof(float)); m_ThreadData->m_ImageDataMutex->Unlock(); } ITK_THREAD_RETURN_TYPE ThreadedToFRawDataReconstruction::ThreadedGenerateDataCallbackFunction(void* data) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)data; if (pInfo == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } int quadrant = pInfo->ThreadID; ThreadDataStruct* threadData = (ThreadDataStruct*) pInfo->UserData; // some needed variables int x = 0; double phi = 0; double phi2 = 0; double A1 = 0; double A2 = 0; double A3 = 0; double A4 = 0; double A5 = 0; double A6 = 0; double A7 = 0; double A8 = 0; double A3m1 = 0; double A4m2 = 0; double A7m5 = 0; double A8m6 = 0; double cair = cAir; double pi = PI; double twoPi = pi + pi; long modFreq = fMod; threadData->m_ThreadDataMutex->Lock(); std::vector quad1 = threadData->m_InputData.at(0); std::vector quad2 = threadData->m_InputData.at(1); std::vector quad3 = threadData->m_InputData.at(2); std::vector quad4 = threadData->m_InputData.at(3); int index = quadrant << 1; int index2 = 3-quadrant; modFreq = threadData->m_ModulationFrequency; int linewidth = threadData->m_LineWidth; int frameheight = threadData->m_FrameHeight; threadData->m_ThreadDataMutex->Unlock(); double intermed1 = cair/(pi*(modFreq << 2)); double intermed2 = intermed1*500; int doubleLwidth = linewidth << 1; int datasize = doubleLwidth*frameheight << 2; do { index += doubleLwidth; x++; do { index -= 8; A1 = htons(quad1.at(index)); A2 = htons(quad2.at(index)); A3 = htons(quad3.at(index)); A4 = htons(quad4.at(index)); A5 = htons(quad1.at(index+1)); A6 = htons(quad2.at(index+1)); A7 = htons(quad3.at(index+1)); A8 = htons(quad4.at(index+1)); phi = atan2((A3 - A1),(A2 - A4)) + pi; phi2 = atan2((A7 - A5),(A6 - A8)); if(phi2<0) phi2 +=twoPi; A3m1 = A3*A3 - 2*A3*A1 + A1*A1; A4m2 = A4*A4 - 2*A4*A2 + A2*A2; A7m5 = A7*A7 - 2*A7*A5 + A5*A5; A8m6 = A8*A8 - 2*A8*A6 + A6*A6; threadData->m_ImageDataMutex->Lock(); threadData->m_OutputData.at(0)[index2] = (phi+phi2)*intermed2; //(((phi*intermed1) + (phi2*intermed1))/2)*1000; // threadData->m_OutputData.at(1)[index2] = (sqrt(A3m1 + A4m2)+sqrt(A7m5 + A8m6))*0.5; //(sqrt(A3m1 + A4m2)/2) + (sqrt(A7m5 + A8m6)/2); threadData->m_OutputData.at(2)[index2] = (A1+A2+A3+A4+A5+A6+A7+A8)*0.125; threadData->m_ImageDataMutex->Unlock(); index2 += 4; }while(index2 <= (x*linewidth) - (1+quadrant)); index += doubleLwidth; }while(index < datasize); threadData->m_Barrier->Wait(); return ITK_THREAD_RETURN_VALUE; } void ThreadedToFRawDataReconstruction::Update() { this->GenerateData(); } + } // end mitk namespace diff --git a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h index 6a47cbf0bd..c1659f1f97 100644 --- a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h +++ b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h @@ -1,114 +1,119 @@ /*=================================================================== 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. ===================================================================*/ #ifndef __mitkThreadedToFRawDataReconstruction_h #define __mitkThreadedToFRawDataReconstruction_h // mitk includes #include "mitkImageSource.h" #include "mitkToFHardwareExports.h" // itk includes #include #include // vtk includes #include "vtkShortArray.h" namespace mitk { struct ThreadDataStruct { std::vector > m_InputData; std::vector m_OutputData; unsigned int m_DataSize; unsigned int m_LineWidth; unsigned int m_FrameHeight; unsigned int m_ModulationFrequency; itk::Barrier::Pointer m_Barrier; ///< barrier to synchronize ends of threads itk::FastMutexLock::Pointer m_ImageDataMutex; ///< mutex for coordinated access to image data itk::FastMutexLock::Pointer m_ThreadDataMutex; ///< mutex to control access to images }; class MITK_TOFHARDWARE_EXPORT ThreadedToFRawDataReconstruction : public itk::ProcessObject { public: mitkClassMacro( ThreadedToFRawDataReconstruction , itk::ProcessObject ); itkNewMacro( Self ); itkGetMacro(Init, bool); void SetChannelData(vtkShortArray* sourceData); - void Initialize(int width, int height, int modulationFrequency, int sourceDataSize); + void Initialize(int width, int height, int modulationFrequency, int sourceDataSize ); void GetDistances(float* dist); void GetAmplitudes(float* ampl); void GetIntensities(float* inten); void GetAllData(float* dist, float* ampl, float* inten); void Update(); protected: /*! \brief standard constructor */ ThreadedToFRawDataReconstruction(); /*! \brief standard destructor */ ~ThreadedToFRawDataReconstruction(); /*! \brief method generating the outputs of this filter. Called in the updated process of the pipeline. This method generates the two outputs of the ToFImageSource: The distance and the intensity image */ virtual void GenerateData(); /*! \brief method configures the camera output and prepares the thread data struct for threaded data generation */ virtual void BeforeThreadedGenerateData(); /*! \brief threader callback function for multi threaded data generation */ static ITK_THREAD_RETURN_TYPE ThreadedGenerateDataCallbackFunction(void* data); + /*! + \brief Method to reduce image size from original controller size to output size (200x200px) + and rotate the image. This is needed due to defect pixel rows at the rim of the chip + */ + void ResizeOutputImage(float* in, float* out); // member variables int m_StackSize; ///< int m_Width; int m_Height; int m_ImageSize; int m_SourceDataSize; vtkShortArray* m_SourceData; bool m_Init; float* m_CISDist; ///< holds the distance information from for one distance image slice float* m_CISAmpl; ///< holds the amplitude information from for one amplitude image slice float* m_CISInten; ///< holds the intensity information from for one intensity image slice float* m_ThreadedCISDist; float* m_ThreadedCISAmpl; float* m_ThreadedCISInten; itk::MultiThreader::Pointer m_Threader; ThreadDataStruct* m_ThreadData; }; } //end mitk namespace #endif // __mitkThreadedToFRawDataReconstruction_h diff --git a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp index fbbfeb5c1d..b0818dc398 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp @@ -1,225 +1,307 @@ /*=================================================================== 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 "mitkToFCameraPMDCamBoardController.h" #include "mitkToFConfig.h" #include +// vnl includes +#include "vnl/vnl_matrix.h" + //Plugin defines for CamBoard #define SOURCE_PARAM "" #define PROC_PARAM "" extern PMDHandle m_PMDHandle; //TODO - extern PMDDataDescription m_DataDescription; //TODO +struct SourceDataStruct { + PMDDataDescription dataDescription; + char sourceData; +}; namespace mitk { - ToFCameraPMDCamBoardController::ToFCameraPMDCamBoardController() + ToFCameraPMDCamBoardController::ToFCameraPMDCamBoardController(): m_InternalCaptureWidth(0), + m_InternalCaptureHeight(0), m_InternalPixelNumber(0) { m_SourcePlugin = MITK_TOF_PMDCAMBOARD_SOURCE_PLUGIN; m_SourceParam = SOURCE_PARAM; m_ProcPlugin = MITK_TOF_PMDCAMBOARD_PROCESSING_PLUGIN; m_ProcParam = PROC_PARAM; } ToFCameraPMDCamBoardController::~ToFCameraPMDCamBoardController() { } bool ToFCameraPMDCamBoardController::OpenCameraConnection() { if(!m_ConnectionCheck) { m_PMDRes = pmdOpen (&m_PMDHandle , m_SourcePlugin , m_SourceParam , m_ProcPlugin , m_ProcParam ); m_ConnectionCheck = ErrorText(m_PMDRes); if (!m_ConnectionCheck) { return m_ConnectionCheck; } - // get image properties from camera this->UpdateCamera(); m_PMDRes = pmdGetSourceDataDescription(m_PMDHandle, &m_DataDescription); - ErrorText(m_PMDRes); - m_CaptureWidth = m_DataDescription.img.numColumns; - m_CaptureHeight = m_DataDescription.img.numRows; + ErrorText(m_PMDRes); + m_InternalCaptureWidth = m_DataDescription.img.numColumns; + m_CaptureWidth = 200; + m_InternalCaptureHeight = m_DataDescription.img.numRows; + m_CaptureHeight = 200; + m_InternalPixelNumber = m_InternalCaptureWidth*m_InternalCaptureHeight; m_PixelNumber = m_CaptureWidth*m_CaptureHeight; - m_NumberOfBytes = m_PixelNumber * sizeof(float); + + m_NumberOfBytes = m_InternalPixelNumber * sizeof(float); m_SourceDataSize = m_DataDescription.size; m_SourceDataStructSize = m_DataDescription.size + sizeof(PMDDataDescription); MITK_INFO << "Datasource size: " << this->m_SourceDataSize <GetIntegrationTime(); - MITK_INFO << "Modulation Frequence: " << this->GetModulationFrequency(); - + MITK_INFO << "Modulation Frequency: " << this->GetModulationFrequency(); return m_ConnectionCheck; } else return m_ConnectionCheck; } bool mitk::ToFCameraPMDCamBoardController::SetDistanceOffset( float offset ) { std::stringstream command; command<<"SetSoftOffset "<m_PMDRes = pmdSourceCommand(m_PMDHandle,0,0,command.str().c_str()); return ErrorText(this->m_PMDRes); } float mitk::ToFCameraPMDCamBoardController::GetDistanceOffset() { char offset[16]; this->m_PMDRes = pmdSourceCommand(m_PMDHandle, offset, 16, "GetSoftOffset"); ErrorText(this->m_PMDRes); return atof(offset); } bool mitk::ToFCameraPMDCamBoardController::SetRegionOfInterest( unsigned int leftUpperCornerX, unsigned int leftUpperCornerY, unsigned int width, unsigned int height ) { + // CAVE: This function does not work properly, don't use unless you know what you're doing!! // check if leftUpperCornerX and width are divisible by 3 otherwise round to the next value divisible by 3 unsigned int factor = leftUpperCornerX/3; leftUpperCornerX = 3*factor; factor = width/3; width = 3*factor; std::stringstream command; command<<"SetROI "<m_PMDRes = pmdSourceCommand(m_PMDHandle,0,0,command.str().c_str()); return ErrorText(this->m_PMDRes); } bool mitk::ToFCameraPMDCamBoardController::SetRegionOfInterest( unsigned int roi[4] ) { return this->SetRegionOfInterest(roi[0],roi[1],roi[2],roi[3]); } unsigned int* mitk::ToFCameraPMDCamBoardController::GetRegionOfInterest() { /* char result[64]; this->m_PMDRes = pmdSourceCommand(m_PMDHandle, result, 64, "GetROI"); ErrorText(this->m_PMDRes); // convert char array to uint array unsigned int roi[4]; std::stringstream currentValue; int counter = 0; std::string resultString = result; char blank = ' '; for (int i=0; i<64; i++) { if (result[i]!=blank) { currentValue<m_PMDRes = pmdSourceCommand(m_PMDHandle, 0, 0, "SetExposureMode Normal"); return ErrorText(this->m_PMDRes); } else if (mode==1) // SMB mode { this->m_PMDRes = pmdSourceCommand(m_PMDHandle, 0, 0, "SetExposureMode SMB"); return ErrorText(this->m_PMDRes); } else { MITK_ERROR<<"Specified exposure mode not supported. Exposure mode must be 0 (Normal) or 1 (SMB)"; return false; } } bool mitk::ToFCameraPMDCamBoardController::SetFieldOfView( float fov ) { std::stringstream commandStream; commandStream<<"SetFOV "<m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, commandStream.str().c_str()); return ErrorText(this->m_PMDRes); } bool mitk::ToFCameraPMDCamBoardController::SetFPNCalibration( bool on ) { if(on) { this->m_PMDRes=pmdSourceCommand(m_PMDHandle,0,0,"SetFPNCalibration On"); return this->ErrorText(this->m_PMDRes); } else { this->m_PMDRes=pmdSourceCommand(m_PMDHandle,0,0,"SetFPNCalibration Off"); return this->ErrorText(this->m_PMDRes); } } bool mitk::ToFCameraPMDCamBoardController::SetFPPNCalibration( bool on ) { if(on) { this->m_PMDRes=pmdProcessingCommand(m_PMDHandle,0,0,"SetFPPNCalibration On"); return this->ErrorText(this->m_PMDRes); } else { this->m_PMDRes=pmdProcessingCommand(m_PMDHandle,0,0,"SetFPPNCalibration Off"); return this->ErrorText(this->m_PMDRes); } } bool mitk::ToFCameraPMDCamBoardController::SetLinearityCalibration( bool on ) { if(on) { this->m_PMDRes=pmdProcessingCommand(m_PMDHandle,0,0,"SetLinearityCalibration On"); return this->ErrorText(this->m_PMDRes); } else { this->m_PMDRes=pmdProcessingCommand(m_PMDHandle,0,0,"SetLinearityCalibration Off"); return this->ErrorText(this->m_PMDRes); } } bool mitk::ToFCameraPMDCamBoardController::SetLensCalibration( bool on ) { if (on) { this->m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, "SetLensCalibration On"); return ErrorText(this->m_PMDRes); } else { this->m_PMDRes = pmdProcessingCommand(m_PMDHandle, 0, 0, "SetLensCalibration Off"); return ErrorText(this->m_PMDRes); } } + bool ToFCameraPMDCamBoardController::GetAmplitudes(float* amplitudeArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdGetAmplitudes(m_PMDHandle, tempArray, this->m_NumberOfBytes); + this->TransformCameraOutput(tempArray, amplitudeArray, false); + delete[] tempArray; + return ErrorText(this->m_PMDRes); + } + + bool ToFCameraPMDCamBoardController::GetAmplitudes(char* sourceData, float* amplitudeArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdCalcAmplitudes(m_PMDHandle, tempArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); + this->TransformCameraOutput(tempArray, amplitudeArray, false); + delete[] tempArray; + return ErrorText(this->m_PMDRes); + } + + bool ToFCameraPMDCamBoardController::GetIntensities(float* intensityArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdGetIntensities(m_PMDHandle, tempArray, this->m_NumberOfBytes); + this->TransformCameraOutput(tempArray, intensityArray, false); + delete[] tempArray; + return ErrorText(this->m_PMDRes); + } + + bool ToFCameraPMDCamBoardController::GetIntensities(char* sourceData, float* intensityArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdCalcIntensities(m_PMDHandle, tempArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); + this->TransformCameraOutput(tempArray, intensityArray, false); + delete[] tempArray; + return ErrorText(this->m_PMDRes); + } + + bool ToFCameraPMDCamBoardController::GetDistances(float* distanceArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdGetDistances(m_PMDHandle, tempArray, this->m_NumberOfBytes); + this->TransformCameraOutput(tempArray, distanceArray, true); + delete[] tempArray; + return ErrorText(this->m_PMDRes); + } + + bool ToFCameraPMDCamBoardController::GetDistances(char* sourceData, float* distanceArray) + { + float* tempArray = new float[m_InternalCaptureWidth*m_InternalCaptureHeight]; + this->m_PMDRes = pmdCalcDistances(m_PMDHandle, tempArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); + this->TransformCameraOutput(tempArray, distanceArray, true); + delete[] tempArray; + + return ErrorText(this->m_PMDRes); + } + + void ToFCameraPMDCamBoardController::TransformCameraOutput( float* in, float* out, bool isDist ) + { + vnl_matrix inMat = vnl_matrix(m_InternalCaptureHeight,m_InternalCaptureWidth); + inMat.copy_in(in); + vnl_matrix outMat = vnl_matrix(m_CaptureHeight, m_CaptureWidth); + vnl_matrix temp = vnl_matrix(m_CaptureHeight, m_CaptureWidth); + temp = inMat.extract(m_CaptureHeight, m_CaptureWidth, 0,1); + outMat = temp.transpose(); + if(isDist) + { + outMat*=1000; + } + outMat.copy_out(out); + inMat.clear(); + outMat.clear(); + temp.clear(); + } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h index c3ba1d6fd1..83b5a928ea 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h @@ -1,116 +1,135 @@ /*=================================================================== 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. ===================================================================*/ #ifndef __mitkToFCameraPMDCamBoardController_h #define __mitkToFCameraPMDCamBoardController_h #include "mitkToFHardwareExports.h" #include "mitkCommon.h" #include "mitkToFCameraPMDController.h" #include "itkObject.h" #include "itkObjectFactory.h" namespace mitk { /** * @brief Interface to the Time-of-Flight (ToF) camera PMD CamBoard * * * @ingroup ToFHardware */ class MITK_TOFHARDWARE_EXPORT ToFCameraPMDCamBoardController : public mitk::ToFCameraPMDController { public: mitkClassMacro( ToFCameraPMDCamBoardController , mitk::ToFCameraPMDController ); itkNewMacro( Self ); /*! \brief opens a connection to the ToF camera and initializes the hardware specific members \return returns whether the connection was successful (true) or not (false) */ virtual bool OpenCameraConnection(); /*! \brief sets an additional distance offset which will be added to all distance values. \param offset offset in m */ bool SetDistanceOffset( float offset ); /*! \brief returns the currently applied distance offset in m \param offset offset in m */ float GetDistanceOffset(); /*! \brief Setting the region of interest, the camera is configured to only output a certain area of the image. \param leftUpperCornerX x value of left upper corner of region \param leftUpperCornerX y value of left upper corner of region \param width width of region \param height height of region */ bool SetRegionOfInterest( unsigned int leftUpperCornerX, unsigned int leftUpperCornerY, unsigned int width, unsigned int height ); /*! \brief Setting the region of interest, the camera is configured to only output a certain area of the image. \param roi region of interest. roi[0]: x value of left upper corner, roi[1]: y value of left upper corner, roi[2]: width, roi[3]: height */ bool SetRegionOfInterest( unsigned int roi[4] ); /*! \brief returns the region of interest currently set \return currently set region of interest. */ unsigned int* GetRegionOfInterest(); /*! \brief sets the exposure mode of the CamCube \param mode exposure mode. 0: normal mode (one exposure), 1: Suppression of motion blur (SMB), minimizes the time needed to capture a distance image from the camera which results in a reduced amount of motion artifact but may lead to increased noise. */ bool SetExposureMode( int mode ); /*! \brief Sets the field of view of the camera lens. \param fov field of view in degrees. The default value is 40 degrees. */ bool SetFieldOfView( float fov ); /* \brief Enable/Disable PMD fixed pattern noise (FPN) calibration \param on enabled (true), disabled (false) */ bool SetFPNCalibration( bool on ); /* \brief Enable/Disable PMD fixed pattern phase noise (FPPN) calibration \param on enabled (true), disabled (false) */ bool SetFPPNCalibration( bool on ); /* \brief Enable/Disable PMD linearity calibration \param on enabled (true), disabled (false) */ bool SetLinearityCalibration( bool on ); /* \brief Enable/Disable PMD lens calibration \param on enabled (true), disabled (false) */ bool SetLensCalibration( bool on ); + bool GetAmplitudes(float* amplitudeArray); + bool GetAmplitudes(char* sourceData, float* amplitudeArray); + bool GetIntensities(float* intensityArray); + bool GetIntensities(char* sourceData, float* intensityArray); + bool GetDistances(float* distanceArray); + bool GetDistances(char* sourceData, float* distanceArray); + + itkGetMacro(InternalCaptureWidth, int); + itkGetMacro(InternalCaptureHeight, int); protected: ToFCameraPMDCamBoardController(); ~ToFCameraPMDCamBoardController(); - private: + /* + \brief Transform the output of the camera, i.e. cut invalid pixels, and rotate 90° + counterclockwise + \param input data array of original size (207x204) + \param rotated output data array of reduced size (200x200) + */ + void TransformCameraOutput(float* in, float* out, bool isDist); + // member variables + unsigned int m_InternalCaptureWidth; + unsigned int m_InternalCaptureHeight; + unsigned int m_InternalPixelNumber; }; } //END mitk namespace #endif diff --git a/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.cpp index 5bbe284f84..eb471b368d 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.cpp @@ -1,79 +1,132 @@ /*=================================================================== 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 "mitkToFCameraPMDCamBoardDevice.h" #include "mitkToFCameraPMDCamBoardController.h" #include "mitkRealTimeClock.h" #include "itkMultiThreader.h" namespace mitk { ToFCameraPMDCamBoardDevice::ToFCameraPMDCamBoardDevice() { m_Controller = ToFCameraPMDCamBoardController::New(); } ToFCameraPMDCamBoardDevice::~ToFCameraPMDCamBoardDevice() { } void ToFCameraPMDCamBoardDevice::SetRegionOfInterest(unsigned int leftUpperCornerX, unsigned int leftUpperCornerY, unsigned int width, unsigned int height) { if (m_Controller.IsNotNull()) { dynamic_cast(m_Controller.GetPointer())->SetRegionOfInterest(leftUpperCornerX,leftUpperCornerY,width,height); } } void ToFCameraPMDCamBoardDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { this->m_PropertyList->SetProperty(propertyKey, propertyValue); ToFCameraPMDCamBoardController::Pointer myController = dynamic_cast(this->m_Controller.GetPointer()); bool boolValue = false; GetBoolProperty(propertyKey, boolValue); if (strcmp(propertyKey, "SetFPNCalibration") == 0) { myController->SetFPNCalibration(boolValue); } else if (strcmp(propertyKey, "SetFPPNCalibration") == 0) { myController->SetFPPNCalibration(boolValue); } else if (strcmp(propertyKey, "SetLinearityCalibration") == 0) { myController->SetLinearityCalibration(boolValue); } else if (strcmp(propertyKey, "SetLensCalibration") == 0) { myController->SetLensCalibration(boolValue); } else if (strcmp(propertyKey, "SetExposureMode") == 0) { if (boolValue) { myController->SetExposureMode(1); } else { myController->SetExposureMode(0); } } } + void ToFCameraPMDCamBoardDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, + int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray) + { + if (m_CameraActive) + { + // 1) copy the image buffer + // 2) convert the distance values from m to mm + + // check for empty buffer + if (this->m_ImageSequence < 0) + { + // buffer empty + MITK_INFO << "Buffer empty!! "; + capturedImageSequence = this->m_ImageSequence; + 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; + //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence; + } + 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; + //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence; + } + 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; + } + + m_ImageMutex->Lock(); + this->m_Controller->GetDistances(this->m_SourceDataBuffer[pos], distanceArray); + this->m_Controller->GetAmplitudes(this->m_SourceDataBuffer[pos], amplitudeArray); + this->m_Controller->GetIntensities(this->m_SourceDataBuffer[pos], intensityArray); + memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize); + m_ImageMutex->Unlock(); + + //memcpy(distanceArray, this->m_DistanceArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + //memcpy(intensityArray, this->m_IntensityArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + //memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + } + else + { + MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; + } + } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.h b/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.h index 3d94778532..e2a1164613 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.h +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardDevice.h @@ -1,70 +1,71 @@ /*=================================================================== 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. ===================================================================*/ #ifndef __mitkToFCameraPMDCamBoardDevice_h #define __mitkToFCameraPMDCamBoardDevice_h #include "mitkToFHardwareExports.h" #include "mitkCommon.h" #include "mitkToFCameraDevice.h" #include "mitkToFCameraPMDDevice.h" #include "mitkToFCameraPMDCamBoardController.h" #include "itkObject.h" #include "itkObjectFactory.h" #include "itkMultiThreader.h" #include "itkFastMutexLock.h" namespace mitk { /** * @brief Device class representing a PMD CamBoard camera * * @ingroup ToFHardware */ class MITK_TOFHARDWARE_EXPORT ToFCameraPMDCamBoardDevice : public ToFCameraPMDDevice { public: mitkClassMacro( ToFCameraPMDCamBoardDevice , ToFCameraPMDDevice ); itkNewMacro( Self ); /*! \brief set a BaseProperty */ virtual void SetProperty( const char *propertyKey, BaseProperty* propertyValue ); /*! \brief Setting the region of interest, the camera is configured to only output a certain area of the image. \param leftUpperCornerX x value of left upper corner of region \param leftUpperCornerX y value of left upper corner of region \param width width of region \param height height of region */ void SetRegionOfInterest( unsigned int leftUpperCornerX, unsigned int leftUpperCornerY, unsigned int width, unsigned int height ); + void GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray); + protected: ToFCameraPMDCamBoardDevice(); ~ToFCameraPMDCamBoardDevice(); - private: }; } //END mitk namespace #endif diff --git a/Modules/ToFHardware/mitkToFCameraPMDController.cpp b/Modules/ToFHardware/mitkToFCameraPMDController.cpp index 96784a72f9..2bceef7292 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDController.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDController.cpp @@ -1,169 +1,169 @@ /*=================================================================== 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 "mitkToFCameraPMDController.h" #include #include PMDHandle m_PMDHandle; //TODO PMDDataDescription m_DataDescription; //TODO struct SourceDataStruct { PMDDataDescription dataDescription; char sourceData; }; namespace mitk { ToFCameraPMDController::ToFCameraPMDController(): m_PMDRes(0), m_PixelNumber(40000), m_NumberOfBytes(0), m_CaptureWidth(200), m_CaptureHeight(200), m_SourceDataSize(0), m_SourceDataStructSize(0), m_ConnectionCheck(false), m_InputFileName("") { } ToFCameraPMDController::~ToFCameraPMDController() { } bool ToFCameraPMDController::CloseCameraConnection() { m_PMDRes = pmdClose(m_PMDHandle); m_ConnectionCheck = ErrorText(m_PMDRes); m_PMDHandle = 0; return m_ConnectionCheck; } bool ToFCameraPMDController::ErrorText(int error) { if(error != PMD_OK) { pmdGetLastError (m_PMDHandle, m_PMDError, 128); MITK_ERROR << "Camera Error " << m_PMDError; return false; } else return true; } bool ToFCameraPMDController::UpdateCamera() { m_PMDRes = pmdUpdate(m_PMDHandle); return ErrorText(m_PMDRes); } bool ToFCameraPMDController::GetAmplitudes(float* amplitudeArray) { this->m_PMDRes = pmdGetAmplitudes(m_PMDHandle, amplitudeArray, this->m_NumberOfBytes); return ErrorText(this->m_PMDRes); } bool ToFCameraPMDController::GetAmplitudes(char* sourceData, float* amplitudeArray) { this->m_PMDRes = pmdCalcAmplitudes(m_PMDHandle, amplitudeArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); return ErrorText(this->m_PMDRes); } bool ToFCameraPMDController::GetIntensities(float* intensityArray) { this->m_PMDRes = pmdGetIntensities(m_PMDHandle, intensityArray, this->m_NumberOfBytes); return ErrorText(this->m_PMDRes); } bool ToFCameraPMDController::GetIntensities(char* sourceData, float* intensityArray) { this->m_PMDRes = pmdCalcIntensities(m_PMDHandle, intensityArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); return ErrorText(this->m_PMDRes); } bool ToFCameraPMDController::GetDistances(float* distanceArray) { this->m_PMDRes = pmdGetDistances(m_PMDHandle, distanceArray, this->m_NumberOfBytes); return ErrorText(this->m_PMDRes); } + bool ToFCameraPMDController::GetDistances(char* sourceData, float* distanceArray) + { + this->m_PMDRes = pmdCalcDistances(m_PMDHandle, distanceArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); + return ErrorText(this->m_PMDRes); + } + bool ToFCameraPMDController::GetSourceData(char* sourceDataArray) { this->m_PMDRes = pmdGetSourceDataDescription(m_PMDHandle, &m_DataDescription); if (!ErrorText(this->m_PMDRes)) { return false; } memcpy(&((SourceDataStruct*)sourceDataArray)->dataDescription, &m_DataDescription, sizeof(m_DataDescription)); this->m_PMDRes = pmdGetSourceData(m_PMDHandle, &((SourceDataStruct*)sourceDataArray)->sourceData, this->m_SourceDataSize); return ErrorText(this->m_PMDRes); } bool ToFCameraPMDController::GetShortSourceData( short* sourceData) { this->m_PMDRes = pmdGetSourceDataDescription(m_PMDHandle,&m_DataDescription); ErrorText( this->m_PMDRes); this->m_PMDRes = pmdGetSourceData(m_PMDHandle,sourceData,m_DataDescription.size); return ErrorText( this->m_PMDRes); } - bool ToFCameraPMDController::GetDistances(char* sourceData, float* distanceArray) - { - this->m_PMDRes = pmdCalcDistances(m_PMDHandle, distanceArray, this->m_NumberOfBytes, m_DataDescription, &((SourceDataStruct*)sourceData)->sourceData); - return ErrorText(this->m_PMDRes); - } - int ToFCameraPMDController::SetIntegrationTime(unsigned int integrationTime) { unsigned int result; this->m_PMDRes = pmdGetValidIntegrationTime(m_PMDHandle, &result, 0, AtLeast, integrationTime); ErrorText(this->m_PMDRes); if (this->m_PMDRes != 0) { return 0; } this->m_PMDRes = pmdSetIntegrationTime(m_PMDHandle, 0, result); ErrorText(this->m_PMDRes); return result; } int ToFCameraPMDController::GetIntegrationTime() { unsigned int integrationTime = 0; this->m_PMDRes = pmdGetIntegrationTime(m_PMDHandle, &integrationTime, 0); ErrorText(this->m_PMDRes); return integrationTime; } int ToFCameraPMDController::SetModulationFrequency(unsigned int modulationFrequency) { unsigned int result; this->m_PMDRes = pmdGetValidModulationFrequency(m_PMDHandle, &result, 0, AtLeast, (modulationFrequency*1000000)); ErrorText(this->m_PMDRes); if (this->m_PMDRes != 0) { return 0; } this->m_PMDRes = pmdSetModulationFrequency(m_PMDHandle, 0, result); ErrorText(this->m_PMDRes); return (result/1000000);; } int ToFCameraPMDController::GetModulationFrequency() { unsigned int modulationFrequency = 0; this->m_PMDRes = pmdGetModulationFrequency (m_PMDHandle, &modulationFrequency, 0); ErrorText(this->m_PMDRes); return (modulationFrequency/1000000); } void ToFCameraPMDController::SetInputFileName(std::string inputFileName) { this->m_InputFileName = inputFileName; } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDDevice.cpp index e369ab3c70..2c94d47052 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDDevice.cpp @@ -1,410 +1,414 @@ /*=================================================================== 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 "mitkToFCameraPMDDevice.h" #include "mitkRealTimeClock.h" #include "itkMultiThreader.h" #include namespace mitk { ToFCameraPMDDevice::ToFCameraPMDDevice() : m_SourceDataBuffer(NULL), m_SourceDataArray(NULL) { } ToFCameraPMDDevice::~ToFCameraPMDDevice() { this->CleanUpSourceData(); CleanupPixelArrays(); } bool ToFCameraPMDDevice::ConnectCamera() { bool ok = false; if (m_Controller) { ok = m_Controller->OpenCameraConnection(); if (ok) { this->m_CaptureWidth = m_Controller->GetCaptureWidth(); this->m_CaptureHeight = m_Controller->GetCaptureHeight(); this->m_SourceDataSize = m_Controller->GetSourceDataStructSize(); this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight; // allocate buffers - AllocatePixelArrays(); + this->AllocatePixelArrays(); this->AllocateSourceData(); - m_CameraConnected = true; + this->m_CameraConnected = true; } } return ok; } bool ToFCameraPMDDevice::DisconnectCamera() { bool ok = false; if (m_Controller) { ok = m_Controller->CloseCameraConnection(); if (ok) { m_CameraConnected = false; } } return ok; } void ToFCameraPMDDevice::StartCamera() { if (m_CameraConnected) { // get the first image this->m_Controller->UpdateCamera(); this->m_ImageMutex->Lock(); //this->m_Controller->GetSourceData(this->m_SourceDataArray); this->m_Controller->GetSourceData(this->m_SourceDataBuffer[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 ToFCameraPMDDevice::StopCamera() { m_CameraActiveMutex->Lock(); m_CameraActive = false; m_CameraActiveMutex->Unlock(); itksys::SystemTools::Delay(100); if (m_MultiThreader.IsNotNull()) { m_MultiThreader->TerminateThread(m_ThreadID); } // wait a little to make sure that the thread is terminated itksys::SystemTools::Delay(10); } bool ToFCameraPMDDevice::IsCameraActive() { m_CameraActiveMutex->Lock(); bool ok = m_CameraActive; m_CameraActiveMutex->Unlock(); return ok; } void ToFCameraPMDDevice::UpdateCamera() { if (m_Controller) { m_Controller->UpdateCamera(); } } ITK_THREAD_RETURN_TYPE ToFCameraPMDDevice::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; } ToFCameraPMDDevice* toFCameraDevice = (ToFCameraPMDDevice*)pInfo->UserData; if (toFCameraDevice!=NULL) { mitk::RealTimeClock::Pointer realTimeClock; realTimeClock = mitk::RealTimeClock::New(); double t1, t2; t1 = realTimeClock->GetCurrentStamp(); int n = 100; bool overflow = false; bool printStatus = false; while (toFCameraDevice->IsCameraActive()) { // update the ToF camera toFCameraDevice->UpdateCamera(); // get the source data from the camera and write it at the next free position in the buffer toFCameraDevice->m_ImageMutex->Lock(); toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataBuffer[toFCameraDevice->m_FreePos]); toFCameraDevice->m_ImageMutex->Unlock(); // call modified to indicate that cameraDevice was modified toFCameraDevice->Modified(); /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TODO Buffer Handling currently only works for buffer size 1 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ //toFCameraDevice->m_ImageSequence++; 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) { // buffer overflow //MITK_INFO << "Buffer overflow!! "; //toFCameraDevice->m_CurrentPos = (toFCameraDevice->m_CurrentPos+1) % toFCameraDevice->m_BufferSize; //toFCameraDevice->m_ImageSequence++; overflow = true; } if (toFCameraDevice->m_ImageSequence % n == 0) { printStatus = true; } if (overflow) { //itksys::SystemTools::Delay(10); overflow = false; } /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! END TODO Buffer Handling currently only works for buffer size 1 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ // print current framerate if (printStatus) { t2 = realTimeClock->GetCurrentStamp() - t1; //MITK_INFO << "t2: " << t2 <<" Time (s) for 1 image: " << (t2/1000) / n << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence; 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; } // TODO: Buffer size currently set to 1. Once Buffer handling is working correctly, method may be reactivated // void ToFCameraPMDDevice::ResetBuffer(int bufferSize) // { // this->m_BufferSize = bufferSize; // this->m_CurrentPos = -1; // this->m_FreePos = 0; // } void ToFCameraPMDDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence) { if (m_CameraActive) { // 1) copy the image buffer // 2) Flip around y- axis (vertical axis) m_ImageMutex->Lock(); this->m_Controller->GetAmplitudes(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_AmplitudeArray); m_ImageMutex->Unlock(); for (int i=0; im_CaptureHeight; i++) { for (int j=0; jm_CaptureWidth; j++) { amplitudeArray[i*this->m_CaptureWidth+j] = this->m_AmplitudeArray[(i+1)*this->m_CaptureWidth-1-j]; } } imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDDevice::GetIntensities(float* intensityArray, int& imageSequence) { if (m_CameraActive) { // 1) copy the image buffer // 2) Flip around y- axis (vertical axis) m_ImageMutex->Lock(); this->m_Controller->GetIntensities(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_IntensityArray); m_ImageMutex->Unlock(); for (int i=0; im_CaptureHeight; i++) { for (int j=0; jm_CaptureWidth; j++) { intensityArray[i*this->m_CaptureWidth+j] = this->m_IntensityArray[(i+1)*this->m_CaptureWidth-1-j]; } } imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } m_ImageMutex->Unlock(); } void ToFCameraPMDDevice::GetDistances(float* distanceArray, int& imageSequence) { if (m_CameraActive) { // 1) copy the image buffer // 2) convert the distance values from m to mm // 3) Flip around y- axis (vertical axis) m_ImageMutex->Lock(); this->m_Controller->GetDistances(this->m_SourceDataBuffer[this->m_CurrentPos], this->m_DistanceArray); m_ImageMutex->Unlock(); for (int i=0; im_CaptureHeight; i++) { for (int j=0; jm_CaptureWidth; j++) { distanceArray[i*this->m_CaptureWidth+j] = 1000 * this->m_DistanceArray[(i+1)*this->m_CaptureWidth-1-j]; } } imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray) { if (m_CameraActive) { // 1) copy the image buffer // 2) convert the distance values from m to mm // 3) Flip around y- axis (vertical axis) // check for empty buffer if (this->m_ImageSequence < 0) { // buffer empty MITK_INFO << "Buffer empty!! "; capturedImageSequence = this->m_ImageSequence; 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; //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence; } 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; //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence; } 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; } m_ImageMutex->Lock(); this->m_Controller->GetDistances(this->m_SourceDataBuffer[pos], this->m_DistanceArray); this->m_Controller->GetAmplitudes(this->m_SourceDataBuffer[pos], this->m_AmplitudeArray); this->m_Controller->GetIntensities(this->m_SourceDataBuffer[pos], this->m_IntensityArray); memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize); m_ImageMutex->Unlock(); - int u, v; - for (int i=0; im_CaptureHeight; i++) - { - for (int j=0; jm_CaptureWidth; j++) - { - u = i*this->m_CaptureWidth+j; - v = (i+1)*this->m_CaptureWidth-1-j; - distanceArray[u] = 1000 * this->m_DistanceArray[v]; // unit in minimeter - //distanceArray[u] = this->m_DistanceArray[v]; // unit in meter - amplitudeArray[u] = this->m_AmplitudeArray[v]; - intensityArray[u] = this->m_IntensityArray[v]; - } - } + memcpy(distanceArray, this->m_DistanceArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + memcpy(intensityArray, this->m_IntensityArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_CaptureWidth*this->m_CaptureHeight*sizeof(float)); + + //int u, v; + //for (int i=0; im_CaptureHeight; i++) + //{ + // for (int j=0; jm_CaptureWidth; j++) + // { + // u = i*this->m_CaptureWidth+j; + // v = (i+1)*this->m_CaptureWidth-1-j; + // distanceArray[u] = 1000 * this->m_DistanceArray[v]; // unit in minimeter + // //distanceArray[u] = this->m_DistanceArray[v]; // unit in meter + // amplitudeArray[u] = this->m_AmplitudeArray[v]; + // intensityArray[u] = this->m_IntensityArray[v]; + // } + //} } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } ToFCameraPMDController::Pointer ToFCameraPMDDevice::GetController() { return this->m_Controller; } void ToFCameraPMDDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { ToFCameraDevice::SetProperty(propertyKey,propertyValue); this->m_PropertyList->SetProperty(propertyKey, propertyValue); if (strcmp(propertyKey, "ModulationFrequency") == 0) { int modulationFrequency = 0; GetIntProperty(propertyKey, modulationFrequency); m_Controller->SetModulationFrequency(modulationFrequency); } else if (strcmp(propertyKey, "IntegrationTime") == 0) { int integrationTime = 0; GetIntProperty(propertyKey, integrationTime); m_Controller->SetIntegrationTime(integrationTime); } } void ToFCameraPMDDevice::AllocateSourceData() { // clean up if array and data have already been allocated CleanUpSourceData(); // (re-) allocate memory this->m_SourceDataArray = new char[this->m_SourceDataSize]; for(int i=0; im_SourceDataSize; i++) {this->m_SourceDataArray[i]=0;} this->m_SourceDataBuffer = new char*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_SourceDataBuffer[i] = new char[this->m_SourceDataSize]; } } void ToFCameraPMDDevice::CleanUpSourceData() { if (m_SourceDataArray) { delete[] m_SourceDataArray; } if (m_SourceDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_SourceDataBuffer[i]; } delete[] this->m_SourceDataBuffer; } } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp index b6ebfe6d69..07ee919472 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp @@ -1,102 +1,224 @@ /*=================================================================== 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 "mitkToFCameraPMDRawDataCamBoardDevice.h" #include "mitkToFCameraPMDCamBoardController.h" // vtk includes #include "vtkSmartPointer.h" namespace mitk { ToFCameraPMDRawDataCamBoardDevice::ToFCameraPMDRawDataCamBoardDevice() { m_Controller = ToFCameraPMDCamBoardController::New(); } ToFCameraPMDRawDataCamBoardDevice::~ToFCameraPMDRawDataCamBoardDevice() { } + bool ToFCameraPMDRawDataCamBoardDevice::ConnectCamera() + { + bool ok = Superclass::ConnectCamera(); + if(ok) + { + this->m_OriginControllerHeight = static_cast(m_Controller.GetPointer())->GetInternalCaptureHeight(); + this->m_OriginControllerWidth = static_cast(m_Controller.GetPointer())->GetInternalCaptureWidth(); + this->m_PixelNumber = m_OriginControllerHeight* m_OriginControllerWidth; + this->AllocatePixelArrays(); + this->AllocateSourceData(); + } + return ok; + } + void ToFCameraPMDRawDataCamBoardDevice::GetChannelSourceData(short* sourceData, vtkShortArray* vtkChannelArray ) { int i = 0; //unsigned int channelDataPosition = 0; - unsigned int channelSize = (this->GetCaptureHeight()*this->GetCaptureWidth()*2); + unsigned int channelSize = (this->m_OriginControllerHeight*this->m_OriginControllerWidth*2); this->SetChannelSize(channelSize); signed short* channel1; signed short* channel2; signed short* channel3; signed short* channel4; vtkSmartPointer cvtkChannelArray = vtkShortArray::New(); cvtkChannelArray->SetNumberOfComponents(channelSize); cvtkChannelArray->SetNumberOfTuples(4); cvtkChannelArray->Allocate(1); channel1 = sourceData; cvtkChannelArray->InsertTupleValue(0,channel1); sourceData += channelSize; channel2 = sourceData; cvtkChannelArray->InsertTupleValue(1,channel2); sourceData += channelSize; channel3 = sourceData; cvtkChannelArray->InsertTupleValue(2,channel3); sourceData += channelSize; channel4 = sourceData; cvtkChannelArray->InsertTupleValue(3,channel4); vtkChannelArray->DeepCopy(cvtkChannelArray); cvtkChannelArray->Delete(); } void ToFCameraPMDRawDataCamBoardDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { ToFCameraPMDRawDataDevice::SetProperty(propertyKey,propertyValue); this->m_PropertyList->SetProperty(propertyKey, propertyValue); ToFCameraPMDCamBoardController::Pointer myController = dynamic_cast(this->m_Controller.GetPointer()); bool boolValue = false; GetBoolProperty(propertyKey, boolValue); if (strcmp(propertyKey, "SetFPNCalibration") == 0) { myController->SetFPNCalibration(boolValue); } else if (strcmp(propertyKey, "SetFPPNCalibration") == 0) { myController->SetFPPNCalibration(boolValue); } else if (strcmp(propertyKey, "SetLinearityCalibration") == 0) { myController->SetLinearityCalibration(boolValue); } else if (strcmp(propertyKey, "SetLensCalibration") == 0) { myController->SetLensCalibration(boolValue); } else if (strcmp(propertyKey, "SetExposureMode") == 0) { if (boolValue) { myController->SetExposureMode(1); } else { myController->SetExposureMode(0); } } } + void ToFCameraPMDRawDataCamBoardDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence) + { + if (m_CameraActive) + { + this->ResizeOutputImage(m_AmplitudeArray,amplitudeArray); + //memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float)); + imageSequence = this->m_ImageSequence; + } + else + { + MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; + } + } + + void ToFCameraPMDRawDataCamBoardDevice::GetIntensities(float* intensityArray, int& imageSequence) + { + if (m_CameraActive) + { + this->ResizeOutputImage(m_IntensityArray, intensityArray); + // memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float)); + imageSequence = this->m_ImageSequence; + } + else + { + MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; + } + } + + void ToFCameraPMDRawDataCamBoardDevice::GetDistances(float* distanceArray, int& imageSequence) + { + if (m_CameraActive) + { + this->ResizeOutputImage(m_DistanceArray, distanceArray); + //memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float)); + imageSequence = this->m_ImageSequence; + } + else + { + MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; + } + } + + void ToFCameraPMDRawDataCamBoardDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, + int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray) + { + if (m_CameraActive) + { + // 1) copy the image buffer + // 2) convert the distance values from m to mm + // 3) Flip around y- axis (vertical axis) + + // check for empty buffer + if (this->m_ImageSequence < 0) + { + // buffer empty + MITK_INFO << "Buffer empty!! "; + capturedImageSequence = this->m_ImageSequence; + 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; + //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence; + } + 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; + //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence; + } + 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; + } + + this->ResizeOutputImage(m_DistanceArray, distanceArray); + this->ResizeOutputImage(m_AmplitudeArray, amplitudeArray); + this->ResizeOutputImage(m_IntensityArray, intensityArray); + //memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float)); + //memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float)); + //memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float)); + memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize); + } + else + { + MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; + } + } + void ToFCameraPMDRawDataCamBoardDevice::ResizeOutputImage(float* in, float* out) + { + vnl_matrix inMat = vnl_matrix(m_OriginControllerHeight, m_OriginControllerWidth); + inMat.copy_in(in); + vnl_matrix outMat = vnl_matrix(m_CaptureHeight,m_CaptureWidth); + vnl_matrix temp = vnl_matrix(m_CaptureHeight,m_CaptureWidth); + temp = inMat.extract(m_CaptureHeight,m_CaptureWidth, 0, 4); + outMat = temp.transpose(); // rotate the image data + outMat.flipud(); + outMat.copy_out(out); + inMat.clear(); + outMat.clear(); + temp.clear(); + } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h index 4b1400ef9b..471642e7d4 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h @@ -1,54 +1,62 @@ /*=================================================================== 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. ===================================================================*/ #ifndef __mitkToFCameraPMDRawDataCamBoardDevice_h #define __mitkToFCameraPMDRawDataCamBoardDevice_h #include "mitkToFHardwareExports.h" #include "mitkCommon.h" #include "mitkToFCameraDevice.h" #include "mitkToFCameraPMDRawDataDevice.h" namespace mitk { /** * @brief Device class representing a PMD CamCube camera * * * @ingroup ToFHardwareMBI */ class MITK_TOFHARDWARE_EXPORT ToFCameraPMDRawDataCamBoardDevice : public ToFCameraPMDRawDataDevice { public: mitkClassMacro( ToFCameraPMDRawDataCamBoardDevice , ToFCameraPMDRawDataDevice ); itkNewMacro( Self ); virtual void SetProperty( const char *propertyKey, BaseProperty* propertyValue ); virtual void GetChannelSourceData(short* sourceData, vtkShortArray* vtkChannelArray ); + bool ConnectCamera(); + void GetIntensities(float* intensityArray, int& imageSequence); + void GetAmplitudes(float* amplitudeArray, int& imageSequence); + void GetDistances(float* distanceArray, int& imageSequence); + void GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray); protected: ToFCameraPMDRawDataCamBoardDevice(); ~ToFCameraPMDRawDataCamBoardDevice(); private: - + void ResizeOutputImage(float* in, float* out); + // member variables + //unsigned int m_OriginControllerHeight; + //unsigned int m_OriginControllerWidth; }; } //END mitk namespace #endif // __mitkToFCameraPMDRawDataCamBoardDevice_h diff --git a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp index 3ac6750a4c..beccaf004c 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp @@ -1,465 +1,471 @@ /*=================================================================== 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 "mitkToFCameraPMDRawDataDevice.h" #include "mitkRealTimeClock.h" - +#include "mitkToFCameraPMDCamBoardController.h" #include "itkMultiThreader.h" #include namespace mitk { ToFCameraPMDRawDataDevice::ToFCameraPMDRawDataDevice() : - m_SourceDataBuffer(NULL), m_SourceDataArray(NULL), m_ShortSourceData(NULL) + m_SourceDataBuffer(NULL), m_SourceDataArray(NULL), m_ShortSourceData(NULL), + m_OriginControllerWidth(0), m_OriginControllerHeight(0) { m_RawDataSource = ThreadedToFRawDataReconstruction::New(); } ToFCameraPMDRawDataDevice::~ToFCameraPMDRawDataDevice() { this->CleanUpSourceData(); } bool ToFCameraPMDRawDataDevice::ConnectCamera() { bool ok = false; if (m_Controller.IsNotNull()) { ok = m_Controller->OpenCameraConnection(); if (ok) { this->m_CaptureWidth = m_Controller->GetCaptureWidth(); this->m_CaptureHeight = m_Controller->GetCaptureHeight(); + this->m_OriginControllerHeight = m_CaptureHeight; + this->m_OriginControllerWidth = m_CaptureWidth; this->m_SourceDataSize = m_Controller->GetSourceDataStructSize(); this->m_PixelNumber = this->m_CaptureWidth * this->m_CaptureHeight; // allocate buffers this->AllocatePixelArrays(); this->AllocateSourceData(); m_CameraConnected = true; } } return ok; } bool ToFCameraPMDRawDataDevice::DisconnectCamera() { bool ok = false; if (m_Controller) { ok = m_Controller->CloseCameraConnection(); if (ok) { m_CameraConnected = false; } } return ok; } void ToFCameraPMDRawDataDevice::StartCamera() { if (m_CameraConnected) { // get the first image this->m_Controller->UpdateCamera(); this->m_ImageMutex->Lock(); this->m_Controller->GetSourceData(this->m_SourceDataBuffer[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(100); } else { MITK_INFO<<"Camera not connected"; } } void ToFCameraPMDRawDataDevice::StopCamera() { m_CameraActiveMutex->Lock(); m_CameraActive = false; m_CameraActiveMutex->Unlock(); itksys::SystemTools::Delay(100); if (m_MultiThreader.IsNotNull()) { m_MultiThreader->TerminateThread(m_ThreadID); } // wait a little to make sure that the thread is terminated itksys::SystemTools::Delay(10); } bool ToFCameraPMDRawDataDevice::IsCameraActive() { m_CameraActiveMutex->Lock(); bool ok = m_CameraActive; m_CameraActiveMutex->Unlock(); return ok; } void ToFCameraPMDRawDataDevice::UpdateCamera() { if (m_Controller) { m_Controller->UpdateCamera(); } } ITK_THREAD_RETURN_TYPE ToFCameraPMDRawDataDevice::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; } ToFCameraPMDRawDataDevice* toFCameraDevice = (ToFCameraPMDRawDataDevice*)pInfo->UserData; if (toFCameraDevice!=NULL) { mitk::RealTimeClock::Pointer realTimeClock = mitk::RealTimeClock::New(); double t1, t2; t1 = realTimeClock->GetCurrentStamp(); int n = 100; bool overflow = false; bool printStatus = false; while (toFCameraDevice->IsCameraActive()) { // update the ToF camera toFCameraDevice->UpdateCamera(); // get the source data from the camera and write it at the next free position in the buffer // call modified to indicate that cameraDevice was modified toFCameraDevice->Modified(); vtkShortArray* channelData = vtkShortArray::New(); //toFCameraDevice->m_ImageMutex->Lock(); toFCameraDevice->m_Controller->GetShortSourceData(toFCameraDevice->m_ShortSourceData); toFCameraDevice->GetChannelSourceData( toFCameraDevice->m_ShortSourceData, channelData ); //toFCameraDevice->m_ImageMutex->Unlock(); if(!toFCameraDevice->m_RawDataSource->GetInit()) { - toFCameraDevice->m_RawDataSource->Initialize(toFCameraDevice->m_CaptureWidth, toFCameraDevice->m_CaptureHeight, - toFCameraDevice->m_Controller->GetModulationFrequency(), toFCameraDevice->GetChannelSize()); + toFCameraDevice->m_RawDataSource->Initialize(toFCameraDevice->m_OriginControllerWidth, + toFCameraDevice->m_OriginControllerHeight, + toFCameraDevice->m_Controller->GetModulationFrequency(), + toFCameraDevice->GetChannelSize()); } toFCameraDevice->m_RawDataSource->SetChannelData(channelData); toFCameraDevice->m_RawDataSource->Update(); toFCameraDevice->m_ImageMutex->Lock(); toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataArray); - toFCameraDevice->m_RawDataSource->GetAllData(toFCameraDevice->m_DistanceArray, toFCameraDevice->m_AmplitudeArray, toFCameraDevice->m_IntensityArray); + toFCameraDevice->m_RawDataSource->GetAllData(toFCameraDevice->m_DistanceArray, + toFCameraDevice->m_AmplitudeArray, toFCameraDevice->m_IntensityArray); toFCameraDevice->m_ImageMutex->Unlock(); /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TODO Buffer Handling currently only works for buffer size 1 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ 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; } channelData->Delete(); if (overflow) { overflow = false; } /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! END TODO Buffer Handling currently only works for buffer size 1 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ // print current framerate if (printStatus) { t2 = realTimeClock->GetCurrentStamp() - t1; //MITK_INFO << "t2: " << t2 <<" Time (s) for 1 image: " << (t2/1000) / n << " Framerate (fps): " << n / (t2/1000) << " Sequence: " << toFCameraDevice->m_ImageSequence; 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; } // TODO: Buffer size currently set to 1. Once Buffer handling is working correctly, method may be reactivated // void ToFCameraPMDDevice::ResetBuffer(int bufferSize) // { // this->m_BufferSize = bufferSize; // this->m_CurrentPos = -1; // this->m_FreePos = 0; // } void ToFCameraPMDRawDataDevice::GetAmplitudes(float* amplitudeArray, int& imageSequence) { if (m_CameraActive) { memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float)); imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDRawDataDevice::GetIntensities(float* intensityArray, int& imageSequence) { if (m_CameraActive) { memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float)); imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDRawDataDevice::GetDistances(float* distanceArray, int& imageSequence) { if (m_CameraActive) { memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float)); imageSequence = this->m_ImageSequence; } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDRawDataDevice::GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray) { if (m_CameraActive) { // 1) copy the image buffer // 2) convert the distance values from m to mm // 3) Flip around y- axis (vertical axis) // check for empty buffer if (this->m_ImageSequence < 0) { // buffer empty MITK_INFO << "Buffer empty!! "; capturedImageSequence = this->m_ImageSequence; 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; //MITK_INFO << "Required image not found! Required: " << requiredImageSequence << " delivered/current: " << this->m_ImageSequence; } 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; //MITK_INFO << "Out of buffer! Required: " << requiredImageSequence << " delivered: " << capturedImageSequence << " current: " << this->m_ImageSequence; } 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; } memcpy(distanceArray, this->m_DistanceArray, this->m_PixelNumber*sizeof(float)); memcpy(amplitudeArray, this->m_AmplitudeArray, this->m_PixelNumber*sizeof(float)); memcpy(intensityArray, this->m_IntensityArray, this->m_PixelNumber*sizeof(float)); memcpy(sourceDataArray, this->m_SourceDataBuffer[this->m_CurrentPos], this->m_SourceDataSize); } else { MITK_WARN("ToF") << "Warning: Data can only be acquired if camera is active."; } } void ToFCameraPMDRawDataDevice::XYAxisFlipImage( float* imageData, float* &flippedData, int xAxis, int yAxis, int dimension ) { int captureWidth = this->GetCaptureWidth(); int captureHeight = this->GetCaptureHeight(); // //flips image around x- axis (horizontal axis) // if(xAxis == 1 && yAxis != 1) { for (int i=0; im_Controller; } void ToFCameraPMDRawDataDevice::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { ToFCameraDevice::SetProperty(propertyKey,propertyValue); this->m_PropertyList->SetProperty(propertyKey, propertyValue); if (strcmp(propertyKey, "ModulationFrequency") == 0) { int modulationFrequency = 0; GetIntProperty(propertyKey, modulationFrequency); m_Controller->SetModulationFrequency(modulationFrequency); } else if (strcmp(propertyKey, "IntegrationTime") == 0) { int integrationTime = 0; GetIntProperty(propertyKey, integrationTime); m_Controller->SetIntegrationTime(integrationTime); } } void ToFCameraPMDRawDataDevice::CleanupPixelArrays() { if (m_IntensityArray) { delete [] m_IntensityArray; } if (m_DistanceArray) { delete [] m_DistanceArray; } if (m_AmplitudeArray) { delete [] m_AmplitudeArray; } if (m_ShortSourceData) { delete [] m_ShortSourceData; } } void ToFCameraPMDRawDataDevice::AllocatePixelArrays() { // free memory if it was already allocated CleanupPixelArrays(); // allocate buffer this->m_IntensityArray = new float[this->m_PixelNumber]; for(int i=0; im_PixelNumber; i++) {this->m_IntensityArray[i]=0.0;} 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_ShortSourceData = new short[this->m_SourceDataSize]; for(int i=0; im_SourceDataSize; i++) {this->m_ShortSourceData[i]=0.0;} } void ToFCameraPMDRawDataDevice::AllocateSourceData() { // clean up if array and data have already been allocated CleanUpSourceData(); // (re-) allocate memory this->m_SourceDataArray = new char[this->m_SourceDataSize]; for(int i=0; im_SourceDataSize; i++) {this->m_SourceDataArray[i]=0;} this->m_SourceDataBuffer = new char*[this->m_MaxBufferSize]; for(int i=0; im_MaxBufferSize; i++) { this->m_SourceDataBuffer[i] = new char[this->m_SourceDataSize]; } } void ToFCameraPMDRawDataDevice::CleanUpSourceData() { if (m_SourceDataArray) { delete[] m_SourceDataArray; } if (m_SourceDataBuffer) { for(int i=0; im_MaxBufferSize; i++) { delete[] this->m_SourceDataBuffer[i]; } delete[] this->m_SourceDataBuffer; } } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h index b687e75a34..a53c4c0443 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h @@ -1,175 +1,178 @@ /*=================================================================== 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. ===================================================================*/ #ifndef __mitkToFCameraPMDRawDataDevice_h #define __mitkToFCameraPMDRawDataDevice_h #include "mitkToFHardwareExports.h" #include "mitkToFCameraDevice.h" #include "mitkToFCameraPMDController.h" #include "mitkThreadedToFRawDataReconstruction.h" namespace mitk { /** * @brief Interface for all representations of PMD ToF devices. * ToFCameraPMDDevice internally holds an instance of ToFCameraPMDController and starts a thread * that continuously grabs images from the controller. A buffer structure buffers the last acquired images * to provide the image data loss-less. * * @ingroup ToFHardware */ class MITK_TOFHARDWARE_EXPORT ToFCameraPMDRawDataDevice : public ToFCameraDevice { public: mitkClassMacro( ToFCameraPMDRawDataDevice , ToFCameraDevice ); itkNewMacro( Self ); itkSetMacro(ChannelSize, int); itkGetMacro(ChannelSize, int); /*! \brief opens a connection to the ToF camera */ virtual bool ConnectCamera(); /*! \brief closes the connection to the camera */ virtual bool DisconnectCamera(); /*! \brief starts the continuous updating of the camera. A separate thread updates the source data, the main thread processes the source data and creates images and coordinates */ virtual void StartCamera(); /*! \brief stops the continuous updating of the camera */ virtual void StopCamera(); /*! \brief updates the camera for image acquisition */ virtual void UpdateCamera(); /*! \brief returns whether the camera is currently active or not */ virtual bool IsCameraActive(); /*! \brief gets the amplitude data from the ToF camera as the strength of the active illumination of every pixel. Caution! The user is responsible for allocating and deleting the images. These values can be used to determine the quality of the distance values. The higher the amplitude value, the higher the accuracy of the according distance value \param imageSequence the actually captured image sequence number \param amplitudeArray contains the returned amplitude data as an array. */ virtual void GetAmplitudes(float* amplitudeArray, int& imageSequence); /*! \brief gets the intensity data from the ToF camera as a greyscale image. Caution! The user is responsible for allocating and deleting the images. \param intensityArray contains the returned intensities data as an array. \param imageSequence the actually captured image sequence number */ virtual void GetIntensities(float* intensityArray, int& imageSequence); /*! \brief gets the distance data from the ToF camera measuring the distance between the camera and the different object points in millimeters. Caution! The user is responsible for allocating and deleting the images. \param distanceArray contains the returned distances data as an array. \param imageSequence the actually captured image sequence number */ virtual void GetDistances(float* distanceArray, int& imageSequence); /*! \brief gets the 3 images (distance, amplitude, intensity) from the ToF camera. Caution! The user is responsible for allocating and deleting the images. \param distanceArray contains the returned distance data as an array. \param amplitudeArray contains the returned amplitude data as an array. \param intensityArray contains the returned intensity data as an array. \param sourceDataArray contains the complete source data from the camera device. \param requiredImageSequence the required image sequence number \param capturedImageSequence the actually captured image sequence number */ virtual void GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray=NULL); // TODO: Buffer size currently set to 1. Once Buffer handling is working correctly, method may be reactivated // /*! // \brief pure virtual method resetting the buffer using the specified bufferSize. Has to be implemented by sub-classes // \param bufferSize buffer size the buffer should be reset to // */ // virtual void ResetBuffer(int bufferSize) = 0; //TODO add/correct documentation for requiredImageSequence and capturedImageSequence in the GetAllImages, GetDistances, GetIntensities and GetAmplitudes methods. /*! \brief returns the corresponding camera controller */ ToFCameraPMDController::Pointer GetController(); virtual void GetChannelSourceData(short* /*sourceData*/, vtkShortArray* /*vtkChannelArray*/ ){}; /*! \brief set a BaseProperty */ virtual void SetProperty( const char *propertyKey, BaseProperty* propertyValue ); protected: ToFCameraPMDRawDataDevice(); ~ToFCameraPMDRawDataDevice(); /*! \brief method for allocating m_SourceDataArray and m_SourceDataBuffer */ virtual void AllocateSourceData(); /*! \brief method for cleaning up memory allocated for m_SourceDataArray and m_SourceDataBuffer */ virtual void CleanUpSourceData(); /*! \brief method for allocating memory for pixel arrays m_IntensityArray, m_DistanceArray and m_AmplitudeArray */ virtual void AllocatePixelArrays(); /*! \brief method for cleanup memory allocated for pixel arrays m_IntensityArray, m_DistanceArray and m_AmplitudeArray */ virtual void CleanupPixelArrays(); /*! \brief Thread method continuously acquiring images from the ToF hardware */ static ITK_THREAD_RETURN_TYPE Acquire(void* pInfoStruct); /*! \brief moves the position pointer m_CurrentPos to the next position in the buffer if that's not the next free position to prevent reading from an empty buffer */ void GetNextPos(); /*! \brief gets the image data and flips it according to user needs Caution! The user is responsible for allocating and deleting the data. \param imageData contains array to the input data. \param flippedData contains flipped output array - Caution! The user is responsible for allocating and deleting the data. Size should be equal to imageData! \param xAxis flag is set to flip around x axis (1 - set, 0 - not set). \param yAxis flag is set to flip around y axis (1 - set, 0 - not set). \param dimension contains the extend of the z dimension (preset is 1) */ void XYAxisFlipImage( float* imageData, float* &flippedData, int xAxis, int yAxis, int dimension = 1 ); ToFCameraPMDController::Pointer m_Controller; ///< corresponding CameraController ThreadedToFRawDataReconstruction::Pointer m_RawDataSource; char** m_SourceDataBuffer; ///< buffer holding the last acquired images char* m_SourceDataArray; ///< array holding the current PMD source data short* m_ShortSourceData; ///< array holding the current PMD raw data + unsigned int m_OriginControllerWidth; + unsigned int m_OriginControllerHeight; + private: //member variables int m_ChannelSize; ///< member holds the size of a single raw data channel }; } //END mitk namespace #endif diff --git a/Modules/ToFHardware/mitkToFImageGrabber.cpp b/Modules/ToFHardware/mitkToFImageGrabber.cpp index 992d20bc8f..2ded3427ad 100644 --- a/Modules/ToFHardware/mitkToFImageGrabber.cpp +++ b/Modules/ToFHardware/mitkToFImageGrabber.cpp @@ -1,279 +1,279 @@ /*=================================================================== 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 "mitkToFImageGrabber.h" #include "mitkToFCameraPMDCamCubeDevice.h" #include "itkCommand.h" namespace mitk { ToFImageGrabber::ToFImageGrabber():m_CaptureWidth(204),m_CaptureHeight(204),m_PixelNumber(41616),m_ImageSequence(0), m_IntensityArray(NULL), m_DistanceArray(NULL), m_AmplitudeArray(NULL), m_SourceDataArray(NULL), m_RgbDataArray(NULL) { // Create the output. We use static_cast<> here because we know the default // output must be of type TOutputImage OutputImageType::Pointer output0 = static_cast(this->MakeOutput(0).GetPointer()); OutputImageType::Pointer output1 = static_cast(this->MakeOutput(1).GetPointer()); OutputImageType::Pointer output2 = static_cast(this->MakeOutput(2).GetPointer()); OutputImageType::Pointer output3 = static_cast(this->MakeOutput(3).GetPointer()); mitk::ImageSource::SetNumberOfRequiredOutputs(3); mitk::ImageSource::SetNthOutput(0, output0.GetPointer()); mitk::ImageSource::SetNthOutput(1, output1.GetPointer()); mitk::ImageSource::SetNthOutput(2, output2.GetPointer()); mitk::ImageSource::SetNthOutput(3, output3.GetPointer()); } ToFImageGrabber::~ToFImageGrabber() { if (m_IntensityArray||m_AmplitudeArray||m_DistanceArray||m_RgbDataArray) { if (m_ToFCameraDevice) { m_ToFCameraDevice->RemoveObserver(m_DeviceObserverTag); } this->DisconnectCamera(); this->CleanUpImageArrays(); } } mitk::ImageSource::DataObjectPointer mitk::ImageSource::MakeOutput(unsigned int) { return static_cast(OutputImageType::New().GetPointer()); } void ToFImageGrabber::GenerateData() { int requiredImageSequence = 0; int capturedImageSequence = 0; unsigned int dimensions[3]; dimensions[0] = this->m_ToFCameraDevice->GetCaptureWidth(); dimensions[1] = this->m_ToFCameraDevice->GetCaptureHeight(); dimensions[2] = 1; mitk::PixelType FloatType = MakeScalarPixelType(); // acquire new image data this->m_ToFCameraDevice->GetAllImages(this->m_DistanceArray, this->m_AmplitudeArray, this->m_IntensityArray, this->m_SourceDataArray, requiredImageSequence, this->m_ImageSequence, this->m_RgbDataArray ); mitk::Image::Pointer distanceImage = this->GetOutput(0); if (!distanceImage->IsInitialized()) { distanceImage->ReleaseData(); distanceImage->Initialize(FloatType, 3, dimensions, 1); } mitk::Image::Pointer amplitudeImage = this->GetOutput(1); if (!amplitudeImage->IsInitialized()) { amplitudeImage->ReleaseData(); amplitudeImage->Initialize(FloatType, 3, dimensions, 1); } mitk::Image::Pointer intensityImage = this->GetOutput(2); if (!intensityImage->IsInitialized()) { intensityImage->ReleaseData(); intensityImage->Initialize(FloatType, 3, dimensions, 1); } mitk::Image::Pointer rgbImage = this->GetOutput(3); if (!rgbImage->IsInitialized()) { rgbImage->ReleaseData(); rgbImage->Initialize(mitk::PixelType(MakePixelType, 3>()),3,dimensions,1); } capturedImageSequence = this->m_ImageSequence; if (m_DistanceArray) { distanceImage->SetSlice(this->m_DistanceArray, 0, 0, 0); } if (m_AmplitudeArray) { amplitudeImage->SetSlice(this->m_AmplitudeArray, 0, 0, 0); } if (m_IntensityArray) { intensityImage->SetSlice(this->m_IntensityArray, 0, 0, 0); } if (m_RgbDataArray) { rgbImage->SetSlice(this->m_RgbDataArray, 0, 0, 0); } } bool ToFImageGrabber::ConnectCamera() { bool ok = m_ToFCameraDevice->ConnectCamera(); if (ok) { m_CaptureWidth = m_ToFCameraDevice->GetCaptureWidth(); m_CaptureHeight = m_ToFCameraDevice->GetCaptureHeight(); m_PixelNumber = m_CaptureWidth * m_CaptureHeight; m_SourceDataSize = m_ToFCameraDevice->GetSourceDataSize(); - AllocateImageArrays(); + this->AllocateImageArrays(); } return ok; } bool ToFImageGrabber::DisconnectCamera() { bool success = m_ToFCameraDevice->DisconnectCamera(); return success; } void ToFImageGrabber::StartCamera() { m_ToFCameraDevice->StartCamera(); } void ToFImageGrabber::StopCamera() { m_ToFCameraDevice->StopCamera(); } bool ToFImageGrabber::IsCameraActive() { return m_ToFCameraDevice->IsCameraActive(); } void ToFImageGrabber::SetCameraDevice(ToFCameraDevice* aToFCameraDevice) { m_ToFCameraDevice = aToFCameraDevice; itk::SimpleMemberCommand::Pointer modifiedCommand = itk::SimpleMemberCommand::New(); modifiedCommand->SetCallbackFunction(this, &ToFImageGrabber::OnToFCameraDeviceModified); m_DeviceObserverTag = m_ToFCameraDevice->AddObserver(itk::ModifiedEvent(), modifiedCommand); this->Modified(); } ToFCameraDevice* ToFImageGrabber::GetCameraDevice() { return m_ToFCameraDevice; } int ToFImageGrabber::GetCaptureWidth() { return m_CaptureWidth; } int ToFImageGrabber::GetCaptureHeight() { return m_CaptureHeight; } int ToFImageGrabber::GetPixelNumber() { return m_PixelNumber; } int ToFImageGrabber::SetModulationFrequency(int modulationFrequency) { this->m_ToFCameraDevice->SetProperty("ModulationFrequency",mitk::IntProperty::New(modulationFrequency)); this->Modified(); return modulationFrequency; } int ToFImageGrabber::SetIntegrationTime(int integrationTime) { this->m_ToFCameraDevice->SetProperty("IntegrationTime",mitk::IntProperty::New(integrationTime)); this->Modified(); return integrationTime; } int ToFImageGrabber::GetIntegrationTime() { int integrationTime = 0; this->m_ToFCameraDevice->GetIntProperty("IntegrationTime",integrationTime); return integrationTime; } int ToFImageGrabber::GetModulationFrequency() { int modulationFrequency = 0; this->m_ToFCameraDevice->GetIntProperty("ModulationFrequency",modulationFrequency); return modulationFrequency; } void ToFImageGrabber::SetBoolProperty( const char* propertyKey, bool boolValue ) { SetProperty(propertyKey, mitk::BoolProperty::New(boolValue)); } void ToFImageGrabber::SetIntProperty( const char* propertyKey, int intValue ) { SetProperty(propertyKey, mitk::IntProperty::New(intValue)); } void ToFImageGrabber::SetFloatProperty( const char* propertyKey, float floatValue ) { SetProperty(propertyKey, mitk::FloatProperty::New(floatValue)); } void ToFImageGrabber::SetStringProperty( const char* propertyKey, const char* stringValue ) { SetProperty(propertyKey, mitk::StringProperty::New(stringValue)); } void ToFImageGrabber::SetProperty( const char *propertyKey, BaseProperty* propertyValue ) { this->m_ToFCameraDevice->SetProperty(propertyKey, propertyValue); } void ToFImageGrabber::OnToFCameraDeviceModified() { this->Modified(); } void ToFImageGrabber::CleanUpImageArrays() { // free buffer if (m_IntensityArray) { delete [] m_IntensityArray; m_IntensityArray = NULL; } if (m_DistanceArray) { delete [] m_DistanceArray; m_DistanceArray = NULL; } if (m_AmplitudeArray) { delete [] m_AmplitudeArray; m_AmplitudeArray = NULL; } if (m_SourceDataArray) { delete [] m_SourceDataArray; m_SourceDataArray = NULL; } if (m_RgbDataArray) { delete [] m_RgbDataArray; m_RgbDataArray = NULL; } } void ToFImageGrabber::AllocateImageArrays() { // cleanup memory if necessary this->CleanUpImageArrays(); // allocate buffer m_IntensityArray = new float[m_PixelNumber]; m_DistanceArray = new float[m_PixelNumber]; m_AmplitudeArray = new float[m_PixelNumber]; m_SourceDataArray = new char[m_SourceDataSize]; m_RgbDataArray = new unsigned char[m_PixelNumber*3]; } }