diff --git a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp index 4121f9a9d2..d29e41d00c 100644 --- a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp +++ b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.cpp @@ -1,325 +1,333 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Module: $RCSfile$ Language: C++ Date: $Date$ Version: $Revision: 11415 $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ // 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 PI 3.14159265; #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 ) { 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*2; - int quadChannelSize = channelSize/4; + 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/4; + 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/2)*sizeof(float)); - memcpy(m_CISAmpl, m_ThreadData->m_OutputData.at(1), (channelSize/2)*sizeof(float)); - memcpy(m_CISInten, m_ThreadData->m_OutputData.at(2), (channelSize/2)*sizeof(float)); + 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; - int index = 0; - int index2 = 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 modFreq = fMod; - double intermed1 = 0; - int linewidth = 0; - int frameheight = 0; + 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); - index = (quadrant*2); - index2 = 3-quadrant; + int index = quadrant << 1; + int index2 = 3-quadrant; modFreq = threadData->m_ModulationFrequency; - linewidth = threadData->m_LineWidth; - frameheight = threadData->m_FrameHeight; + 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 += 2*linewidth; + 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 +=2*pi; + if(phi2<0) phi2 +=twoPi; - intermed1 = cair/(4*pi*modFreq); 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*intermed1) + (phi2*intermed1))/2)*1000; - threadData->m_OutputData.at(1)[index2] = (sqrt(A3m1 + A4m2)/2) + (sqrt(A7m5 + A8m6)/2); - threadData->m_OutputData.at(2)[index2] = (A1+A2+A3+A4+A5+A6+A7+A8)/8; + 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 += 2*linewidth; + index += doubleLwidth; - }while(index < 2*(linewidth*frameheight*4)); + }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 8cb4d1c7f7..e622f5b1a7 100644 --- a/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h +++ b/Modules/ToFHardware/mitkThreadedToFRawDataReconstruction.h @@ -1,115 +1,116 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Module: $RCSfile$ Language: C++ Date: $Date$ Version: $Revision: 11415 $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #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 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); - + // 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/mitkToFCameraPMDRawDataDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp index 77795a368d..4fd9cba9a5 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp @@ -1,489 +1,467 @@ /*========================================================================= Program: Medical Imaging & Interaction Toolkit Module: $RCSfile$ Language: C++ Date: $Date: 2010-05-27 16:06:53 +0200 (Do, 27 Mai 2010) $ Version: $Revision: $ Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. See MITKCopyright.txt or http://www.mitk.org/copyright.html for details. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the above copyright notices for more information. =========================================================================*/ #include "mitkToFCameraPMDRawDataDevice.h" #include "mitkRealTimeClock.h" #include "itkMultiThreader.h" #include namespace mitk { ToFCameraPMDRawDataDevice::ToFCameraPMDRawDataDevice() : m_SourceDataArray(NULL), m_SourceDataBuffer(NULL), m_ShortSourceData(NULL) { 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_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 - vtkShortArray* channelData = vtkShortArray::New(); - toFCameraDevice->m_ImageMutex->Lock(); - toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataArray); - toFCameraDevice->m_ImageMutex->Unlock(); - toFCameraDevice->m_Controller->GetShortSourceData(toFCameraDevice->m_ShortSourceData); - toFCameraDevice->GetChannelSourceData( toFCameraDevice->m_ShortSourceData, channelData ); - // 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(); - /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - TODO Buffer Handling currently only works for buffer size 1 - !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ if(!toFCameraDevice->m_RawDataSource->GetInit()) { toFCameraDevice->m_RawDataSource->Initialize(toFCameraDevice->m_CaptureWidth, toFCameraDevice->m_CaptureHeight, toFCameraDevice->m_Controller->GetModulationFrequency(), toFCameraDevice->GetChannelSize()); } toFCameraDevice->m_RawDataSource->SetChannelData(channelData); toFCameraDevice->m_RawDataSource->Update(); toFCameraDevice->m_ImageMutex->Lock(); - toFCameraDevice->m_RawDataSource->GetAmplitudes(toFCameraDevice->m_AmplitudeArray); - toFCameraDevice->m_RawDataSource->GetIntensities(toFCameraDevice->m_IntensityArray); - toFCameraDevice->m_RawDataSource->GetDistances(toFCameraDevice->m_DistanceArray); + toFCameraDevice->m_Controller->GetSourceData(toFCameraDevice->m_SourceDataArray); + 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) { - // Flip around y- axis (vertical axis) - m_ImageMutex->Lock(); - this->XYAxisFlipImage(this->m_AmplitudeArray, amplitudeArray, 1, 0 ); - m_ImageMutex->Unlock(); + 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) { - // Flip around y- axis (vertical axis) - m_ImageMutex->Lock(); - this->XYAxisFlipImage(this->m_IntensityArray, intensityArray, 0, 1); - m_ImageMutex->Unlock(); + 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) { - // Flip around y- axis (vertical axis) - m_ImageMutex->Lock(); - this->XYAxisFlipImage(this->m_DistanceArray,distanceArray, 1, 1); - m_ImageMutex->Unlock(); + 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) { 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; } - int u, v; - m_ImageMutex->Lock(); - 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] = this->m_DistanceArray[v]; // unit in millimeter - amplitudeArray[u] = this->m_AmplitudeArray[v]; - intensityArray[u] = this->m_IntensityArray[v]; - } - } + 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); - m_ImageMutex->Unlock(); } 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(propertyValue, modulationFrequency); m_Controller->SetModulationFrequency(modulationFrequency); } else if (strcmp(propertyKey, "IntegrationTime") == 0) { int integrationTime = 0; GetIntProperty(propertyValue, 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; } } }