diff --git a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp index b0818dc398..4dc1608d9e 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.cpp @@ -1,307 +1,306 @@ /*=================================================================== 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(): 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_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_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 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 83b5a928ea..2303dcf900 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h +++ b/Modules/ToFHardware/mitkToFCameraPMDCamBoardController.h @@ -1,135 +1,146 @@ /*=================================================================== 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 ); - + /* + \brief Returns amplitude data, performs camera data transform + \param output data array + */ bool GetAmplitudes(float* amplitudeArray); bool GetAmplitudes(char* sourceData, float* amplitudeArray); + /* + \brief Returns intensity data, performs camera data transform + \param output data array + */ bool GetIntensities(float* intensityArray); bool GetIntensities(char* sourceData, float* intensityArray); + /* + \brief Returns distance data, performs camera data transform + \param output data array + */ 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/mitkToFCameraPMDRawDataCamBoardDevice.cpp b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp index 07ee919472..c956bd27ae 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.cpp @@ -1,224 +1,215 @@ /*=================================================================== 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->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.flipud(); // flip image upside down outMat.copy_out(out); - inMat.clear(); + inMat.clear(); // clean data outMat.clear(); temp.clear(); } } diff --git a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h index 471642e7d4..5042b503d0 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataCamBoardDevice.h @@ -1,62 +1,81 @@ /*=================================================================== 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 ); - + /*! + \brief set a BaseProperty + */ virtual void SetProperty( const char *propertyKey, BaseProperty* propertyValue ); - + /*! + \brief Transforms the sourceData into an array with four tuples holding the channels for + raw data reconstruction. + */ virtual void GetChannelSourceData(short* sourceData, vtkShortArray* vtkChannelArray ); - + /*! + \brief Establishes camera connection and sets the class variables + */ bool ConnectCamera(); + /*! + \brief Returns intensity data + */ void GetIntensities(float* intensityArray, int& imageSequence); + /*! + \brief Returns amplitude data + */ void GetAmplitudes(float* amplitudeArray, int& imageSequence); + /*! + \brief Returns distance data + */ void GetDistances(float* distanceArray, int& imageSequence); + /*! + \brief Returns all image data at once. + */ void GetAllImages(float* distanceArray, float* amplitudeArray, float* intensityArray, char* sourceDataArray, int requiredImageSequence, int& capturedImageSequence, unsigned char* rgbDataArray); protected: ToFCameraPMDRawDataCamBoardDevice(); - ~ToFCameraPMDRawDataCamBoardDevice(); + private: + /*! + \brief Method performs resize of the image data and flips it upside down + */ 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 beccaf004c..177dae0d8d 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.cpp @@ -1,471 +1,470 @@ /*=================================================================== 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_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; + this->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_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_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 + // print current frame rate 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 a53c4c0443..76afd674ad 100644 --- a/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h +++ b/Modules/ToFHardware/mitkToFCameraPMDRawDataDevice.h @@ -1,178 +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 ); + //member variables 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; + unsigned int m_OriginControllerWidth; ///< holds the original controller width + unsigned int m_OriginControllerHeight; ///< holds the original controller height private: - //member variables int m_ChannelSize; ///< member holds the size of a single raw data channel }; } //END mitk namespace #endif